实现Madgwick IMU算法

Question

我正在尝试使用Sebastian Madgwick的IMU algorithms从我的传感器获得滚动，俯仰和偏航，这是一个MPU-9150。我下载的文件夹附带了一些示例数据，我只是用我自己的数据替换了这些数据，确保单位/约定保持不变等等。问题是算法的输出角度与我的数据没有太大意义，非常嘈杂，尤其是偏航角。滚动和俯仰看似合理，但我可以从加速度计/陀螺仪数据中获得更好的效果。我附上了他的示例数据（有效），我的数据和输出的图。

在尝试使用这些算法之前，我是否需要对数据进行额外的过滤？到目前为止，我一直在玩中值滤波，这似乎没有什么区别。

由于

Answer 1

我碰巧在Tobias Simon的Magdwick算法的不稳定性问题上找到了一些注释，详见

http://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root

问题在于Magdwick实现中使用的快速反平方根。我不清楚问题出在哪里。也许它与算法的精度有关，或者可能是因为原始代码在64位机器上不起作用。用不同的（更好的？）替换实现实现了令人惊讶的更好和更稳定的AHRS计算。 （或者对64位机器的代码进行一些小改动也解决了这个问题？请阅读Tobias Simon原始注释的注释。）

可以在这里找到更好的逆平方根实现

https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/