python實現kalman濾波的一個簡單例子

語言: CN / TW / HK

2年前研究濾波的時候就一直想寫一篇關於kalman濾波器的文章,只是由於各種各樣的原因一直拖到現在。趁著五一有空把這個簡單的小問題處理一下,給出Kalman濾波的一個簡單python實現,完成這篇博文。

雖然kalman濾波已經有很好的開源庫來實現,比如pykalman

http://pykalman.github.io/

但為了更深入地理解kalman濾波的方法,有必要給出一個簡單的例子,對理解各種引數的作用和影響,以及演算法本身的思想和意義都有幫助。

 

卡爾曼濾波的本質是對最小二乘法的迭代運算,可以給出時間序列的狀態估計。假設其要估計的過程如下:

x[k+1] = A[k]*x[k] + B*u[k] + w[k]  // 狀態方程
z[k] = H[k]*x[k] + v[k]             // 測量值
p(w) ~ N(0, Q)
p(v) ~ N(0, R)

其中w和v代表滿足正態分佈的噪音項,該正態分佈均值為0,協方差矩陣分別為Q和R。u代表對x的控制項,z代表測量值,k+1和k代表不同時刻的值。A,B,H分別為相應的關聯矩陣。卡爾曼濾波將該過程的預測值分為兩部分,一是通過模型對先驗值的推斷,稱為時間更新;二是通過測量值進行修正,稱為測量更新。其核心方程為:

// 時間更新
xb[k+1] = A[k]*x[k] + B*u[k]
Pb[k+1] = A[k]*P[k]*transverse(A[k]) + Q[k]
// 測量更新
K[k] = Pb[k]*transverse(H[k])*inverse(H[k]*Pb[k]*transverse(H[k])+R[k])
x[k] = xb[k] + K*(z[k] - H[k]*xb[k])
P[k] = (I - K[k]*H[k])*Pb[k]

其中xb為狀態先驗估計值,Pb為先驗誤差協方差矩陣,P為後驗誤差協方差矩陣。在每次時間更新中,利用前一個後驗估計值給出下一時刻的先驗估計值xb,並給出下一個時刻的先驗誤差協方差估計。在每次測量更新中,先計算出卡爾曼增益K,然後利用測量值z和先驗估計值xb計算出當前的後驗估計值x,最後再給出當前的後驗誤差協方差估計。

這兩個更新過程融合了先驗估計(從過去的資料和模型推斷的系統狀態)和可能存在噪音的測量值,從而給出了系統最有可能的狀態(分佈)。該方法的優點在於,在測量和控制都不夠精確的情況下,給出結合二者資料的最佳估計。下面給出簡單的python程式碼及執行結果供參考。

import numpy

class Kalman_Filter:
    def __init__(self, A, H, Q, R, z, B = None, impulse = None):
        self._A = A
        self._H = H
        self._Q = Q
        self._R = R
        self._z = z

        self.m = len(z)
        self.n = len(z[0])
        self._identity = numpy.ones([self.n, self.n])

        if (B is None):
            self._B = numpy.zeros([self.n, self.n])
        else:
            self._B = B
        if (impulse is None):
            self._impulse = numpy.zeros([self.m, self.n])
        else:
            self._impulse = impulse

    def __del__(self):
        return

    def _kalman(self, xb, Pb, z, impulse):
        # 測量更新
        tmp = numpy.matmul(Pb, self._H.T)
        K = numpy.matmul(tmp, numpy.linalg.inv(numpy.matmul(self._H, tmp) + self._R))
        x = xb + numpy.matmul(K, (z - numpy.matmul(self._H, xb)))
        P = numpy.matmul((self._identity - numpy.matmul(K, self._H)), Pb)
        # 時間更新
        xb = numpy.matmul(self._A, x) + numpy.matmul(self._B, impulse)
        Pb = numpy.matmul(numpy.matmul(self._A, P), self._A.T) + self._Q
        return x, xb, Pb

    def _kalman1d(self, xb, Pb, z, impulse):
        # 測量更新
        tmp = Pb*self._H
        K = tmp/(self._H*tmp + self._R)
        x = xb + K*(z - self._H*xb)
        P = (1 - K*self._H)*Pb
        # 時間更新
        xb = self._A*x + self._B*impulse
        Pb = self._A*P*self._A + self._Q
        return x, xb, Pb

    def get_filtered_data(self, xb, Pb):
        xx = []
        for i in range(0, self.m):
            if (self.n == 1):
                (x, xb, Pb) = self._kalman1d(xb, Pb, self._z[i], self._impulse[i])
            else:
                (x, xb, Pb) = self._kalman(xb, Pb, self._z[i], self._impulse[i])
            xx.append(x)
        return xx



