From 0c62a9528deeee7eedae43250e9f0328c742c240 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 07:45:50 -0700 Subject: [PATCH] fixed issue that pos and vel are in nav frame --- gtsam/navigation/NavState.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7d4797132..6a196cb75 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -228,18 +228,27 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } + + // 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)); + + 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_t_v(H) << nRb.matrix().transpose() * (-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_R(H) << skewSymmetric(dV(xi)); 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; + D_t_t(H) -= nRb.matrix().transpose() * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= nRb.matrix().transpose() * dt * D_cross2_state; } } return xi;