I'd like to implement an Extended Kalman Filter to estimate the state$$s=[w,x,y,z,av_x,av_y,av_z]$$with $[w,x,y,z]^T$ respresenting the current orientation as a quaternion and $[av_x,av_y,av_z]^T$ representing the estimated angular velocity.

Moreover I do not have a control input (besides a timestamp). My measurements are quaternions. My prediction function would estimate the new state like this. The angular velocities stay constant. The quaternion is predicted with the current estimated angular velocity and the old quaternion. In Pseudo code:

$$ q_t = q_{t-1} \times (\text{Angular velocity} × Δ\text{Time}) $$ In particular I used this approach: https://stackoverflow.com/a/24201879/654005

Moreover I've implemented the Jacobian (Following https://en.wikipedia.org/wiki/Extended_Kalman_filter notation) $$ F = \frac {df}{x} $$

The H Matrix (Jacobian of Measurement) has just ones since h(x) just takes the quaternion part of x to get the measurement z.

I initialized the State with 0. Unfortunatelly my Jacobian returns "NaN" for most of the values if Angular Velocity is 0. Thus I modified the Jacobian by checking the Angular Velocity for 0. If it is 0, I return an Identity Matrix. But now the AV stays at 0 forever. If I initialize AV with small values it is updated.

My noise matrices are ordinary diagonal matrices representing the process and measurement noise.

Is this the correct approach to estimate orientation and orientation derivative (= angular velocity) with an extended kalman filter?

Thanks for your support.

  • $\begingroup$ A better approach might be to use a multiplicative extended Kalman filter. $\endgroup$ – Kwin van der Veen Apr 8 at 13:35
  • $\begingroup$ Thank you. Can you please be more specific, why this would be a better approach? Do you have an implementation of this approach as well? $\endgroup$ – Soccertrash Apr 8 at 14:25

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.