diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b05a8a3e5..a62ca06a8 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -194,23 +194,31 @@ TEST( NavState, Lie ) { /* ************************************************************************* */ TEST(NavState, Update) { - const Vector3 omega(M_PI / 100.0, 0.0, 0.0); - const Vector3 acc(0.1, 0.0, 0.0); - const double deltaT = 10; + Vector3 omega(M_PI / 100.0, 0.0, 0.0); + Vector3 acc(0.1, 0.0, 0.0); + double dt = 10; Matrix9 aF; Matrix93 aG1, aG2; boost::function update = - boost::bind(&NavState::update, _1, _2, _3, deltaT, boost::none, + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, boost::none, boost::none); Vector3 b_acc = kAttitude * acc; - NavState expected(kAttitude.expmap(deltaT * omega), - kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT), - kVelocity + b_acc * deltaT); - NavState actual = kState1.update(acc, omega, deltaT, aF, aG1, aG2); + NavState expected(kAttitude.expmap(dt * omega), + kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), + kVelocity + b_acc * dt); + NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); + + // Try different values + omega = Vector3(0.1, 0.2, 0.3); + acc = Vector3(0.4, 0.5, 0.6); + kState1.update(acc, omega, dt, aF, aG1, aG2); + EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); } /* ************************************************************************* */