Merge pull request #2287 from scottiyio/centripetal

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

Author: dellaert

gtsam/navigation/NavState.cpp

@@ -436,6 +436,13 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
 }
 
 //------------------------------------------------------------------------------
+
+// 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 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
     OptionalJacobian<9, 9> H) const {
   Rot3 nRb = R_;
@@ -444,37 +451,42 @@ 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
-  Vector9 n_xi, xi;
-  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
+  Vector9 n_xi;
+  // Coriolis (first order) acceleration corrections
   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);
+
+  // Centrifugal (second order) acceleration corrections
+  Matrix3 D_c2_nt; // To store Jacobian (if needed/desired)
   if (secondOrder) {
-    const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
+    const Vector3 omega_cross2_t = doubleCross(omega, n_t, nullptr, H ? &D_c2_nt : nullptr);
     dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
     dV(n_xi) -= dt * omega_cross2_t;
   }
 
-  // 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);
+  // Transform correction from navigation frame -> body frame and get Jacobians
+  Vector9 xi;
+  Matrix3 D_dR_R, D_dP_R, D_dV_R;
+  dR(xi) = nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0);
+  dP(xi) = nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0);
+  dV(xi) = nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
 
+  // Assemble Jacobians
   if (H) {
     H->setZero();
-    const Matrix3 Omega = skewSymmetric(omega);
-    const Matrix3 D_cross_state = Omega * R();
+    const Vector3 omega_b = nRb.unrotate(omega);
+    const Matrix3 D_c1_b = skewSymmetric(omega_b);
     H->setZero();
     D_R_R(H) << D_dR_R;
-    D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
+    D_t_v(H) << (-dt2) * D_c1_b;
     D_t_R(H) << D_dP_R;
-    D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
+    D_v_v(H) << (-2.0 * dt) * D_c1_b;
     D_v_R(H) << D_dV_R;
     if (secondOrder) {
-      const Matrix3 D_cross2_state = Omega * D_cross_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;
+      Matrix3 D_c2_b = D_c1_b * D_c1_b;
+      D_t_t(H) -= (0.5 * dt2) * D_c2_b;
+      D_v_t(H) -= dt * D_c2_b;
     }
   }
   return xi;

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;