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

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

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

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

我使用的是RobotC（与其他C派生类一样），如果有人可以给我有关如何在C中实现此功能的示例，我将不胜感激。

感谢您的帮助！

解决方案结果：

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

结果1

结果＃2

如您所见，该滤波器可校正陀螺仪的漂移，并将两个信号合并为一个平滑信号。

编辑：由于我还是要修复损坏的图像，所以我认为显示用于生成此数据的装备会很有趣：

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

PS。如果您卡在卡尔曼滤波器上，这是实现它的Arduino的一些C语法代码。

Gamasutra.com刊登了一篇有关使用Kalman过滤器进行WiiMote过滤的文章。

本文结尾处有一些指向C ++源代码的链接。