diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 06d00b842..dfbbe9912 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -68,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
 void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
     boost::optional<const Pose3&> body_P_sensor,
-    boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
+    OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
 
   Vector3 correctedAcc, correctedOmega;
   correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
@@ -106,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
     //           intNoise         accNoise      omegaNoise
     (*G_test) << I_3x3 * deltaT,   Z_3x3,        Z_3x3,                                 // pos
                  Z_3x3,            R_i * deltaT, Z_3x3,                                 // vel
-                 Z_3x3,            Z_3x3,        D_Rincr_integratedOmega * deltaT;                // angle
+                 Z_3x3,            Z_3x3,        D_Rincr_integratedOmega * deltaT;      // angle
     // Propagation with no approximation:
     // preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
   }
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index e0a6b861a..125c81040 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -115,7 +115,7 @@ public:
      */
     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
         boost::optional<const Pose3&> body_P_sensor = boost::none,
-        boost::optional<Matrix&> Fout = boost::none, boost::optional<Matrix&> Gout = boost::none);
+        OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
 
     /// methods to access class variables
     Matrix measurementCovariance() const {return measurementCovariance_;}
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 019b35ed9..36bc9aabb 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -75,7 +75,7 @@ public:
 
   /// Update preintegrated measurements
   void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
-      boost::optional<Matrix3&> H = boost::none){
+      OptionalJacobian<3, 3> H = boost::none){
     deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
     deltaTij_ += deltaT;
   }
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index ee4d239a2..e0c13f43d 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -116,7 +116,7 @@ public:
 
   /// Update preintegrated measurements
   void updatePreintegratedMeasurements(const Vector3& correctedAcc,
-      const Rot3& incrR, const double deltaT, boost::optional<Matrix&> F = boost::none) {
+      const Rot3& incrR, const double deltaT,  OptionalJacobian<9, 9> F) {
 
     Matrix3 dRij = deltaRij(); // expensive
     Vector3 temp = dRij * correctedAcc * deltaT;
@@ -127,9 +127,19 @@ public:
     }
     deltaVij_ += temp;
 
+//    const Matrix3 R_i = deltaRij();
+//    OptionalJacobian<3,3> F_angles_angles;
+//    updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none);
+//    if(F){
+//      Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
+//      //    pos          vel              angle
+//      *F << I_3x3,       I_3x3 * deltaT,  Z_3x3,          // pos
+//            Z_3x3,       I_3x3,           F_vel_angles,   // vel
+//            Z_3x3,       Z_3x3,           F_angles_angles;// angle
+//    }
     if(F){
-      Matrix3 F_angles_angles;
       const Matrix3 R_i = deltaRij();
+      Matrix3 F_angles_angles;
       updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
       Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
       F->resize(9,9);