convert covariance of quaternions into covariance of Euler angles

I'm currently adding a new user message to the INS application to output the covariance of the estimated values.

Just to check, I'm getting them from the diagonal elements of the gKalmanFilter.P, is that right ?

However, the algorithm maintains covariance of quaternion elements. Would someone could give me an idea on how to convert that to covariances for euler angles ?

Thanks in advance.

Generally, this is difficult. However, you can use a first-order approximation.

euler_angles = f(quaternion) ≈ f(quaternion_0) + (df/dquaternion)*(delta_quaterninon)

H = df/dquaternion
  = [droll/dq0   droll/dq1    droll/dq2   droll/dq2
     dpitch/dq0  dpitch/dq1   dpitch/dq2  dpitch/dq3
     dyaw/dq0    dyaw/dq1     dyaw/dq2    dyaw/dq3]

cov(euler_angles) = H*cov(quaternion)*H'

The choice of the function f depends on the rotation sequence of Euler angles you want. We use ZYX in our code.

Log in to reply