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

ImuFactor Jacobian test passed. 

Need to integrate at least two IMU measurements to get information on the position
release/4.3a0
Duy-Nguyen Ta 2015-09-15 11:14:45 -04:00
parent 704411de4e
commit 7f19e2ea86
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -641,7 +641,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.set_body_P_sensor(body_P_sensor); pim.set_body_P_sensor(body_P_sensor);
// Check updatedDeltaXij derivatives // Check updatedDeltaXij derivatives
Matrix3 D_correctedAcc_measuredOmega; Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero();
pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, D_correctedAcc_measuredOmega); pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, D_correctedAcc_measuredOmega);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
@ -714,6 +714,10 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-6)); EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-6));
// integrate one more time (at least twice) to get position information
// otherwise factor cov noise from preint_cov is not positive definite
pim.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor);
// Create factor // Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis); kNonZeroOmegaCoriolis);
@ -723,7 +727,6 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
Vector3 actual_v2; Vector3 actual_v2;
ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim, ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim,
kGravityAlongNavZDown, kZeroOmegaCoriolis); kGravityAlongNavZDown, kZeroOmegaCoriolis);
// Regression test with // Regression test with
Rot3 expectedR( // Rot3 expectedR( //
0.456795409, -0.888128414, 0.0506544554, // 0.456795409, -0.888128414, 0.0506544554, //
@ -742,8 +745,10 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
values.insert(V(2), v2); values.insert(V(2), v2);
values.insert(B(1), bias); values.insert(B(1), bias);
// factor.get_noiseModel()->print("noise: "); // Make sure the noise is valid
// Make sure linearization is correct // Make sure linearization is correct
double diffDelta = 1e-5; double diffDelta = 1e-8;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
} }