# 导航 – 使用卡尔曼滤波器集成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
``````

``````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()
``````