c - 结合陀螺仪和加速度计数据

Question

我正在使用 Lego Mindstorm 的 NXT 系统构建一个平衡机器人。我正在使用 HiTechnic 的两个传感器，第一个是加速度计，第二个是陀螺仪。我已经成功地过滤掉了来自两个传感器的噪音，以及在 -90 到 90 度之间的范围内的派生角度，0 度是完美平衡的。

我的下一个挑战是结合两个传感器值来校正陀螺仪随时间的漂移。下面是我根据实际数据创建的示例图，用于演示陀螺仪的漂移：

我见过的最常用的组合这些传感器坚如磐石的方法是使用卡尔曼滤波器。但是，我不是微积分专家，我真的不懂数学符号，但我确实了解源代码中的数学。

我正在使用 RobotC（就像任何其他 C 派生词一样），如果有人能给我提供如何在 C 中完成此任务的示例，我将不胜感激。

感谢您的帮助！

解决结果：

好的，kersny 通过向我介绍互补过滤器解决了我的问题。这是说明我的结果的图表：

结果#1

结果#2

如您所见，滤波器校正陀螺漂移并将两个信号组合成一个平滑信号。

编辑：因为无论如何我都在修复损坏的图像，所以我认为展示我用来生成这些数据的装备会很有趣：

Answer 1

卡尔曼滤波器非常棒，但我发现互补滤波器更容易实现，结果相似。我找到的用于编码互补滤波器的最佳文章是这个 wiki（以及这篇关于将传感器转换为工程单位的文章）和此页面上 zip 文件中的 PDF （在技术文档下，我相信 zip 中的文件名是过滤器.pdf);

PS。如果你卡在卡尔曼滤波器上，这里有一些用于实现它的 Arduino 的 C 语法代码。

Answer 2

Gamasutra.com发表了一篇关于使用卡尔曼滤波器进行 WiiMote 滤波的文章。

文末有一些 C++ 源代码的链接。