我在网上到处搜索,但找不到解决我问题的方法;
我正在使用我的 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 函数,但它对我不起作用。我认为在我的案例中,这可能恰好是一个错误的协调系统。
希望我已经清楚地解释了我的问题;我非常感谢任何意见;
谢谢,
迪涵
最佳答案
事实上,Madgwick 使用 NED 坐标来实现它,并且它的代码针对 NED 进行了优化,这就是为什么很难知道您应该更改哪一行以提供 ENU 四元数。
感谢其论文或its internal report你可以找到公式:
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)];
哪里:
- 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 ...对于这两种实现。
关于iphone - Madgwick IMU算法在iphone上模拟,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/26416738/