diff --git a/gtsam/navigation/AggregateImuReadings.cpp b/gtsam/navigation/AggregateImuReadings.cpp
index 9830299dc..d5273263f 100644
--- a/gtsam/navigation/AggregateImuReadings.cpp
+++ b/gtsam/navigation/AggregateImuReadings.cpp
@@ -36,19 +36,24 @@ namespace sugar {
 static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) { return v.segment<3>(0); }
 static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) { return v.segment<3>(3); }
 static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) { return v.segment<3>(6); }
+
+typedef const Vector9 constV9;
+static Eigen::Block<constV9, 3, 1> dR(constV9& v) { return v.segment<3>(0); }
+static Eigen::Block<constV9, 3, 1> dP(constV9& v) { return v.segment<3>(3); }
+static Eigen::Block<constV9, 3, 1> dV(constV9& v) { return v.segment<3>(6); }
 }  // namespace sugar
 
 // See extensive discussion in ImuFactor.lyx
-void AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
-                                          const Vector3& w_body, double dt,
-                                          Vector9* zeta,
-                                          OptionalJacobian<9, 9> A,
-                                          OptionalJacobian<9, 3> B,
-                                          OptionalJacobian<9, 3> C) {
+Vector9 AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
+                                             const Vector3& w_body, double dt,
+                                             const Vector9& zeta,
+                                             OptionalJacobian<9, 9> A,
+                                             OptionalJacobian<9, 3> B,
+                                             OptionalJacobian<9, 3> C) {
   using namespace sugar;
 
-  const Vector3 theta = dR(*zeta);
-  const Vector3 v = dV(*zeta);
+  const Vector3 theta = dR(zeta);
+  const Vector3 v = dV(zeta);
 
   // Calculate exact mean propagation
   Matrix3 H;
@@ -57,9 +62,10 @@ void AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
   const Vector3 a_nav = R * a_body;
   const double dt22 = 0.5 * dt * dt;
 
-  dR(*zeta) += invH * w_body * dt;     // theta
-  dP(*zeta) += v * dt + a_nav * dt22;  // position
-  dV(*zeta) += a_nav * dt;             // velocity
+  Vector9 zetaPlus;
+  dR(zetaPlus) = dR(zeta) + invH * w_body * dt;     // theta
+  dP(zetaPlus) = dP(zeta) + v * dt + a_nav * dt22;  // position
+  dV(zetaPlus) = dV(zeta) + a_nav * dt;             // velocity
 
   if (A) {
     // First order (small angle) approximation of derivative of invH*w:
@@ -84,6 +90,8 @@ void AggregateImuReadings::UpdateEstimate(const Vector3& a_body,
     C->block<3, 3>(3, 0) = Z_3x3;
     C->block<3, 3>(6, 0) = Z_3x3;
   }
+
+  return zetaPlus;
 }
 
 void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
@@ -96,7 +104,7 @@ void AggregateImuReadings::integrateMeasurement(const Vector3& measuredAcc,
   // Do exact mean propagation
   Matrix9 A;
   Matrix93 B, C;
-  UpdateEstimate(a_body, w_body, dt, &zeta_, A, B, C);
+  zeta_ = UpdateEstimate(a_body, w_body, dt, zeta_, A, B, C);
 
   // propagate uncertainty
   // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
diff --git a/gtsam/navigation/AggregateImuReadings.h b/gtsam/navigation/AggregateImuReadings.h
index 7cc8fab95..5c77f6104 100644
--- a/gtsam/navigation/AggregateImuReadings.h
+++ b/gtsam/navigation/AggregateImuReadings.h
@@ -74,11 +74,11 @@ class AggregateImuReadings {
 
   // Update integrated vector on tangent manifold zeta with acceleration
   // readings a_body and gyro readings w_body, bias-corrected in body frame.
-  static void UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
-                             double dt, Vector9* zeta,
-                             OptionalJacobian<9, 9> A = boost::none,
-                             OptionalJacobian<9, 3> B = boost::none,
-                             OptionalJacobian<9, 3> C = boost::none);
+  static Vector9 UpdateEstimate(const Vector3& a_body, const Vector3& w_body,
+                                double dt, const Vector9& zeta,
+                                OptionalJacobian<9, 9> A = boost::none,
+                                OptionalJacobian<9, 3> B = boost::none,
+                                OptionalJacobian<9, 3> C = boost::none);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/navigation/tests/testAggregateImuReadings.cpp b/gtsam/navigation/tests/testAggregateImuReadings.cpp
index 86acf93ff..63a127ee4 100644
--- a/gtsam/navigation/tests/testAggregateImuReadings.cpp
+++ b/gtsam/navigation/tests/testAggregateImuReadings.cpp
@@ -39,8 +39,7 @@ static boost::shared_ptr<AggregateImuReadings::Params> defaultParams() {
 }
 
 Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
-  Vector9 zeta_plus = zeta;
-  AggregateImuReadings::UpdateEstimate(a, w, kDt, &zeta_plus);
+  Vector9 zeta_plus = AggregateImuReadings::UpdateEstimate(a, w, kDt, zeta);
   return zeta_plus;
 }
 
@@ -50,10 +49,9 @@ TEST(AggregateImuReadings, UpdateEstimate1) {
   const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
   Vector9 zeta;
   zeta.setZero();
-  Vector9 zeta2 = zeta;
   Matrix9 aH1;
   Matrix93 aH2, aH3;
-  pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3);
+  pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
   EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
   EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
   EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
@@ -65,10 +63,9 @@ TEST(AggregateImuReadings, UpdateEstimate2) {
   const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
   Vector9 zeta;
   zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
-  Vector9 zeta2 = zeta;
   Matrix9 aH1;
   Matrix93 aH2, aH3;
-  pim.UpdateEstimate(acc, omega, kDt, &zeta2, aH1, aH2, aH3);
+  pim.UpdateEstimate(acc, omega, kDt, zeta, aH1, aH2, aH3);
   // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
   EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
   EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-7));