diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp
index f6924a824..14f255ec0 100644
--- a/fuse_core/include/fuse_core/util.hpp
+++ b/fuse_core/include/fuse_core/util.hpp
@@ -317,9 +317,9 @@ static inline void quaternionToAngleAxis(const double* q, double* angle_axis, do
     else
     {
       jacobian_map.setZero();
+      jacobian_map(0, 0) = 2.0;
       jacobian_map(1, 1) = 2.0;
       jacobian_map(2, 2) = 2.0;
-      jacobian_map(3, 3) = 2.0;
     }
   }
 }