Madgwick IMU algorithm simulate on iphone

I've searched everywhere on the web but could not find a solution of my problem;

I'm trying Madgwick MadgwickAHRSupdateIMU algorithm (The one with 6 parameters - 3 output of gyro and 3 output of accelerometer) using my iPhone; but could not get a stable pitch/roll/yaw angle;

Below is the link of Madgwick algorithm -

http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

Below is the link of the source code I'm using -

http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip

So my first question is I'm wondering what convention should I use when feeding into Madgwick's MadgwickAHRSupdateIMU function. I'm pretty sure coordination of my iPhone is ENU - x positive points to East, y positive points to North, z positive points to observer. I have tried different combinations of swapping and inverting axis; none of them works perfect. (gy, gx, -gz, ay, ax, -az) gives the best result, though it's still very unstable;

The second question is what QuaternionToEuler convention should I use, I'm not very familiar with this topic but I guess different QuaternionToEuler convention is according to different coordination system. Madgwick gives QuaternionToEuler function in his paper but it did not work for me. I think it probably happen to be a wrong coordination system in my case.

Hope I've clearly explained my questions; and I really appreciate for any input;

thanks,

Dihan

Solution

In fact, Madgwick uses the NED coordinates for its implementation and it's code is optimized for NED, that's why it's difficult to know which line you should change to provide ENU quaternions.

Thanks to its paper or its internal report you can find the formula : Madgwick F function

And J is the Jacobian matrix of F.

Unoptimized F et J functions for matlab implementation are :

F = [2*(q(2)*q(4) - q(1)*q(3)) - accelerometer(1)
    2*(q(1)*q(2) + q(3)*q(4)) - accelerometer(2)
    2*(0.5 - q(2)^2 - q(3)^2) - accelerometer(3)
    2*b(2)*(0.5 - q(3)^2 - q(4)^2) + 2*b(3)*(q(1)*q(4) + q(2)*q(3)) + 2*b(4)*(q(2)*q(4) - q(1)*q(3)) - magnetometer(1)
    2*b(2)*(q(2)*q(3) - q(1)*q(4)) + 2*b(3)*(0.5 - q(2)^2 - q(4)^2) + 2*b(4)*(q(1)*q(2) + q(3)*q(4)) - magnetometer(2)
    2*b(2)*(q(1)*q(3) + q(2)*q(4)) + 2*b(3)*(q(3)*q(4) - q(1)*q(2)) + 2*b(4)*(0.5 - q(2)^2 - q(3)^2) - magnetometer(3)
];

J = [-2*q(3),                   2*q(4),                                -2*q(1),                                2*q(2)
    2*q(2),                     2*q(1),                                 2*q(4),                                2*q(3)
    0,                         -4*q(2),                                -4*q(3),                                 0
     2*b(3)*q(4)-2*b(4)*q(3),   2*b(3)*q(3)+2*b(4)*q(4),              -4*b(2)*q(3)+2*b(3)*q(2)-2*b(4)*q(1),   -4*b(2)*q(4)+2*b(3)*q(1)+2*b(4)*q(2)
    -2*b(2)*q(4)+2*b(4)*q(2),   2*b(2)*q(3)-4*b(3)*q(2)+2*b(4)*q(1),   2*b(2)*q(2)+2*b(4)*q(4),               -2*b(2)*q(1)-4*b(3)*q(4)+2*b(4)*q(3)
     2*b(2)*q(3)-2*b(3)*q(2),   2*b(2)*q(4)-2*b(3)*q(1)-4*b(4)*q(2),   2*b(2)*q(1)+2*b(3)*q(4)-4*b(4)*q(3),    2*b(2)*q(2)+2*b(3)*q(3)];

where:

q is the quaternion at t-1
b is the quaternion from reference frame of earth magnetic field.
```
b = [0 0 norm([h(2) h(3)]) h(4)]; % for ENU because y points to the North
b = [0 norm([h(2) h(3)]) 0 h(4)]; % for NED because x points to the North
```
More information about b can be found here https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Description

Of course, you always need to put data in the right order: gx, gy, gz ax, ay, az ... for both implementations.