Madgwick IMU algorithm simulate on iphone

Question

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

Hi, did you solve the problem? I think because the coordination using is different — Richard Le
– Richard Le, Commented Nov 20, 2014 at 4:07
@RichardLe Hi Richard, thanks for your reply. I have not tried to solve the problem since I don't have this issue in my real device. The iPhone application is only used for simulation because the real device has not been completed so I wrote this IOS program to simulate in order to test software. I don't think it's caused by wrong coordination since the same coordination is used when testing with the real device. — Ted Cheng
– Ted Cheng, Commented Nov 22, 2014 at 3:38
@RichardLe By the way, I post another question; can you please check ? stackoverflow.com/questions/26540861/… thanks! — Ted Cheng
– Ted Cheng, Commented Nov 22, 2014 at 3:42
I am applying Madgwick filter in Android phone with accelerator, gyroscope and magnetometer, the yaw output seems to rotate 90 degree as I expected. I guess the cause of problem is from the different coordination. If you solved this problem please share — Richard Le
– Richard Le, Commented Nov 23, 2014 at 6:57
Have you read this post?stackoverflow.com/questions/13499838/… — Richard Le
– Richard Le, Commented Nov 23, 2014 at 6:59

Thibaud Michel · Accepted Answer · 2015-07-21 08:09:23Z

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.

Collectives™ on Stack Overflow

Madgwick IMU algorithm simulate on iphone

1 Answer 1

Comments

Your Answer

Linked

Hot Network Questions

Collectives™ on Stack Overflow

1 Answer 1

Comments

Your Answer

Sign up or log in

Post as a guest

Linked

Related