From e3d36da18817c9bcd9d0292d482b1a6175d76757 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 31 Jul 2015 13:33:23 -0700
Subject: [PATCH] Switched argument order

---
 gtsam/navigation/NavState.cpp           | 27 +++++++++++++------------
 gtsam/navigation/NavState.h             |  4 ++--
 gtsam/navigation/tests/testNavState.cpp |  8 ++++----
 3 files changed, 20 insertions(+), 19 deletions(-)

diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 3b2093d1a..2e7917688 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -238,7 +238,7 @@ Matrix7 NavState::wedge(const Vector9& xi) {
 #define D_v_v(H) (H)->block<3,3>(6,6)
 
 //------------------------------------------------------------------------------
-NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
+NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
     const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
     OptionalJacobian<9, 3> G2) const {
 
@@ -248,9 +248,9 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
   double dt22 = 0.5 * dt * dt;
 
   // Integrate on tangent space. TODO(frank): coriolis?
-  dR(xi) << dt * omega;
-  dP(xi) << dt * b_v + dt22 * acceleration;
-  dV(xi) << dt * acceleration;
+  dR(xi) << dt * b_omega;
+  dP(xi) << dt * b_v + dt22 * b_acceleration;
+  dV(xi) << dt * b_acceleration;
 
   // Bring back to manifold
   Matrix9 D_newState_xi;
@@ -261,21 +261,22 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
   if (F) {
     F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
   }
-  // derivative wrt omega
-  if (G1) {
-    // D_newState_dRxi = D_newState_xi.leftCols<3>()
-    // D_dRxi_omega = dt * I_3x3
-    // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
-    *G1 = D_newState_xi.leftCols<3>() * dt;
-  }
   // derivative wrt acceleration
-  if (G2) {
+  if (G1) {
     // D_newState_dPxi = D_newState_xi.middleCols<3>(3)
     // D_dPxi_acc = dt22 * I_3x3
     // D_newState_dVxi = D_newState_xi.rightCols<3>()
     // D_dVxi_acc = dt * I_3x3
     // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
-    *G2 = D_newState_xi.middleCols<3>(3) * dt22 + D_newState_xi.rightCols<3>() * dt;
+    *G1 = D_newState_xi.middleCols<3>(3) * dt22
+        + D_newState_xi.rightCols<3>() * dt;
+  }
+  // derivative wrt omega
+  if (G2) {
+    // D_newState_dRxi = D_newState_xi.leftCols<3>()
+    // D_dRxi_omega = dt * I_3x3
+    // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
+    *G2 = D_newState_xi.leftCols<3>() * dt;
   }
   return newState;
 }
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index e33b5fc9f..9561aa77b 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -212,9 +212,9 @@ public:
   /// @name Dynamics
   /// @{
 
-  /// Integrate forward in time given angular velocity and acceleration
+  /// Integrate forward in time given angular velocity and acceleration in body frame
   /// Uses second order integration for position, returns derivatives except dt.
-  NavState update(const Vector3& omega, const Vector3& acceleration,
+  NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
       const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
       OptionalJacobian<9, 3> G2) const;
 
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index a885936aa..b05a8a3e5 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -206,11 +206,11 @@ TEST(NavState, Update) {
   NavState expected(kAttitude.expmap(deltaT * omega),
       kPosition + Point3((kVelocity + b_acc * deltaT / 2) * deltaT),
       kVelocity + b_acc * deltaT);
-  NavState actual = kState1.update(omega, acc, deltaT, aF, aG1, aG2);
+  NavState actual = kState1.update(acc, omega, deltaT, aF, aG1, aG2);
   EXPECT(assert_equal(expected, actual));
-  EXPECT(assert_equal(numericalDerivative31(update, kState1, omega, acc, 1e-7), aF, 1e-7));
-  EXPECT(assert_equal(numericalDerivative32(update, kState1, omega, acc, 1e-7), aG1, 1e-7));
-  EXPECT(assert_equal(numericalDerivative33(update, kState1, omega, acc, 1e-7), aG2, 1e-7));
+  EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
+  EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
+  EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
 }
 
 /* ************************************************************************* */