Madgwick IMU算法在iphone上模拟(Madgwick IMU algorithm simulate on iphone)

我在网上到处搜索，但找不到我的问题的解决方案;

我正在使用我的iPhone尝试Madgwick MadgwickAHRSupdateIMU算法（带有6个参数的一个 - 3个陀螺仪输出和3个加速度计输出）; 但无法获得稳定的俯仰/滚转/偏航角度;

以下是Madgwick算法的链接 -

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

以下是我正在使用的源代码的链接 -

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

所以我的第一个问题是我想知道在进入Madgwick的MadgwickAHRSupdateIMU函数时我应该使用什么约定。我非常确定我的iPhone的协调是ENU-x积极指向东方，y积极指向北方，z积极指向观察者。我尝试过交换和反转轴的不同组合; 它们都不是完美的。（gy，gx，-gz，ay，ax，-az）给出了最好的结果，尽管它仍然非常不稳定;

第二个问题是我应该使用的QuaternionToEuler约定，我对这个主题不是很熟悉，但我猜不同的QuaternionToEuler约定是根据不同的协调系统。 Madgwick在他的论文中给出了QuaternionToEuler函数，但它对我不起作用。我认为在我的案例中可能恰好是一个错误的协调系统。

希望我已经清楚地解释了我的问题; 我真的很感激任何投入;

谢谢，

Dihan

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

最满意答案

事实上，Madgwick使用NED坐标进行实现，并且它的代码针对NED进行了优化，这就是为什么很难知道应该更改哪条线来提供ENU四元数。

感谢其论文或内部报告，您可以找到以下公式： Madgwick F功能

J是F的雅可比矩阵。

用于matlab实现的未优化的F et J函数是：

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)];

哪里：

q是t-1处的四元数

b是地球磁场参考系的四元数。

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

有关b的更多信息，请访问https://en.wikipedia.org/wiki/Earth%27s_magnetic_field#Description

当然，您总是需要按正确的顺序放置数据：gx，gy，gz ax，ay，az ......用于两种实现。

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.

更多推荐