卡爾曼濾波器的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()