Madgwick IMU algorithm simulates on iphone
I have searched all over the internet but could not find a solution to my problem;
I am trying to use Madgwick MadgwickAHRSupdateIMU algorithm (the one with 6 parameters - 3 gyroscope outputs and 3 accelerometer outputs) using my iPhone; but could not get a stable tilt / roll / yaw angle;
Following is the link of Maggwick's algorithm -
http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
Below is the link to the source code which I am 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 MadgwickAHRSupdateIMU function. I'm pretty sure my iPhone's coordination is ENU - x positive points east, y positive points north, z positive points for the observer. I've tried different combinations of axis swap and invert; none of them work perfectly. (gy, gx, -gz, ay, ax, -az) gives the best result, although it is still very unstable;
The second question is how to use the QuaternionToEuler convention, I am not very good at this topic, but in my opinion the different QuaternionToEuler conventions correspond to a different coordinate system. Madgwick gives the QuaternionToEuler function in his article, but that didn't work for me. I think this is probably the wrong coordination system in my case.
I hope I have explained my questions clearly; and I really appreciate any input;
thank,
Dihan
source to share
In fact, Madgwick uses NED coordinates to implement it, and this code is optimized for NED, so it's hard to figure out which line you should change to provide ENU quaternions.
Thanks to his document or his internal report, you can find the formula:
And J is the Jacobi matrix F.
Unoptimized F and J functions for matlab implementation:
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 - quaternion at t-1
-
b - quaternion from the earth's magnetic field reference frame.
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 on b can be found here https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Description
Of course, you always need to put the data in the correct order: gx, gy, gz ax, ay, az ... for both implementations.
source to share