Unit Test IMUFactor: Inconsistent covariance matrices

[cdebeunne]

Description

It is about unit testing for IMU factors on file /navigation/tests/testImuFactor.cpp. I noticed that the assert_equal tolerance value in the unit test accelerating is set to 0.1 while the expected values of the matrix is way lower. I ran the test by setting this tolerance value to a lower value and the two covariance matrices seem really different, in particular for the block concerning the delta pose $\Delta \mathbf{p}$.

Steps to reproduce

I simply edited line 116 of testImuFactor.cpp to:

EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-4));

And I ran the test. I obtain the following when I print the error:

not equal:
expected = [
	6.47995e-08, 3.4762e-09, -4.50256e-09, 2.63202e-08, -8.90439e-07, 4.8413e-09, 2.61732e-08, -8.91936e-07, 2.52439e-09;
	3.4762e-09, 5.94131e-08, -4.37545e-10, 7.48855e-07, -1.86883e-08, -9.23621e-08, 8.00356e-07, -4.90974e-08, -6.01781e-08;
	-4.50256e-09, -4.37545e-10, 6.41243e-08, -1.00309e-09, 7.83734e-08, -9.46484e-10, 7.17183e-09, 6.34273e-08, -1.09659e-09;
	2.63202e-08, 7.48855e-07, -1.00309e-09, 4.25815e-05, -7.13657e-07, -2.19664e-06, 2.88484e-05, -5.82306e-07, -1.19486e-06;
	-8.90439e-07, -1.86883e-08, 7.83734e-08, -7.13657e-07, 4.59046e-05, 2.71718e-08, -1.89505e-07, 3.1307e-05, -2.28136e-07;
	4.8413e-09, -9.23621e-08, -9.46484e-10, -2.19664e-06, 2.71718e-08, 2.36278e-05, -1.31696e-06, -2.81335e-07, 1.19304e-05;
	2.61732e-08, 8.00356e-07, 7.17183e-09, 2.88484e-05, -1.89505e-07, -1.31696e-06, 2.40432e-05, -4.0352e-07, -7.91974e-07;
	-8.91936e-07, -4.90974e-08, 6.34273e-08, -5.82306e-07, 3.1307e-05, -2.81335e-07, -4.0352e-07, 2.56096e-05, -2.44826e-07;
	2.52439e-09, -6.01781e-08, -1.09659e-09, -1.19486e-06, -2.28136e-07, 1.19304e-05, -7.91974e-07, -2.44826e-07, 8.12407e-06
]
actual = [
	6.3462e-08, 0, 0, 0, -8.139e-07, 1.6278e-08, 0, -8.56736e-07, 1.71347e-08;
	0, 6.3462e-08, 0, 8.139e-07, 0, 0, 8.56736e-07, 0, 0;
	0, 0, 6.3462e-08, -1.6278e-08, 0, 0, -1.71347e-08, 0, 0;
	0, 8.139e-07, -1.6278e-08, 0.00034465, 0, 0, 2.98559e-05, 0, 0;
	-8.139e-07, 0, 0, 0, 0.000344642, -3.9409e-07, 0, 2.98489e-05, -3.46978e-07;
	1.6278e-08, 0, 0, 0, -3.9409e-07, 0.000324945, 0, -3.46978e-07, 1.25069e-05;
	0, 8.56736e-07, -1.71347e-08, 2.98559e-05, 0, 0, 2.46178e-05, 0, 0;
	-8.56736e-07, 0, 0, 0, 2.98489e-05, -3.46978e-07, 0, 2.46113e-05, -3.2556e-07;
	1.71347e-08, 0, 0, 0, -3.46978e-07, 1.25069e-05, 0, -3.2556e-07, 8.33984e-06
]
actual - expected = [
	-1.33755e-09, -3.4762e-09, 4.50256e-09, -2.63202e-08, 7.65396e-08, 1.14367e-08, -2.61732e-08, 3.51997e-08, 1.46103e-08;
	-3.4762e-09, 4.04887e-09, 4.37545e-10, 6.50446e-08, 1.86883e-08, 9.23621e-08, 5.63804e-08, 4.90974e-08, 6.01781e-08;
	4.50256e-09, 4.37545e-10, -6.62353e-10, -1.52749e-08, -7.83734e-08, 9.46484e-10, -2.43066e-08, -6.34273e-08, 1.09659e-09;
	-2.63202e-08, 6.50446e-08, -1.52749e-08, 0.000302068, 7.13657e-07, 2.19664e-06, 1.00745e-06, 5.82306e-07, 1.19486e-06;
	7.65396e-08, 1.86883e-08, -7.83734e-08, 7.13657e-07, 0.000298737, -4.21262e-07, 1.89505e-07, -1.45807e-06, -1.18842e-07;
	1.14367e-08, 9.23621e-08, 9.46484e-10, 2.19664e-06, -4.21262e-07, 0.000301318, 1.31696e-06, -6.56436e-08, 5.76552e-07;
	-2.61732e-08, 5.63804e-08, -2.43066e-08, 1.00745e-06, 1.89505e-07, 1.31696e-06, 5.74646e-07, 4.0352e-07, 7.91974e-07;
	3.51997e-08, 4.90974e-08, -6.34273e-08, 5.82306e-07, -1.45807e-06, -6.56436e-08, 4.0352e-07, -9.98262e-07, -8.07339e-08;
	1.46103e-08, 6.01781e-08, 1.09659e-09, 1.19486e-06, -1.18842e-07, 5.76552e-07, 7.91974e-07, -8.07339e-08, 2.15775e-07
]
/media/ce.debeunne/HDD/gtsam/gtsam/navigation/tests/testImuFactor.cpp:118: Failure: "assert_equal(estimatedCov, pim.preintMeasCov(), 1e-4)"

Expected behavior

I personally expect this test to pass, at least, for a threshold around the highest value of the matrix. To me, it may hide an inconsistency in either the numerical or the closed form covariance. Or is there any explanation for this?

Thank you for this awesome software and for your eventual response.
Cheers