# =========== test ===============
import matplotlib.pyplot

t = numpy.linspace(0,10,100)   # 橫座標,時間
# ================= 2d ==================
A = numpy.array([[1,0.1], [0,1]])
H = numpy.array([[1,0],[0,1]])
Q = 0.5*numpy.array([[1,0],[0,1]])
R = 0.5*numpy.array([[1,0],[0,1]])
noise = numpy.random.randn(2, 100)
real = numpy.vstack((10*numpy.sin(t), 10*numpy.cos(t)))   # 真實值
z = real + noise   # 測量值
kf = Kalman_Filter(A, H, Q, R, z.T)
xb = numpy.array([0,10])
Pb = numpy.array([[1,0],[0,1]])
x = kf.get_filtered_data(xb, Pb)

fig = matplotlib.pyplot.figure(figsize=(10.24,7.68))
matplotlib.pyplot.plot(t, z.T, 'r')
matplotlib.pyplot.plot(t, real.T, 'g')
matplotlib.pyplot.plot(t, x, 'b')
matplotlib.pyplot.show()

# =================== 1d =================
A = 1
H = 1
Q = 0.5
R = 0.5
B = -1     # 根據反饋進行修正
noise = numpy.random.randn(1, 100)
real = 10*numpy.exp(-t*t)
z = real + noise
kf1 = Kalman_Filter(A, H, Q, R, z.T)   # 不加反饋
kf2 = Kalman_Filter(A, H, Q, R, z.T, B, noise.T)   # 反饋修正
xb = 10
Pb = 1
x1 = kf1.get_filtered_data(xb, Pb)
x2 = kf2.get_filtered_data(xb, Pb)

fig = matplotlib.pyplot.figure(figsize=(10.24,7.68))
matplotlib.pyplot.subplot(3,1,1)  # 下面畫第一個圖,不帶反饋修正
matplotlib.pyplot.plot(t, z.T, 'r')
matplotlib.pyplot.plot(t, real.T, 'g')
matplotlib.pyplot.plot(t, x1, 'b')
matplotlib.pyplot.subplot(3,1,2)    # 下面畫第二個圖,帶反饋修正
matplotlib.pyplot.plot(t, z.T, 'r')
matplotlib.pyplot.plot(t, real.T, 'g')
matplotlib.pyplot.plot(t, x2, 'b')
matplotlib.pyplot.subplot(3,1,3)     # 下面畫第三個圖,比較帶反饋和不帶反饋的結果
matplotlib.pyplot.plot(t, x1, 'r')
matplotlib.pyplot.plot(t, real.T, 'g')
matplotlib.pyplot.plot(t, x2, 'b')
matplotlib.pyplot.show()

計算結果如下。可以看到,在大部分情況下,藍線(Kalman濾波結果)要比紅線(測量值)更加接近綠線(真實值)。在第二個圖中,對比了加入外部反饋以根據測量結果進行修正和不加的情況。可以看到,增加反饋後在某些較大的值處給出比較好的結果,在0點附近震盪更加均勻。但反饋無法在所有位置都改善結果。