diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index a955cc7dd..23a72a6bb 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -246,11 +246,11 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
   Matrix39 D_xiP_state;
   double dt22 = 0.5 * dt * dt;
   dR(xi) << dt * omega;
-  dP(xi) << dt * bodyVelocity(D_xiP_state) + dt22 * acceleration;
+  dP(xi) << dt * bodyVelocity(F ? &D_xiP_state : 0) + dt22 * acceleration;
   dV(xi) << dt * acceleration;
 
   Matrix9 D_result_xi;
-  NavState result = retract(xi, F, D_result_xi);
+  NavState result = retract(xi, F, G1 || G2 ? &D_result_xi : 0);
 
   // Derivative wrt state is computed by retract directly
   // However, as dP(xi) also depends on state, we need to add that contribution
@@ -259,11 +259,19 @@ NavState NavState::update(const Vector3& omega, const Vector3& acceleration,
   }
   // derivative wrt omega
   if (G1) {
-    *G1 = dt * D_result_xi.leftCols<3>();
+    // D_result_dRxi = D_result_xi.leftCols<3>()
+    // D_dRxi_omega = dt * I_3x3
+    // *G1 = D_result_omega = D_result_dRxi * D_dRxi_omega
+    *G1 = D_result_xi.leftCols<3>() * dt;
   }
   // derivative wrt acceleration
   if (G2) {
-    *G2 = dt22 * D_result_xi.middleCols<3>(3) + dt * D_result_xi.rightCols<3>();
+    // D_result_dPxi = D_result_xi.middleCols<3>(3)
+    // D_dPxi_acc = dt22 * I_3x3
+    // D_result_dVxi = D_result_xi.rightCols<3>()
+    // D_dVxi_acc = dt * I_3x3
+    // *G2 = D_result_acc = D_result_dPxi * D_dPxi_acc + D_result_dVxi * D_dVxi_acc
+    *G2 = D_result_xi.middleCols<3>(3) * dt22 + D_result_xi.rightCols<3>() * dt;
   }
   return result;
 }