update prototype

gtsam/navigation/NavState.cpp

@@ -226,6 +226,50 @@ Matrix7 NavState::wedge(const Vector9& xi) {
 #define D_v_t(H) (H)->block<3,3>(6,3)
 #define D_v_v(H) (H)->block<3,3>(6,6)
 
+//------------------------------------------------------------------------------
+NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
+    const double deltaT, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
+    OptionalJacobian<9, 3> G2) const {
+
+  TIE(nRb, n_t, n_v, *this);
+
+  // Calculate acceleration in *current* i frame, i.e., before rotation update below
+  Matrix3 D_acc_R;
+  const Matrix3 dRij = R(); // expensive in quaternion case
+  const Vector3 i_acc = nRb.rotate(acceleration, D_acc_R);
+
+  // rotation vector describing rotation increment computed from the
+  // current rotation rate measurement
+  const Vector3 integratedOmega = omega * deltaT;
+  Matrix3 D_incrR_integratedOmega;
+  Rot3 incrR = Rot3::Expmap(integratedOmega, G1 ? &D_incrR_integratedOmega : 0); // expensive !!
+
+  Matrix3 D_Rij_incrR;
+  double dt22 = 0.5 * deltaT * deltaT;
+  /// TODO(frank): use retract(deltaT*[omega, bRn*n_v * 0.5*deltaT*acceleration, acceleration])
+  /// But before we do, figure out retract derivatives that are nicer than Lie-generated ones
+  NavState result(nRb.compose(incrR, D_Rij_incrR),
+      n_t + Point3(dt22 * i_acc + deltaT * n_v), n_v + deltaT * i_acc);
+
+  // derivative wrt state
+  if (F) {
+    F->setIdentity();
+    D_R_R(F) << D_Rij_incrR;
+    D_t_R(F) << dt22 * D_acc_R;
+    D_t_v(F) << I_3x3 * deltaT;
+    D_v_R(F) << deltaT * D_acc_R;
+  }
+  // derivative wrt omega
+  if (G1) {
+    *G1 << D_incrR_integratedOmega * deltaT, Z_3x3, Z_3x3;
+  }
+  // derivative wrt acceleration
+  if (G2) {
+    *G2 << Z_3x3, dt22 * dRij, dRij * deltaT;
+  }
+  return result;
+}
+
 //------------------------------------------------------------------------------
 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
     OptionalJacobian<9, 9> H) const {

gtsam/navigation/NavState.h

@@ -209,6 +209,12 @@ public:
   /// @name Dynamics
   /// @{
 
+  /// Integrate forward in time given angular velocity and acceleration
+  /// Uses second order integration for position, returns derivatives except deltaT.
+  NavState update(const Vector3& omega, const Vector3& acceleration,
+      const double deltaT, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
+      OptionalJacobian<9, 3> G2) const;
+
   /// Compute tangent space contribution due to Coriolis forces
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
       OptionalJacobian<9, 9> H = boost::none) const;

gtsam/navigation/tests/testNavState.cpp

@@ -139,7 +139,8 @@ TEST( NavState, Manifold ) {
 
   // Check localCoordinates derivatives
   boost::function<Vector9(const NavState&, const NavState&)> local =
-      boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none);
+      boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
+          boost::none);
   // from state1 to state2
   kState1.localCoordinates(state2, aH1, aH2);
   EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
@@ -177,6 +178,27 @@ TEST( NavState, Lie ) {
   EXPECT(assert_equal(xi, -state3.logmap(state2)));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Update) {
+  const Vector3 omega(M_PI / 100.0, 0.0, 0.0);
+  const Vector3 acc(0.1, 0.0, 0.0);
+  const double deltaT = 10;
+  Matrix9 aF;
+  Matrix93 aG1, aG2;
+  boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
+      boost::bind(&NavState::update, _1, _2, _3, deltaT, boost::none,
+          boost::none, boost::none);
+  Vector3 b_acc = kRotation * acc;
+  NavState expected(kRotation.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);
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(numericalDerivative31(update, kState1, omega, acc), aF));
+  EXPECT(assert_equal(numericalDerivative32(update, kState1, omega, acc), aG1));
+  EXPECT(assert_equal(numericalDerivative33(update, kState1, omega, acc), aG2));
+}
+
 /* ************************************************************************* */
 static const double dt = 2.0;
 boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
@@ -207,7 +229,7 @@ TEST(NavState, Coriolis2) {
 }
 
 /* ************************************************************************* */
-TEST(NavState, correctPIM) {
+TEST(NavState, CorrectPIM) {
   Vector9 xi;
   xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
   double dt = 0.5;