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.