diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 8c87306a0..7e540b793 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -306,7 +306,7 @@ TEST(ImuFactor, ErrorAndJacobians) { EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); // Make sure linearization is correct - double diffDelta = 1e-5; + double diffDelta = 1e-7; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); // Actual Jacobians @@ -391,7 +391,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { values.insert(B(1), bias); // Make sure linearization is correct - double diffDelta = 1e-5; + double diffDelta = 1e-7; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } @@ -431,7 +431,7 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { values.insert(B(1), bias); // Make sure linearization is correct - double diffDelta = 1e-5; + double diffDelta = 1e-7; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } @@ -826,15 +826,15 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Matrix expected(9,9); expected << - 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.0, 0.0,// - 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.0,// - 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0,// - 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,// - 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,// - 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0,// - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290780477, 0.0, 9.23468723e-05,// - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0290688208, 4.62505461e-06,// - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.23468723e-05, 4.62505461e-06, 0.0299907267; + 0.0290780477, 4.63277848e-07, 9.23468723e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 4.63277848e-07, 0.0290688208, 4.62505461e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 9.23468723e-05, 4.62505461e-06, 0.0299907267, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0026, 0.0, 0.0, 0.005, // + 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01, 0.0, // + 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.01; EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-6)); // Create factor @@ -960,15 +960,15 @@ TEST(ImuFactor, PredictArbitrary) { Matrix expected(9,9); expected << // - 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, -0.0144839494, -0.0454268484, 0.00489577218,// - 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, 0.044978128, -0.0149428917, 0.00839301168, // - -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, 0.0100471195, 0.00609093775, 0.000448720395, // - 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, -0.0409843415, -0.13554868, 0.00879031682, // - 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, 0.134423822, -0.0471183681, 0.0162199769, // - -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764, 0.0383280513, 0.0247643646, 0.00112485862,// - -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, 0.0299999995, 0.0, 0.0, // - -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, 0.0, 0.0299999995, 0.0, // - 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, 0.0, 0.0, 0.0299999995; + 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // + 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // + 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // + -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // + 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // + 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // + -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // + 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // + 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); // Create factor