Madgwick传感器融合在LSM9DS0上

时间:2014-09-17 23:59:16

标签: accelerometer quaternions gyroscope sensor-fusion

我试图在STM Cortex M3微控制器上的LSM9DS0传感器(加速度计,陀螺仪和磁力计)上实现Madgwick传感器融合算法from here

所有传感器的原始数据似乎都很好。

我的问题是:当我用传感器保持传感器的z轴水平或向下(即滚动或俯仰角度大于90度)时 - 滤波器的四元数变得非常不稳定并随机翻转180度。 更准确地说,q0和q3是不断变化的符号,导致180度旋转。

我尝试使用常量值而不是真正的传感器输出,但仍然有这种行为。

当z轴或多或少垂直时,四元数似乎是合理的。

我还没有在示例视频中看到过这样的内容。

我试图忽略磁力计数据并使用6DOF版本的过滤器,但这是一场灾难;四元数只是飞来飞去,无法控制地旋转。

我必须将beta参数设置得相当大(~100),因为较小的值会因突然翻转而产生非常不稳定的结果。我发现这很奇怪,因为通常beta约为0.5-0.05。

滤波器更新频率为1KHz。

有人可以帮助我吗?

1 个答案:

答案 0 :(得分:3)

Here据说madgwick过滤器的代码有错误!

渐变体面的步骤应如下所示:

s0= -_2q2*(2*(q1q3 - q0q2) - ax)    +   _2q1*(2*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2*(q1q3 - q0q2) - ax) +   _2q0*(2*(q0q1 + q2q3) - ay) +   -4*q1*(2*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
s2= -_2q0*(2*(q1q3 - q0q2) - ax)    +     _2q3*(2*(q0q1 + q2q3) - ay)   +   (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2*(q1q3 - q0q2) - ax) +   _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

并且官方网站上的代码已过时,很快就会被替换。

纠正这一点产生了令人满意的结果。

我的另一个错误是没有正确阅读功能原型。我复制了稍微更改过的代码版本,其中加速度计和陀螺仪值互换了。