From 9f91aedd6abd30215e05653de18418f9e7261ee1 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Wed, 2 Sep 2015 16:52:54 -0400 Subject: [PATCH] test centrifugal derivative --- gtsam/navigation/tests/testImuFactor.cpp | 29 ++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 97f76bad8..50d6a8533 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -157,6 +157,35 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim, return assert_equal(Matrix(10000*Q), 10000*actual, 1); } +/* ************************************************************************* */ +Vector3 centrifugal(const Vector3& omega_s, const Pose3& bTs, boost::optional H1) { + Rot3 bRs = bTs.rotation(); + Vector3 j_correctedOmega = bRs * omega_s; + + const Vector3 b_arm = bTs.translation().vector(); + + // Subtract out the the centripetal acceleration from the measured one + // to get linear acceleration vector in the body frame: + const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); + const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm + if (H1) { + double wdp = j_correctedOmega.dot(b_arm); + *H1 = - (diag(Vector3::Constant(wdp)) + j_correctedOmega * b_arm.transpose())*bRs.matrix() + 2*b_arm*omega_s.transpose(); + } + return -body_Omega_body * b_velocity_bs; +} + +/* ************************************************************************* */ +TEST(ImuFactor, centrifugalDeriv) { + static const Pose3 bTs(Rot3::ypr(0.1,0.2,0.3), Point3(1.,4.,7.)); + Vector3 omega_s(0.2,0.4,0.6); + Matrix3 H1; + Vector3 cb = centrifugal(omega_s, bTs, H1); + (void) cb; + Matrix Hnum = numericalDerivative11(boost::bind(centrifugal, _1, bTs, boost::none), omega_s, 1e-6); + EXPECT(assert_equal(Hnum, H1, 1e-5)); +} + /* ************************************************************************* */ TEST(ImuFactor, StraightLine) { // Set up IMU measurements