12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added extra test on covariance propagation

release/4.3a0
Luca 2014-12-09 16:05:39 -05:00
parent d22868d524
commit 013c8a4cef
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -352,6 +352,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual; Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual); body_P_sensor, Factual, Gactual);
@ -412,6 +414,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */