卡尔曼滤波器的Python实现

本文介绍了一个基于Python语言的关于卡尔曼滤波器的Demo用于理解卡尔曼滤波器，

卡尔曼滤波器实际上是用来融合多传感器的一种方式，首先我们先解决一个问题，已知传感器A与传感器B分别测量同一个量，二者误差满足均值为零的正态分布，根据极大似然估计，能够得到最优的值应当是，传感器示数的加权平均，均值分别为传感器误差的倒数，

对于一个具有位移传感器与速度传感器的物体，一方面，位移能够通过速度与时间的乘积和上一时刻位移的求和得到一个预测值，

速度是通过速度传感器得到的，满足正态分布，上一时刻位移同样满足正态分布，二者求和即为正态分布求和，等于一个均值为二者均值求和，方差为二者方差求和的正态分布。

预测与实际测得的位移通过上述融合后就得到最优的位移，Python代码如下

import numpy as np
import random
x = 0
v = 0
a = 0
xa = 0
va = 0
aa = 0
xd = 0
vd = 0
ad = 0
xList = []
vList = []
xaList = []
dxList = []
dvList = []
da = 1
dv = 10
dx = 100
for i in range(10000):

    # random.seed(1)
    # 实际运动过程
    a = int(10 * random.random() - 5)
    v += a
    x += v

    # 传感器采集参数，误差满足正态分布
    # random.seed(1)
    tmpa = np.random.normal(a, da, 1)[0]
    # random.seed(1)
    tmpv = np.random.normal(v, dv, 1)[0]
    # random.seed(1)
    tmpx = np.random.normal(x, dx, 1)[0]

    # 加速度数据直接是传感器速度
    aa = tmpa 
    ad = da * da

    # 速度预测是加速度与速度合，方差是二者方差和
    va += aa
    vd += ad

    # 预测速度与速度传感器数据根据方差加权融合
    va = (va / vd + tmpv / (dv * dv)) / (1 / vd + 1 / (dv * dv))
    vd = 1 / (1 / vd + 1 / (dv * dv))

    # 与速度相同
    xa += va
    xd += vd

    # 与速度相同
    xa = (xa / xd + tmpx / (dx * dx)) / (1 / xd + 1 / (dx * dx))
    xd = 1 / (1 / xd + 1 / (dx * dx))
    
    xList.append(x)
    vList.append(v)
    xaList.append(xa)
    dvList.append(v - va)
    dxList.append(x - xa)
from matplotlib import pyplot as plt
plt.plot(xList)
plt.plot(xaList)
plt.figure()
plt.hist(dxList, 100)
print(xd)
from numpy import mean, var
print(mean(dxList), var(dxList))
print(vd)
from numpy import mean, var
print(mean(dvList), var(dvList))
plt.show()