diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h
index 0e1401375..ed68ac077 100644
--- a/gtsam/navigation/ConstantVelocityFactor.h
+++ b/gtsam/navigation/ConstantVelocityFactor.h
@@ -52,20 +52,11 @@ class ConstantVelocityFactor : public NoiseModelFactor2<NavState, NavState> {
         static const Vector3 b_accel{0.0, 0.0, 0.0};
         static const Vector3 b_omega{0.0, 0.0, 0.0};
 
-        NavState predicted;
-        Matrix predicted_H_x1;
+        Matrix99 predicted_H_x1;
+        NavState predicted = x1.update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 : nullptr, {}, {});
 
-        if (H1)
-            predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {});
-        else
-            predicted = x1.update(b_accel, b_omega, dt_, boost::none, {}, {});
-
-        Matrix error_H_predicted;
-        Vector9 error;
-        if (H1)
-            error = predicted.localCoordinates(x2, error_H_predicted, H2);
-        else
-            error = predicted.localCoordinates(x2, boost::none, H2);
+        Matrix99 error_H_predicted;
+        Vector9 error = predicted.localCoordinates(x2, H1 ? &error_H_predicted : nullptr, H2);
 
         if (H1) {
             *H1 = error_H_predicted * predicted_H_x1;