iphone上的Madgwick IMU算法模拟答案

【问题标题】：Madgwick IMU algorithm simulate on iphoneiphone上的Madgwick IMU算法模拟
【发布时间】：2015-07-21 16:09:23
【问题描述】：

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

我正在使用我的 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 函数，但它对我不起作用。我认为这可能恰好是我的情况下的错误协调系统。

希望我已经清楚地解释了我的问题；我非常感谢您的任何意见；

谢谢，

地涵

【问题讨论】：

您好，您的问题解决了吗？我认为是因为协调使用不同
@RichardLe 嗨，理查德，感谢您的回复。我没有尝试解决这个问题，因为我的真实设备中没有这个问题。 iPhone应用程序只用于模拟，因为真机还没有完成，所以我写了这个IOS程序来模拟，以便测试软件。我不认为这是由错误的协调引起的，因为在使用真实设备进行测试时使用了相同的协调。
@RichardLe 顺便说一句，我发布另一个问题；你能检查一下吗？ stackoverflow.com/questions/26540861/… 谢谢！
我在带有加速器、陀螺仪和磁力计的 Android 手机中应用 Madgwick 滤波器，偏航输出似乎像我预期的那样旋转了 90 度。我想问题的原因是来自不同的协调。如果你解决了这个问题，请分享
你看过这篇文章吗？stackoverflow.com/questions/13499838/…

标签： iphone algorithm sensors quaternions

【解决方案1】：

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

感谢它的论文或its internal report，您可以找到公式：

而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 ...。

【讨论】：