diff --git a/include/IMU/math/IMU_tools_Lie.h b/include/IMU/math/IMU_tools_Lie.h
index 0646064933f1608d50952bd5e3b274f61baef899..7dad22964556e53e0c7cccb5381ce9a98519d73a 100644
--- a/include/IMU/math/IMU_tools_Lie.h
+++ b/include/IMU/math/IMU_tools_Lie.h
@@ -417,14 +417,18 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
         Id.setIdentity();
         Matrix<T, 3, 1> u = th / thn;
         Matrix<T, 3, 3> ux = skew(u);
+        Matrix<T, 3, 3> ux2 = ux*ux;
+        T thn2 = thn*thn;
+        T sin_thn = sin(thn);
+        T cos_thn = cos(thn);
 
         Q = Id 
-        + ((1  - cos(thn))/thn)*ux 
-        + ((thn - sin(thn))/(thn*thn))*(ux*ux);
+        + ((1  - cos_thn)/thn)*ux
+        + ((thn - sin_thn)/thn2)*(ux2);
         
         P = 0.5*Id 
-        + ((thn - sin(thn))/(thn*thn))*ux  
-        + ((cos(thn) + 0.5*thn*thn - 1)/(thn*thn))*(ux*ux);
+        + ((thn - sin_thn)/thn2)*ux
+        + ((cos_thn + 0.5*thn2 - 1)/thn2)*ux2;
     }
 }