diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 6ea135922cdb919ef1c0927f162359328dcfc13e..a2140e0992e3c6e22e8828ea9a8468513db6f15d 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -431,7 +431,7 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
 
         Q = Id 
         + ((1  - cos_thn)/thn)*ux
-        + ((thn - sin_thn)/thn2)*(ux2);
+        + ((thn - sin_thn)/thn)*(ux2);
         
         P = 0.5*Id 
         + ((thn - sin_thn)/thn2)*ux