Incorrect selection of estimator sub-state causes unintended/incorrect updates

[mauricefallon]

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.