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; } }