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

More tests

release/4.3a0
dellaert 2015-08-01 17:21:15 -07:00
parent f8df938b30
commit b8f05e1e35
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/navigation/tests/testNavState.cpp
View File

@ -194,23 +194,31 @@ TEST( NavState, Lie ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, Update) { TEST(NavState, Update) {
const Vector3 omega(M_PI / 100.0, 0.0, 0.0); Vector3 omega(M_PI / 100.0, 0.0, 0.0);
const Vector3 acc(0.1, 0.0, 0.0); Vector3 acc(0.1, 0.0, 0.0);
const double deltaT = 10; double dt = 10;
Matrix9 aF; Matrix9 aF;
Matrix93 aG1, aG2; Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update = boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> 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); boost::none, boost::none);
Vector3 b_acc = kAttitude * acc; Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(deltaT * omega), NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT), kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),
kVelocity + b_acc * deltaT); kVelocity + b_acc * dt);
NavState actual = kState1.update(acc, omega, deltaT, aF, aG1, aG2); NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); 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(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */