diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 3a8876ec5..50949c761 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -24,6 +24,18 @@ namespace gtsam {
 
 #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
 
+//------------------------------------------------------------------------------
+NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
+    OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
+    OptionalJacobian<9, 3> H3) {
+  if (H1)
+    *H1 << I_3x3, Z_3x3, Z_3x3;
+  if (H2)
+    *H2 << Z_3x3, R.transpose(), Z_3x3;
+  if (H3)
+    *H3 << Z_3x3, Z_3x3, R.transpose();
+  return NavState(R, t, v);
+}
 //------------------------------------------------------------------------------
 NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
     OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
@@ -94,145 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const {
 }
 
 //------------------------------------------------------------------------------
-NavState NavState::inverse() const {
+NavState NavState::retract(const Vector9& xi, //
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
   TIE(nRb, n_t, n_v, *this);
-  const Rot3 bRn = nRb.inverse();
-  return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
-}
-
-//------------------------------------------------------------------------------
-NavState NavState::operator*(const NavState& bTc) const {
-  TIE(nRb, n_t, n_v, *this);
-  TIE(bRc, b_t, b_v, bTc);
-  return NavState(nRb * bRc, n_t + nRb * b_t, n_v + nRb * b_v);
-}
-
-//------------------------------------------------------------------------------
-//NavState NavState::retract(const Vector9& xi) const {
-//  TIE(nRb, n_t, n_v, *this);
-//  const Rot3 bRc = Rot3::Expmap(dR(xi));
-//  return NavState(nRb * bRc, n_t + nRb * dP(xi), n_v + nRb * dV(xi));
-//}
-
-//------------------------------------------------------------------------------
-NavState::PositionAndVelocity //
-NavState::operator*(const PositionAndVelocity& b_tv) const {
-  TIE(nRb, n_t, n_v, *this);
-  const Point3& b_t = b_tv.first;
-  const Velocity3& b_v = b_tv.second;
-  return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
-}
-
-//------------------------------------------------------------------------------
-Point3 NavState::operator*(const Point3& b_t) const {
-  return Point3(R_ * b_t + t_);
-}
-
-//------------------------------------------------------------------------------
-NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
-    OptionalJacobian<9, 9> H) {
-  Matrix3 D_R_xi;
-  const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0);
-  const Point3 p = Point3(dP(xi));
-  const Vector v = dV(xi);
-  const NavState result(R, p, v);
-  if (H) {
-    *H << D_R_xi, Z_3x3, Z_3x3, //
-    Z_3x3, R.transpose(), Z_3x3, //
-    Z_3x3, Z_3x3, R.transpose();
+  Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
+  const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
+  const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
+  const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
+  const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
+  if (H1) {
+    *H1 << D_R_nRb, Z_3x3, Z_3x3, //
+    // Note(frank): the derivative of n_t with respect to xi is nRb
+    // We pre-multiply with nRc' to account for NavState::Create
+    // Then we make use of the identity nRc' * nRb = bRc'
+    nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
+    // Similar reasoning for v:
+    nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
   }
-  return result;
+  if (H2) {
+    *H2 << D_bRc_xi, Z_3x3, Z_3x3, //
+    Z_3x3, bRc.transpose(), Z_3x3, //
+    Z_3x3, Z_3x3, bRc.transpose();
+  }
+  return NavState(nRc, t, v);
 }
 
 //------------------------------------------------------------------------------
-Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
-    OptionalJacobian<9, 9> H) {
+Vector9 NavState::localCoordinates(const NavState& g, //
+    OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
+  Matrix3 D_dR_R, D_dt_R, D_dv_R;
+  const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
+  const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
+  const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
+
   Vector9 xi;
   Matrix3 D_xi_R;
-  xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v();
-  if (H) {
-    *H << D_xi_R, Z_3x3, Z_3x3, //
-    Z_3x3, x.R(), Z_3x3, //
-    Z_3x3, Z_3x3, x.R();
+  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
+  if (H1) {
+    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
+    D_dt_R, -I_3x3, Z_3x3, //
+    D_dv_R, Z_3x3, -I_3x3;
+  }
+  if (H2) {
+    *H2 << D_xi_R, Z_3x3, Z_3x3, //
+    Z_3x3, dR.matrix(), Z_3x3, //
+    Z_3x3, Z_3x3, dR.matrix();
   }
   return xi;
 }
 
-//------------------------------------------------------------------------------
-NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
-  if (H)
-    throw runtime_error("NavState::Expmap derivative not implemented yet");
-
-  Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
-  Eigen::Block<const Vector9, 3, 1> v = dP(xi);
-  Eigen::Block<const Vector9, 3, 1> a = dV(xi);
-
-  // NOTE(frank): See Pose3::Expmap
-  Rot3 nRb = Rot3::Expmap(n_omega_nb);
-  double theta2 = n_omega_nb.dot(n_omega_nb);
-  if (theta2 > numeric_limits<double>::epsilon()) {
-    // Expmap implements a "screw" motion in the direction of n_omega_nb
-    Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
-    Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
-    Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
-        / theta2;
-    Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
-    Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
-    Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
-    return NavState(nRb, n_t, n_v);
-  } else {
-    return NavState(nRb, Point3(v), a);
-  }
-}
-
-//------------------------------------------------------------------------------
-Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
-  if (H)
-    throw runtime_error("NavState::Logmap derivative not implemented yet");
-
-  TIE(nRb, n_p, n_v, nTb);
-  Vector3 n_t = n_p;
-
-  // NOTE(frank): See Pose3::Logmap
-  Vector9 xi;
-  Vector3 n_omega_nb = Rot3::Logmap(nRb);
-  double theta = n_omega_nb.norm();
-  if (theta * theta <= numeric_limits<double>::epsilon()) {
-    xi << n_omega_nb, n_t, n_v;
-  } else {
-    Matrix3 W = skewSymmetric(n_omega_nb / theta);
-    // Formula from Agrawal06iros, equation (14)
-    // simplified with Mathematica, and multiplying in n_t to avoid matrix math
-    double factor = (1 - theta / (2. * tan(0.5 * theta)));
-    Vector3 n_x = W * n_t;
-    Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
-    Vector3 n_y = W * n_v;
-    Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
-    xi << n_omega_nb, v, a;
-  }
-  return xi;
-}
-
-//------------------------------------------------------------------------------
-Matrix9 NavState::AdjointMap() const {
-  // NOTE(frank): See Pose3::AdjointMap
-  const Matrix3 nRb = R();
-  Matrix3 pAr = skewSymmetric(t()) * nRb;
-  Matrix3 vAr = skewSymmetric(v()) * nRb;
-  Matrix9 adj;
-  //     nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
-  adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
-  return adj;
-}
-
-//------------------------------------------------------------------------------
-Matrix7 NavState::wedge(const Vector9& xi) {
-  const Matrix3 Omega = skewSymmetric(dR(xi));
-  Matrix7 T;
-  T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
-  return T;
-}
-
 //------------------------------------------------------------------------------
 // sugar for derivative blocks
 #define D_R_R(H) (H)->block<3,3>(0,0)
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index 02da46317..a1544735d 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Vector.h>
-#include <gtsam/base/ProductLieGroup.h>
+#include <gtsam/base/Manifold.h>
 
 namespace gtsam {
 
@@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
 
 /**
  * Navigation state: Pose (rotation, translation) + velocity
- * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity
+ * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
  */
-class NavState: public LieGroup<NavState, 9> {
+class NavState {
 private:
 
   // TODO(frank):
@@ -42,6 +42,10 @@ private:
 
 public:
 
+  enum {
+    dimension = 9
+  };
+
   typedef std::pair<Point3, Velocity3> PositionAndVelocity;
 
   /// @name Constructors
@@ -49,7 +53,7 @@ public:
 
   /// Default constructor
   NavState() :
-      t_(0,0,0), v_(Vector3::Zero()) {
+      t_(0, 0, 0), v_(Vector3::Zero()) {
   }
   /// Construct from attitude, position, velocity
   NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@@ -59,15 +63,15 @@ public:
   NavState(const Pose3& pose, const Velocity3& v) :
       R_(pose.rotation()), t_(pose.translation()), v_(v) {
   }
-  /// Construct from Matrix group representation (no checking)
-  NavState(const Matrix7& T) :
-      R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
-  }
   /// Construct from SO(3) and R^6
   NavState(const Matrix3& R, const Vector9 tv) :
       R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
   }
   /// Named constructor with derivatives
+  static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
+      OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
+      OptionalJacobian<9, 3> H3);
+  /// Named constructor with derivatives
   static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
       OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
 
@@ -116,7 +120,8 @@ public:
   /// @{
 
   /// Output stream operator
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState&  state);
+  GTSAM_EXPORT
+  friend std::ostream &operator<<(std::ostream &os, const NavState& state);
 
   /// print
   void print(const std::string& s = "") const;
@@ -124,29 +129,6 @@ public:
   /// equals
   bool equals(const NavState& other, double tol = 1e-8) const;
 
-  /// @}
-  /// @name Group
-  /// @{
-
-  /// identity for group operation
-  static NavState identity() {
-    return NavState();
-  }
-
-  /// inverse transformation with derivatives
-  NavState inverse() const;
-
-  /// Group compose is the semi-direct product as specified below:
-  /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
-  NavState operator*(const NavState& bTc) const;
-
-  /// Native group action is on position/velocity pairs *in the body frame* as follows:
-  /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
-  PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
-
-  /// Act on position alone, n_t = nRb * b_t + n_t
-  Point3 operator*(const Point3& b_t) const;
-
   /// @}
   /// @name Manifold
   /// @{
@@ -172,32 +154,15 @@ public:
     return v.segment<3>(6);
   }
 
-  // Chart at origin, constructs components separately (as opposed to Expmap)
-  struct ChartAtOrigin {
-    static NavState Retract(const Vector9& xi, //
-        OptionalJacobian<9, 9> H = boost::none);
-    static Vector9 Local(const NavState& x, //
-        OptionalJacobian<9, 9> H = boost::none);
-  };
+  /// retract with optional derivatives
+  NavState retract(const Vector9& v, //
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
+          boost::none) const;
 
-  /// @}
-  /// @name Lie Group
-  /// @{
-
-  /// Exponential map at identity - create a NavState from canonical coordinates
-  static NavState Expmap(const Vector9& xi, //
-      OptionalJacobian<9, 9> H = boost::none);
-
-  /// Log map at identity - return the canonical coordinates for this NavState
-  static Vector9 Logmap(const NavState& p, //
-      OptionalJacobian<9, 9> H = boost::none);
-
-  /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
-  /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
-  Matrix9 AdjointMap() const;
-
-  /// wedge creates Lie algebra element from tangent vector
-  static Matrix7 wedge(const Vector9& xi);
+  /// localCoordinates with optional derivatives
+  Vector9 localCoordinates(const NavState& g, //
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
+          boost::none) const;
 
   /// @}
   /// @name Dynamics
@@ -237,14 +202,7 @@ private:
 
 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
 template<>
-struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> {
+struct traits<NavState> : internal::Manifold<NavState> {
 };
 
-// Partial specialization of wedge
-// TODO: deprecate, make part of traits
-template<>
-inline Matrix wedge<NavState>(const Vector& xi) {
-  return NavState::wedge(xi);
-}
-
 } // namespace gtsam
diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h
index 73842ffca..dddafad7a 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -67,7 +67,7 @@ public:
   Vector3  deltaPij() const override { return preintegrated_.segment<3>(3); }
   Vector3  deltaVij() const override { return preintegrated_.tail<3>(); }
   Rot3     deltaRij() const override { return Rot3::Expmap(theta()); }
-  NavState deltaXij() const override { return NavState::Retract(preintegrated_); }
+  NavState deltaXij() const override { return NavState().retract(preintegrated_); }
 
   const Vector9& preintegrated() const { return preintegrated_; }
   Vector3 theta() const     { return preintegrated_.head<3>(); }
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index fb6045d33..8ea8ce9b5 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
 
 /* ************************************************************************* */
 TEST(NavState, Constructor) {
+  boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
+      boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
+          boost::none);
+  Matrix aH1, aH2, aH3;
+  EXPECT(
+      assert_equal(kState1,
+          NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
+  EXPECT(
+      assert_equal(
+          numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
+  EXPECT(
+      assert_equal(
+          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+  EXPECT(
+      assert_equal(
+          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Constructor2) {
   boost::function<NavState(const Pose3&, const Vector3&)> construct =
       boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
           boost::none);
@@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
   EXPECT(assert_equal((Matrix )eH, aH));
 }
 
-/* ************************************************************************* */
-TEST( NavState, MatrixGroup ) {
-  // check roundtrip conversion to 7*7 matrix representation
-  Matrix7 T = kState1.matrix();
-  EXPECT(assert_equal(kState1, NavState(T)));
-
-  // check group product agrees with matrix product
-  NavState state2 = kState1 * kState1;
-  Matrix T2 = T * T;
-  EXPECT(assert_equal(state2, NavState(T2)));
-  EXPECT(assert_equal(T2, state2.matrix()));
-}
-
 /* ************************************************************************* */
 TEST( NavState, Manifold ) {
   // Check zero xi
@@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
   Rot3 drot = Rot3::Expmap(xi.head<3>());
   Point3 dt = Point3(xi.segment<3>(3));
   Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
-  NavState state2 = kState1 * NavState(drot, dt, dvel);
+  NavState state2 = NavState(kState1.attitude() * drot,
+      kState1.position() + kState1.attitude() * dt,
+      kState1.velocity() + kState1.attitude() * dvel);
   EXPECT(assert_equal(state2, kState1.retract(xi)));
   EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
 
@@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
   NavState state3 = state2.retract(xi);
   EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
 
-  // Check derivatives for ChartAtOrigin::Retract
-  Matrix9 aH;
-  //  For zero xi
-  boost::function<NavState(const Vector9&)> Retract = boost::bind(
-      NavState::ChartAtOrigin::Retract, _1, boost::none);
-  NavState::ChartAtOrigin::Retract(kZeroXi, aH);
-  EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
-  //  For non-zero xi
-  NavState::ChartAtOrigin::Retract(xi, aH);
-  EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
-
-  // Check derivatives for ChartAtOrigin::Local
-  //  For zero xi
-  boost::function<Vector9(const NavState&)> Local = boost::bind(
-      NavState::ChartAtOrigin::Local, _1, boost::none);
-  NavState::ChartAtOrigin::Local(kIdentity, aH);
-  EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
-  //  For non-zero xi
-  NavState::ChartAtOrigin::Local(kState1, aH);
-  EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
-
   // Check retract derivatives
   Matrix9 aH1, aH2;
   kState1.retract(xi, aH1, aH2);
@@ -151,6 +139,12 @@ TEST( NavState, Manifold ) {
   EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
   EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
 
+  // Check retract derivatives on state 2
+  const Vector9 xi2 = -3.0*xi;
+  state2.retract(xi2, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
+  EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
+
   // Check localCoordinates derivatives
   boost::function<Vector9(const NavState&, const NavState&)> local =
       boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
@@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
   EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
 }
 
-/* ************************************************************************* */
-TEST( NavState, Lie ) {
-  // Check zero xi
-  EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
-  EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
-  EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
-  EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
-
-  // Expmap/Logmap roundtrip
-  Vector xi(9);
-  xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
-  NavState state2 = NavState::Expmap(xi);
-  EXPECT(assert_equal(xi, NavState::Logmap(state2)));
-
-  // roundtrip from state2 to state3 and back
-  NavState state3 = state2.expmap(xi);
-  EXPECT(assert_equal(xi, state2.logmap(state3)));
-
-  // For the expmap/logmap (not necessarily expmap/local) -xi goes other way
-  EXPECT(assert_equal(state2, state3.expmap(-xi)));
-  EXPECT(assert_equal(xi, -state3.logmap(state2)));
-}
-
 /* ************************************************************************* */
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 TEST(NavState, Update) {
@@ -201,8 +172,8 @@ TEST(NavState, Update) {
   Matrix9 aF;
   Matrix93 aG1, aG2;
   boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
-      boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
-          boost::none, boost::none);
+  boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
+      boost::none, boost::none);
   Vector3 b_acc = kAttitude * acc;
   NavState expected(kAttitude.expmap(dt * omega),
       kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),