initialization of orientation calculation for Kalman, Madgwick & Mahony

[lembit1]

analytical() rotates the initial quat according to R_initialOrientation. Kalman, Madgwick and Mahony all appear to initialize to zero quat-vector. Is it possible to initialize these 3 functions analogously to analytical function?

[gcosne]

Asking the question for Kalman Initialization:

According to Yun et al.: The state vector is 7-D, with the first three components being the angular rate and the last four components being the quaternion

Does changing the line State Initialization with something like:

x_k = np.zeros(7)  # state vector
if quat_init:
    x_k[3:] = quat_init

would it be enough? It doesn't seem to initialize well when I do that (although I can correctly initialize Madgwick).

Thank you for your help!

[thomas-haslwanter]

I am currently trying to re-write it, since I don't think it is correct. I have also implemented some movement simulations (under skinematics/simulations), to get a better control on the accuracy of the implementations. But work is going slowly. It would be massively helpful to have some other Kalman implementation for IMUs for reference - so if you know anything that is published, I would be grateful.

[gcosne]

Thank you for your answer, I am personally looking for a performant Kalman filter for smartphone yaw estimation, would work the same for general IMUs. I suggest you have a look at this extensive study from Inria benchmarks-attitude Although I have yet to find the best trade-off between implementation cost and attitude-estimation performance.