Use doublecross instead of cross(Cross(a, b)) in NavState

gtsam/navigation/NavState.cpp

@@ -444,14 +444,19 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);
 
-  // Get perturbations in nav frame
+  // Because our navigation frames are placed on a spinning Earth, we experience two apparent forces on our inertials
+  // Let Omega be the Earth's rotation rate in the navigation frame
+  // Coriolis acceleration = -2 * (omega X n_v)
+  // Centrifugal acceleration (secondOrder) = -omega X (omega X n_t)
+  // We would also experience a rotation of (omega*dt) over time - so, counteract by compensating rotation by (-omega * dt)
+  // Integrate centrifugal & coriolis accelerations to yield position, velocity perturbations
   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));
+    const Vector3 omega_cross2_t = doubleCross(omega, n_t);
     dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
     dV(n_xi) -= dt * omega_cross2_t;
   }

gtsam/navigation/tests/testNavState.cpp

@@ -371,7 +371,7 @@ TEST(NavState, Coriolis3) {
   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));
+          n_aCorr2 = -doubleCross(n_omega, 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();
 
@@ -402,6 +402,24 @@ TEST(NavState, Coriolis3) {
 
 }
 
+TEST(NavState, Coriolis4) {
+  /** Consider a navigation frame with zero angular velocity.
+   * We expect that the coriolis correction does nothing and the Jacobian is zero.
+   */
+  const Vector3 omega(0.0, 0.0, 0.0);
+
+  const Vector9 expected_correction = Vector9::Zero();
+  const Matrix9 expected_H = Matrix9::Zero();
+  Matrix9 actual_H;
+
+  const Vector9 actual_correction =
+        kState1.coriolis(dt, omega, true, actual_H);
+
+  EXPECT(assert_equal(expected_correction, actual_correction));
+  EXPECT(assert_equal(expected_H, actual_H));
+}
+
+
 /* ************************************************************************* */
 TEST(NavState, CorrectPIM) {
   Vector9 xi;