Navigation 利用卡尔曼滤波器整合GPS和加速度计数据的伪代码

我有一些每秒收集数据的加速计传感器：

      AC.X  AC.Y AC.Z
9234 -0.98 -0.10 0.03
9235 -0.98 -0.10 0.03
9236 -0.98 -0.10 0.03
9237 -0.98 -0.10 0.03
9238 -0.98 -0.10 0.03
9239 -0.98 -0.10 0.03

GPS数据集以较长的时间间隔捕获：

        X1        X2         X3 X4          X5 X6 X7 X8   X9   X10 X11  X12 X13 X14 X15 X16 X17 X18
81  $GPGGA 181553.00 4105.86029  N 08754.49966  W  1 06 1.90 00208   M -033   M     *53            
133 $GPGGA 181554.00 4105.86052  N 08754.49954  W  1 05 2.04 00208   M -033   M     *54            
185 $GPGGA 181555.00 4105.86002  N 08754.49950  W  1 06 1.90 00208   M -033   M     *59            
237 $GPGGA 181556.00 4105.85944  N 08754.49889  W  1 06 1.90 00207   M -033   M     *58            
289 $GPGGA 181557.00 4105.85952  N 08754.49877  W  1 07 1.30 00210   M -033   M     *52            
341 $GPGGA 181558.00 4105.85925  N 08754.49852  W  1 06 1.35 00209   M -033   M     *56  

---

我一直在阅读如何使用卡尔曼滤波器来整合这些数据，我开始很好地理解算法的工作原理。两种数据集都引入了不同类型的误差（加速度计数据随时间漂移，但实际上无噪声；GPS数据漂移小，但有大量噪声），而卡尔曼滤波器允许我们利用这些误差剖面来获得更好的导航数据。然而，我仍在努力将其转化为代码。我用R和Python编写代码，但我只是在寻找一些通用的伪代码来开始。你知道有什么好的资源或者有处理这类问题的经验吗？

卡尔曼滤波器是一种基于以前数据的算法。当您有可预测的运动（例如摆动的钟摆）时，它很容易实现。但是，对于一般用途，可能很难正确实施

我发现，如果您只想平滑GPS，那么实现更简单的LLS滤波器（线性最小二乘法）更容易（在某些情况下更可靠）。下面是python示例，其中考虑了二维空间中的三个点，我将其用作代码的起点：

import numpy as np
import matplotlib.pyplot as plt

x = np.random.rand(5,2)*10
a = np.matrix([ [1,x[0][0]], [1,x[1][0]], [1,x[2][0]] ])
b = np.matrix([ [x[0][2]], [x[1][3]], [x[2][4]] ])
yy = (a.T * a).I * a.T*b
xx = np.linspace(1,10,50)
y = np.array(yy[0]+yy[1]*xx)

plt.figure(1)
plt.plot(xx,y.T,color='r')
plt.scatter([x[0][0],x[1][0],x[2][0] ],[x[0][5],x[1][6],x[2][7] ]) 
plt.show()

---

如果您仍然对使用卡尔曼滤波器感兴趣，这对于在动态系统的加速计上实现卡尔曼滤波器非常有用

如果您接受使用Javascript的帮助，我将不胜荣幸。我将非常感谢。。谢谢@ProllyGeekThanks，但这并不能真正回答我的问题。平滑数据是可以的，但我需要集成两个数据集，因为它们有不同的错误模式。@斯图，很抱歉，我一开始可能误解了你的问题。你想要的更多吗？这篇文章提供了很多关于如何实现卡尔曼滤波器的信息，我相信他的“另一个例子”与您尝试实现的是一样的。Phil对“Bob Zoo”提出的线程“gps平滑”的回答也有一些示例实现，虽然不是用R/Python实现的，但应该会有所帮助。是的！非常感谢。你仍然赢得了150个代表。很高兴我能帮上忙。我发现CodeProject是一个非常有用的网站，特别是对于更多的嵌入式编程项目，我会在将来推荐它们作为源代码。