From 2df20b4f375afab5cb51ee5637b0bcfb2e4cd6c6 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 19 Jul 2015 23:57:00 -0700
Subject: [PATCH] Second order coriolis done

---
 gtsam/navigation/PreintegrationBase.cpp  | 15 ++++++++++-----
 gtsam/navigation/tests/testImuFactor.cpp |  6 +++---
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index e731249bd..2c9b8b6ae 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -182,6 +182,7 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
     const Pose3& pose_i = state_i.pose();
     const Vector3& vel_i = state_i.velocity();
     const Matrix3 Ri = pose_i.rotation().matrix();
+    const Vector3& t_i = state_i.translation().vector();
     const double dt = deltaTij(), dt2 = dt * dt;
 
     const Vector3& omegaCoriolis = *p().omegaCoriolis;
@@ -190,16 +191,20 @@ Vector9 PreintegrationBase::integrateCoriolis(const NavState& state_i,
     NavState::dP(result) -= omegaCoriolis.cross(vel_i) * dt2; // NOTE(luca): we got rid of the 2 wrt INS paper
     NavState::dV(result) -= 2.0 * omegaCoriolis.cross(vel_i) * dt;
     if (p().use2ndOrderCoriolis) {
-      Vector3 temp = omegaCoriolis.cross(
-          omegaCoriolis.cross(pose_i.translation().vector()));
+      Vector3 temp = omegaCoriolis.cross(omegaCoriolis.cross(t_i));
       NavState::dP(result) -= 0.5 * temp * dt2;
       NavState::dV(result) -= temp * dt;
     }
     if (H) {
       const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
-      H->block<3,3>(0,0) = -D_dP_Ri;
-      H->block<3,3>(3,6) = - omegaCoriolisHat * dt2;
-      H->block<3,3>(6,6) = - 2.0 * omegaCoriolisHat * dt;
+      H->block<3,3>(0,0) -= D_dP_Ri;
+      H->block<3,3>(3,6) -= omegaCoriolisHat * dt2;
+      H->block<3,3>(6,6) -= 2.0 * omegaCoriolisHat * dt;
+      if (p().use2ndOrderCoriolis) {
+        const Matrix3 omegaCoriolisHat2 = omegaCoriolisHat * omegaCoriolisHat;
+        H->block<3,3>(3,3) -= 0.5 * omegaCoriolisHat2 * dt2;
+        H->block<3,3>(6,3) -= omegaCoriolisHat2 * dt;
+      }
     }
   }
   return result;
diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp
index cd64c2212..70b20ef00 100644
--- a/gtsam/navigation/tests/testImuFactor.cpp
+++ b/gtsam/navigation/tests/testImuFactor.cpp
@@ -218,7 +218,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   p->accelerometerCovariance = kMeasuredAccCovariance;
   p->integrationCovariance = kIntegrationErrorCovariance;
   p->use2ndOrderIntegration = false;
-  p->use2ndOrderCoriolis = false;
+  p->use2ndOrderCoriolis = true;
 
   PreintegratedImuMeasurements pim(p, bias);
   pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@@ -236,7 +236,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   }
   {
     Vector9 expected; // TODO(frank): taken from output. Should really verify.
-    expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.0, -0.08, 0.06, 0.0, -0.08, 0.06;
+    expected << -0.0212372436, -0.0407423986, -0.0974116854, 0.1038, 0.038, -0.0804, 0.1038, 0.038, -0.0804;
     Matrix99 actualH;
     EXPECT(assert_equal(expected, pim.integrateCoriolis(state1, actualH), 1e-5));
     Matrix expectedH = numericalDerivative11<Vector9, NavState>(
@@ -246,7 +246,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) {
   }
   {
     Vector9 expected; // TODO(frank): taken from output. Should really verify.
-    expected << 0.0415946095, -0.0407423986, -0.0974116854, 1, -0.08, 9.85, -0.187214027, 0.110178303, 0.0436304821;
+    expected << 0.0415946095, -0.0407423986, -0.0974116854, 1.1038, 0.038, 9.7096, -0.0834140265, 0.228178303, -0.0967695179;
     Matrix99 actualH1, actualH2;
     Vector9 biasCorrectedDelta = pim.biasCorrectedDelta(bias);
     EXPECT(