Incorrect selection of estimator sub-state causes unintended/incorrect updates #89
Description
The EKF correction is made here: RBISIndexedMeasurement::updateFilter which calls
- indexedMeasurement() which calls
- matrixMeasurementGetKandCovDelta() in rbis.cpp to calculate the Kalman Gain (K)
The state update is then used to create the correction:
- dstate = RBIS(K * z_resid); ("Updated state estimate" on EKF Wikipedia page)
For a position measurement (for example from ICP):
- I noticed that K and dstate are non zero in velocity and orientation. As a result, the orientation/velocity part of the dstate (the change in state) is non-zero and hence:
- the orientation and velocity is changed when a position measurement is made. This is not as intended.
I believe this is because the calculation of the Kalman Gain in matrixMeasurementGetKandCovDelta() wrongly selects parts of the state space
@mcamurri @simalpha : I believe this is the reason for the incorrect behaviour when integrating ICP on HyQ
@manuelli @siyuanfeng-tri : I think this could also cause the instability in orientation we are seeing for Valkyrie - but I don't want to jump to conclusions.