diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6a196cb75..fcb85d2b7 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,6 +215,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { TIE(nRb, n_t, n_v, *this); + Matrix3 bRn = nRb.matrix().transpose(); const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); @@ -231,8 +232,8 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, // NOTE(tim): position and velocity changes dP and dV are in nav frame; need // to put them into local frame for subsequent retract - dP(xi) = nRb.unrotate(dP(xi)); - dV(xi) = nRb.unrotate(dV(xi)); + dP(xi) = bRn * dP(xi); + dV(xi) = bRn * dV(xi); if (H) { @@ -241,14 +242,14 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, const Matrix3 D_cross_state = Omega * R(); H->setZero(); D_R_R(H) << D_dP_R; - D_t_v(H) << nRb.matrix().transpose() * (-dt2) * D_cross_state; + D_t_v(H) << bRn * (-dt2) * D_cross_state; D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << nRb.matrix().transpose() * (-2.0 * dt) * D_cross_state; + D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; D_v_R(H) << skewSymmetric(dV(xi)); if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; + D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= bRn * dt * D_cross2_state; } } return xi;