diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index 0aaa440fa..2c51bc017 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -90,7 +90,8 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     if (H) {
       H->resize(3, 6);
-      *H << zeros(3, 3) << eye(3); // TODO make static
+      H->block < 3, 3 > (0, 0) << zeros(3, 3);
+      H->block < 3, 3 > (0, 3) << p.rotation().matrix();
     }
     // manifold equivalent of h(x)-z -> log(z,h(x))
     return nT_.localCoordinates(p.translation());
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index 539482da9..c9c6fe233 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -54,15 +54,15 @@ TEST( GPSFactor, Constructors ) {
   Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
 
   // Calculate numerical derivatives
-  Matrix expectedH1 = numericalDerivative11<Pose3>(
+  Matrix expectedH = numericalDerivative11<Pose3>(
       boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), pose);
 
   // Use the factor to calculate the derivative
-  Matrix actualH1;
-  factor.evaluateError(pose, actualH1);
+  Matrix actualH;
+  factor.evaluateError(pose, actualH);
 
   // Verify we get the expected error
-  CHECK(assert_equal(expectedH1, actualH1, 1e-9));
+  CHECK(assert_equal(expectedH, actualH, 1e-8));
 }
 
 // *************************************************************************