Madgwick IMU算法在iphone上模拟

时间:2014-10-17 01:55:50

标签: iphone algorithm sensor quaternions

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

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

1 个答案:

答案 0 :(得分:1)

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

感谢其论文或its internal report,您可以找到以下公式: Madgwick F function

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

其中:

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