From 8ae4e2afd9d3752d70668fb86e5bd620fd97678e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Jul 2015 01:56:13 -0400 Subject: [PATCH] A fully functioning predict from preintegrated tangent vector --- gtsam/navigation/NavState.cpp | 121 ++++++++++++++++++------ gtsam/navigation/NavState.h | 20 ++-- gtsam/navigation/tests/testNavState.cpp | 74 +++++++++------ 3 files changed, 149 insertions(+), 66 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index d41b28bf0..aa33fe858 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -203,54 +203,115 @@ Matrix7 NavState::wedge(const Vector9& xi) { //------------------------------------------------------------------------------ // sugar for derivative blocks -#define D_R_R(H) H->block<3,3>(0,0) -#define D_t_t(H) H->block<3,3>(3,3) -#define D_t_v(H) H->block<3,3>(3,6) -#define D_v_t(H) H->block<3,3>(6,3) -#define D_v_v(H) H->block<3,3>(6,6) +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ -void NavState::addCoriolis(Vector9* xi, const Vector3& omega, double dt, - bool secondOrder, OptionalJacobian<9, 9> H) const { - const Rot3& nRb = attitude(); - const Point3& n_t = position(); // derivative is R() ! - const Vector3& n_v = velocity(); // ditto +Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, + OptionalJacobian<9, 9> H) const { + TIE(nRb, n_t, n_v, *this); const double dt2 = dt * dt; - const Vector3 omega_cross_vel = omega.cross(n_v); + + Vector9 xi; Matrix3 D_dP_R; - dR(*xi) -= nRb.unrotate(omega * dt, H ? &D_dP_R : 0); - dP(*xi) -= (dt2 * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(*xi) -= ((2.0 * dt) * omega_cross_vel); + dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); + dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); - dP(*xi) -= (0.5 * dt2) * omega_cross2_t; - dV(*xi) -= dt * omega_cross2_t; + dP(xi) -= (0.5 * dt2) * omega_cross2_t; + dV(xi) -= dt * omega_cross2_t; } if (H) { + H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); - D_R_R(H) -= D_dP_R; - D_t_v(H) -= dt2 * D_cross_state; - D_v_v(H) -= (2.0 * dt) * D_cross_state; + D_R_R(H) << D_dP_R; + D_t_v(H) << (-dt2) * D_cross_state; + D_v_v(H) << (-2.0 * dt) * D_cross_state; if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; D_t_t(H) -= (0.5 * dt2) * D_cross2_state; D_v_t(H) -= dt * D_cross2_state; } } -} - -//------------------------------------------------------------------------------ -Vector9 NavState::coriolis(const Vector3& omega, double dt, bool secondOrder, - OptionalJacobian<9, 9> H) const { - Vector9 xi; - xi.setZero(); - if (H) - H->setZero(); - addCoriolis(&xi, omega, dt, secondOrder, H); return xi; } -} /// namespace gtsam +//------------------------------------------------------------------------------ +Vector9 NavState::predictXi(const Vector9& pim, double dt, + const Vector3& gravity, boost::optional omegaCoriolis, + bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) const { + const Rot3& nRb = R_; + const Velocity3& n_v = v_; // derivative is Ri ! + const double dt2 = dt * dt; + + Vector9 delta; + Matrix3 D_dP_Ri, D_dP_dP, D_dV_Ri, D_dV_dV; + dR(delta) = dR(pim); + // TODO(frank): + // - why do we integrate n_v here ? + // - the dP and dV should be in body ! How come semi-retract works out ?? + // - should we rename t_ to p_? if not, we should rename dP do dT + dP(delta) = nRb.rotate(dP(pim), H1 ? &D_dP_Ri : 0, H2 ? &D_dP_dP : 0) + + n_v * dt + 0.5 * gravity * dt2; + dV(delta) = nRb.rotate(dV(pim), H1 ? &D_dV_Ri : 0, H2 ? &D_dV_dV : 0) + + gravity * dt; + + if (omegaCoriolis) { + delta += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + } + + if (H1 || H2) { + Matrix3 Ri = nRb.matrix(); + + if (H1) { + if (!omegaCoriolis) + H1->setZero(); // if coriolis H1 is already initialized + D_t_R(H1) += D_dP_Ri; + D_t_v(H1) += I_3x3 * dt * Ri; + D_v_R(H1) += D_dV_Ri; + } + if (H2) { + H2->setZero(); + D_R_R(H2) << I_3x3; + D_t_t(H2) << D_dP_dP; + D_v_v(H2) << D_dV_dV; + } + } + + return delta; +} +//------------------------------------------------------------------------------ +NavState NavState::predict(const Vector9& pim, double dt, + const Vector3& gravity, boost::optional omegaCoriolis, + bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) const { + + Matrix9 D_delta_state, D_delta_pim; + Vector9 delta = predictXi(pim, dt, gravity, omegaCoriolis, + use2ndOrderCoriolis, H1 ? &D_delta_state : 0, H2 ? &D_delta_pim : 0); + + Matrix9 D_predicted_state, D_predicted_delta; + NavState predicted = retract(delta, H1 ? &D_predicted_state : 0, + H1 || H2 ? &D_predicted_delta : 0); + // TODO(frank): the derivatives of retract are very sparse: inline below: + if (H1) + *H1 = D_predicted_state + D_predicted_delta * D_delta_state; + if (H2) + *H2 = D_predicted_delta * D_delta_pim; + return predicted; +} +//------------------------------------------------------------------------------ + +}/// namespace gtsam diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 290cc0bd5..f9b046fe3 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,15 +203,23 @@ public: /// @name Dynamics /// @{ - // Compute tangent space contribution due to coriolis forces - Vector9 coriolis(const Vector3& omega, double dt, bool secondOrder = false, + /// Compute tangent space contribution due to Coriolis forces + Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, OptionalJacobian<9, 9> H = boost::none) const; - // Add tangent space contribution due to coriolis forces - // Additively modifies xi and H in place (if given) - void addCoriolis(Vector9* xi, const Vector3& omega, double dt, - bool secondOrder = false, OptionalJacobian<9, 9> H = boost::none) const; + /// Combine preintegrated measurements, in the form of a tangent space vector, + /// with gravity, velocity, and Coriolis forces into a tangent space update. + Vector9 predictXi(const Vector9& pim, double dt, const Vector3& gravity, + boost::optional omegaCoriolis, bool use2ndOrderCoriolis = + false, OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none) const; + /// Combine preintegrated measurements, in the form of a tangent space vector, + /// with gravity, velocity, and Coriolis forces into a new state. + NavState predict(const Vector9& pim, double dt, const Vector3& gravity, + boost::optional omegaCoriolis, bool use2ndOrderCoriolis = + false, OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none) const; /// @} private: diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index c5b46831e..683a97d50 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -29,6 +29,8 @@ static const Point3 kPosition(1.0, 2.0, 3.0); static const Velocity3 kVelocity(0.4, 0.5, 0.6); static const NavState kIdentity; static const NavState kState1(kRotation, kPosition, kVelocity); +static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); +static const Vector3 kGravity(0, 0, 9.81); /* ************************************************************************* */ TEST( NavState, Attitude) { @@ -142,48 +144,60 @@ TEST( NavState, Lie ) { } /* ************************************************************************* */ +static const double dt = 2.0; +boost::function coriolis = boost::bind( + &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); + TEST(NavState, Coriolis) { Matrix9 actualH; - Vector3 omegaCoriolis(0.02, 0.03, 0.04); - double dt = 0.5; + // first-order - bool secondOrder = false; - kState1.coriolis(omegaCoriolis, dt, secondOrder, actualH); - Matrix expectedH = numericalDerivative11( - boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder, - boost::none), kState1); - EXPECT(assert_equal(expectedH, actualH)); + kState1.coriolis(dt, kOmegaCoriolis, false, actualH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), actualH)); // second-order - secondOrder = true; - kState1.coriolis(omegaCoriolis, dt, secondOrder, actualH); - expectedH = numericalDerivative11( - boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder, - boost::none), kState1); - EXPECT(assert_equal(expectedH, actualH)); + kState1.coriolis(dt, kOmegaCoriolis, true, actualH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), actualH)); } -/* ************************************************************************* */ TEST(NavState, Coriolis2) { + Matrix9 actualH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); - Matrix9 actualH; - Vector3 omegaCoriolis(0.02, 0.03, 0.04); - double dt = 2.0; // first-order - bool secondOrder = false; - state2.coriolis(omegaCoriolis, dt, secondOrder, actualH); - Matrix expectedH = numericalDerivative11( - boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder, - boost::none), state2); - EXPECT(assert_equal(expectedH, actualH)); + state2.coriolis(dt, kOmegaCoriolis, false, actualH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), actualH)); // second-order - secondOrder = true; - state2.coriolis(omegaCoriolis, dt, secondOrder, actualH); - expectedH = numericalDerivative11( - boost::bind(&NavState::coriolis, _1, omegaCoriolis, dt, secondOrder, - boost::none), state2); - EXPECT(assert_equal(expectedH, actualH)); + state2.coriolis(dt, kOmegaCoriolis, true, actualH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, PredictXi) { + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + double dt = 0.5; + Matrix9 actualH1, actualH2; + boost::function predictXi = + boost::bind(&NavState::predictXi, _1, _2, dt, kGravity, kOmegaCoriolis, + false, boost::none, boost::none); + kState1.predictXi(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(predictXi, kState1, xi), actualH1)); + EXPECT(assert_equal(numericalDerivative22(predictXi, kState1, xi), actualH2)); +} + +/* ************************************************************************* */ +TEST(NavState, Predict) { + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + double dt = 0.5; + Matrix9 actualH1, actualH2; + boost::function predict = + boost::bind(&NavState::predict, _1, _2, dt, kGravity, kOmegaCoriolis, + false, boost::none, boost::none); + kState1.predict(xi, dt, kGravity, kOmegaCoriolis, false, actualH1, actualH2); + EXPECT(assert_equal(numericalDerivative21(predict, kState1, xi), actualH1)); + EXPECT(assert_equal(numericalDerivative22(predict, kState1, xi), actualH2)); } /* ************************************************************************* */