UKF, missing addition with matrix R in the covariance correction update

[lkdo]

In function Ukf::correct,

• the Kalman gain is computed correctly using the (P_zz + R)^-1
• and in the covariance update, the P_zz must similarly be added with R i.e. P = P - (K (P_zz + R) K') instead of P = P - (K P_zz K')

[ayrton04]

I believe you are correct, yes. The formulation within the original paper seems to ignore the measurement covariance at all, whereas the formulation on Wikipedia includes it.

In any case, including R as you stated is clearly correct. I need to get the UKF cleanup PR merged, and then if you have the cycles, I'd welcome a PR to fix this. Thanks for pointing it out!

[lkdo]

Thanks. Sure, I'll do a PR after the merge.

[ayrton04]

Sorry for the delay. The PR is merged into noetic-devel. If you wouldn't mind submitting your PR to that branch, I can make sure it finds its way to the ROS 2 branches. I'm reluctant to push all this back to melodic, but could be convinced otherwise.

[ayrton04]

And thank you!

[roncapat]

Any updates on this?

[lkdo]

Hi.

The last news on this was that one of the unit tests is breaking as a result of the change.

I don't really understand why, so I tried a small change in the unit test UKF ImuDifferentialIO ( which is the one that fails as far as I can see):

Changing: ros::Publisher imuPub = nh.advertise("/imu_input3", 5); to ros::Publisher imuPub = nh.advertise("/imu_input0", 5);

Makes the test suceed.

Both topics have the following config: [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]

I don't really understand the difference between imu_input3 and imu_input0, they should be the same. What is the difference ? Can someone else confirm that the test passes with this change and maybe provide an explanation ?

Also, a more advanced ukf covariance update in the correct step is:

  // (6.bis) Replace with Joseph form which is more numerically stable: P =  (I-K*H)*P*(I-K*H)' + K*R*K'
  // H = stochastic linearization derived from the fact that P_xz = PH' in the linear case
  // paper "On Iterative Unscented Kalman Filter using Optimization"

  stateToMeasurementSubset = crossCovar.transpose()*estimateErrorCovariance_.inverse();
  Eigen::MatrixXd IKH = Eigen::MatrixXd::Identity(STATE_SIZE,STATE_SIZE) - kalmanGainSubset*stateToMeasurementSubset;
  estimateErrorCovariance_ =  IKH*estimateErrorCovariance_*IKH.transpose() + 
     kalmanGainSubset*measurementCovarianceSubset*kalmanGainSubset.transpose();

[lkdo]

Oh, imu3_differential = true.

This is the difference.