Bug in CombinedImuFactor: Discrete Bias Covariance Wrong

To go from continuous-time noise density to discrete-time covariances is different for the additive white-noise (Cov_discrete = 1/dt * Cov_continuous) and the bias (Cov_discrete = dt * Cov_continous) as shown in the Appendix of the Crassidis paper or as summarized here: https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics

Therefore, we would have to change the Lines 136-137 in CombinedImuFactor.cpp as follows:

Old:

G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_;

Suggested:

G_measCov_Gt.block<3, 3>(9, 9) = deltaT * biasAccCovariance_;
G_measCov_Gt.block<3, 3>(12, 12) = deltaT * biasOmegaCovariance_;

I have always been working with the plain ImuFactor and was surprised to get very bad results for the same noise settings with the CombinedImuFactor. With this fix, my results on simulated data are consistent.

@dellaert @lucacarlone I didn't have time yet to check the latest IMU refactor. Would you prefer if I fix it directly there or in a separate pull request?

Comments (2)