From 013c8a4cefc9af2c09b99c82c5dc2098ab9003c0 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 16:05:39 -0500 Subject: [PATCH] added extra test on covariance propagation --- gtsam/navigation/tests/testCombinedImuFactor.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index bb6828216..4c4aeb7c7 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -352,6 +352,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + Matrix Factual, Gactual; preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, body_P_sensor, Factual, Gactual); @@ -412,6 +414,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Fexpected * oldPreintCovariance * Fexpected.transpose() + + (1/newDeltaT) * Gexpected * Gexpected.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual, 1e-7)); } /* ************************************************************************* */