在iPhone上模拟Madgwick IMU算法

Question

我在网上搜索了很多地方，但是没有找到解决我的问题的方法;

我正在使用我的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

- Ted Cheng

---

嗨，你解决问题了吗？我认为是因为使用的协调方式不同。 - Richard Le

@RichardLe 你好Richard，感谢你的回复。由于我在真实设备上没有遇到这个问题，所以我还没有尝试解决它。iPhone应用程序仅用于模拟，因为真实设备尚未完成，所以我编写了这个IOS程序进行模拟以测试软件。我不认为这是由于错误的协调引起的，因为在与真实设备进行测试时使用的是相同的协调。 - Ted Cheng

@RichardLe 是的，您必须确保四元数到欧拉角的协调和方程式正确。请告诉我Android手机的坐标系以及您如何输入加速度计、陀螺仪和磁力计，以及您用于将四元数转换为欧拉角的方程式，我会看看是否能找出问题所在。 - Ted Cheng

感谢您的回复。由于我不知道发表论文中使用的坐标系（Madgwick在他的论文中没有提到，他只给出了四元数），所以我按照正常方式输入（没有交换或反转），并使用论文中描述的四元数转换为欧拉角，输出结果当然是错误的。您可以阅读下面的帖子了解Android坐标系http://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root?commentId=705844%3AComment%3A1840039 - Richard Le

@RichardLe 抱歉最近太忙了，我会尽力找出我在应用程序中使用的公式，并在接下来的几天内发布。代码是很久以前写的，所以我需要一些时间来整理它。谢谢。 - Ted Cheng

Answer 1

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

感谢其论文或 内部报告, 您可以找到公式：

J 是 F 的雅可比矩阵。

Matlab 实现的未经优化的 F 和 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)];

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

当然，你总是需要按正确的顺序放置数据：gx、gy、gz、ax、ay、az...对于这两种实现都是如此。

- Thibaud Michel