Avoid R=R.inverse() for Eigen matrices in a couple places in KalmanUpdators
Mentioning @amorley
-
Replace
R=R.inverse()
either by usingR.inverse()
in expression or havingR_inv=R.inverse()
As I think otherwise we might run in Aliasing issue, https://eigen.tuxfamily.org/dox/group__TopicAliasing.html, this depends on the internals. -
Also try to use a bit more the ATLAS logging macros etc . Note this code has some methods that conditionally execute in verbose / Debug (did not touch the relevant if much)