C 组合陀螺仪和加速度计数据

我正在用乐高Mindstorm的NXT系统制作一个平衡机器人。我使用的是HiTechnic公司的两个传感器，第一个是加速计，第二个是陀螺仪。我已经成功地从两个传感器中滤除了噪声，并在-90到90度的范围内导出了两个传感器的角度，0度完全平衡

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

我见过的最常用的方法是使用卡尔曼滤波器将这些传感器组合在一起。然而，我不是微积分专家，我真的不懂数学符号，但我懂源代码中的数学

我使用的是RobotC（和其他C语言的衍生工具一样），如果有人能给我举个例子说明如何在C语言中实现这一点，我将不胜感激

谢谢你的帮助

解决方案结果：

好的，kersny通过介绍互补滤波器解决了我的问题。这是一张图表，显示了我的结果：

结果#1

结果#2

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

编辑：因为我一直在修复损坏的图像，所以我觉得展示我用来生成这些数据的装备会很有趣：

运行

---

<> P>在本文末尾有一些C++源代码链接。

< P>卡尔曼滤波器是伟大的，所有的，但我发现互补滤波器更容易实现类似的结果。我找到的关于编码补充过滤器的最佳文章有（以及关于将传感器转换为工程单位的文章）和zip文件中的PDF（在技术文档中，我相信zip文件中的文件名是Filter.PDF）

---

注：如果您的卡尔曼滤波器无法正常工作，请参阅实现它的Arduino的C语法代码。

我感谢您的回答。这篇文章也很棒，但我真的需要知道如何使用卡尔曼（或其他）滤波器组合两个传感器值，而不是仅仅平滑一个。据我所知，你平滑一个，然后使用它对另一个应用校正因子。不过，我不能说我完全理解它的工作原理。这些tlb.org/scooter2.html“>两篇tlb.org/scooter.html”>文章可能会有所帮助。太棒了，我相信这正是我想要的。Filter.pdf文件真的帮了大忙，解释并解决了我的确切问题。我还没有核实（我正在工作）。但今晚，我会努力让这一切继续下去，并将我的问题标记为已回答！很高兴我能帮忙！如果你想看到一个实际的例子，请访问我的博客。我建立了一个赛格威式的平衡滑板车，我很快就会发布更多的数据。维基链接似乎是deadYay archive.org：你的数据显然有分歧。如果您的初始数据不一致，卡尔曼滤波或任何其他方法都不会帮助您。我不确定您是否完全理解图形显示的内容，这是已知的陀螺数据漂移问题。这就是数据发散的原因，这就是我正在寻找的过滤器/集成将使用加速计数据进行纠正的原因。此外，产生剧烈漂移的原因是，为了说明我的问题，我非常灵活地晃动了传感器我不知道你在画什么，因为你没有标记轴的，但不管你的数据是否明显偏离相同的y值，给定相同的x值，这都是非常糟糕的数据。如果你对它应用任何过滤器，它试图在最小二乘意义上最小化误差（例如，卡尔曼滤波器所做的）你将平均一个误差，这个误差随着你的x值的增加而增加。很明显，数据的一部分告诉你一些事情，另一部分告诉你其他事情。卡尔曼滤波器的一个很好的替代方法是互补滤波器，它更容易实现：