From 0c62a9528deeee7eedae43250e9f0328c742c240 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 07:45:50 -0700 Subject: [PATCH 1/5] 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; From cb6573ad1fa0484034614e40b51336857a3d2aa2 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:01:52 -0700 Subject: [PATCH 2/5] added another coriolis unit test --- gtsam/navigation/tests/testNavState.cpp | 43 +++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 48dd3bc3e..95fd31c21 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) { EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); } +TEST(NavState, Coriolis3) { + /** Consider a massless plate with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * second order Coriolis corrections are as expected. + */ + + // Get true first and second order coriolis accelerations + double dt = 2.0, dt2 = dt * dt; + Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0); + Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v), + n_aCorr2 = -n_omega.cross(n_omega.cross(n_t)); + Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0), + bRn = nRb.inverse(); + + // Get expected first and second order corrections in the nav frame + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), + n_dV1e = dt * n_aCorr1, + n_dV2e = dt * (n_aCorr1 + n_aCorr2); + + // Get expected first and second order corrections in the body frame + Vector3 dRe = -dt * (bRn * n_omega), + b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e, + b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e; + + // Get actual first and scond order corrections in body frame + NavState kState2(nRb, n_t, n_v); + Vector9 dXi1a = kState2.coriolis(dt, n_omega, false), + dXi2a = kState2.coriolis(dt, n_omega, true); + Vector3 dRa = NavState::dR(dXi1a), + b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a), + b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a); + + EXPECT(assert_equal(dRe, dRa)); + EXPECT(assert_equal(b_dP1e, b_dP1a)); + EXPECT(assert_equal(b_dV1e, b_dV1a)); + EXPECT(assert_equal(b_dP2e, b_dP2a)); + EXPECT(assert_equal(b_dV2e, b_dV2a)); + +} + /* ************************************************************************* */ TEST(NavState, CorrectPIM) { Vector9 xi; From c3bddf18168fa5c95c7bce42d794368f703b0982 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Tue, 1 Sep 2020 13:07:51 -0700 Subject: [PATCH 3/5] added cached rotation bRn --- gtsam/navigation/NavState.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) 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; From 688292f114d04f8b519c6b9681b2bb690bb773d7 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 07:59:19 -0700 Subject: [PATCH 4/5] cleaned up notation --- gtsam/navigation/NavState.cpp | 39 +++++++++++++++++------------------ 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index fcb85d2b7..0181ea45f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -215,41 +215,40 @@ 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); - Vector9 xi; - Matrix3 D_dP_R; - 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); + // Get perturbations in nav frame + Vector9 n_xi, xi; + Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; + dR(n_xi) << ((-dt) * omega); + dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(n_xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); - dP(xi) -= (0.5 * dt2) * omega_cross2_t; - dV(xi) -= dt * omega_cross2_t; + dP(n_xi) -= (0.5 * dt2) * omega_cross2_t; + dV(n_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) = bRn * dP(xi); - dV(xi) = bRn * dV(xi); - + // Transform n_xi into the body frame + xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), + nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0), + nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); 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) << bRn * (-dt2) * D_cross_state; - D_t_R(H) << skewSymmetric(dP(xi)); - D_v_v(H) << bRn * (-2.0 * dt) * D_cross_state; - D_v_R(H) << skewSymmetric(dV(xi)); + D_R_R(H) << D_dR_R; + D_t_v(H) << D_body_nav * (-dt2) * D_cross_state; + D_t_R(H) << D_dP_R; + D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state; + D_v_R(H) << D_dV_R; if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= bRn * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= bRn * dt * D_cross2_state; + D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= D_body_nav * dt * D_cross2_state; } } return xi; From 335b57ca363da3b4a1ca9c9a24c59a6d5b5fc339 Mon Sep 17 00:00:00 2001 From: Timothy Setterfield Date: Wed, 2 Sep 2020 08:08:36 -0700 Subject: [PATCH 5/5] fixed typo --- gtsam/navigation/tests/testNavState.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 95fd31c21..5bafe4392 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -193,7 +193,7 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless plate with an attached nav frame at + /** Consider a massless planet with an attached nav frame at * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and