diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp
index 0fcdc6180..3246bed68 100644
--- a/gtsam/navigation/BarometricFactor.cpp
+++ b/gtsam/navigation/BarometricFactor.cpp
@@ -45,9 +45,11 @@ bool BarometricFactor::equals(const NonlinearFactor& expected,
 Vector BarometricFactor::evaluateError(const Pose3& p, const double& bias,
                                        boost::optional<Matrix&> H,
                                        boost::optional<Matrix&> H2) const {
+    Matrix tH;
+    Vector ret =(Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
+    if (H) (*H) = tH.block<1,6>(2,0);
     if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished();
-    if (H) (*H) = (Matrix(1, 6) << 0., 0., 0., 0., 0., 1.).finished();
-    return (Vector(1) << (p.translation().z() + bias - nT_)).finished();
+    return ret;
 }
 
 }  // namespace gtsam