這篇文章主要講解了“python實(shí)現(xiàn)kalman濾波的方法”，文中的講解內(nèi)容簡(jiǎn)單清晰，易于學(xué)習(xí)與理解，下面請(qǐng)大家跟著小編的思路慢慢深入，一起來(lái)研究和學(xué)習(xí)“python實(shí)現(xiàn)kalman濾波的方法”吧！

成都創(chuàng)新互聯(lián)公司專(zhuān)業(yè)為企業(yè)提供永吉網(wǎng)站建設(shè)、永吉做網(wǎng)站、永吉網(wǎng)站設(shè)計(jì)、永吉網(wǎng)站制作等企業(yè)網(wǎng)站建設(shè)、網(wǎng)頁(yè)設(shè)計(jì)與制作、永吉企業(yè)網(wǎng)站模板建站服務(wù)，10年永吉做網(wǎng)站經(jīng)驗(yàn)，不只是建網(wǎng)站，更提供有價(jià)值的思路和整體網(wǎng)絡(luò)服務(wù)。

卡爾曼濾波的本質(zhì)是對(duì)最小二乘法的迭代運(yùn)算，可以給出時(shí)間序列的狀態(tài)估計(jì)。假設(shè)其要估計(jì)的過(guò)程如下：

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

其中w和v代表滿(mǎn)足正態(tài)分布的噪音項(xiàng)，該正態(tài)分布均值為0，協(xié)方差矩陣分別為Q和R。u代表對(duì)x的控制項(xiàng)，z代表測(cè)量值，k+1和k代表不同時(shí)刻的值。A，B，H分別為相應(yīng)的關(guān)聯(lián)矩陣。卡爾曼濾波將該過(guò)程的預(yù)測(cè)值分為兩部分，一是通過(guò)模型對(duì)先驗(yàn)值的推斷，稱(chēng)為時(shí)間更新；二是通過(guò)測(cè)量值進(jìn)行修正，稱(chēng)為測(cè)量更新。其核心方程為：

// 時(shí)間更新
xb[k+1] = A[k]*x[k] + B*u[k]
Pb[k+1] = A[k]*P[k]*transverse(A[k]) + Q[k]
// 測(cè)量更新
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為狀態(tài)先驗(yàn)估計(jì)值，Pb為先驗(yàn)誤差協(xié)方差矩陣，P為后驗(yàn)誤差協(xié)方差矩陣。在每次時(shí)間更新中，利用前一個(gè)后驗(yàn)估計(jì)值給出下一時(shí)刻的先驗(yàn)估計(jì)值xb，并給出下一個(gè)時(shí)刻的先驗(yàn)誤差協(xié)方差估計(jì)。在每次測(cè)量更新中，先計(jì)算出卡爾曼增益K，然后利用測(cè)量值z(mì)和先驗(yàn)估計(jì)值xb計(jì)算出當(dāng)前的后驗(yàn)估計(jì)值x，最后再給出當(dāng)前的后驗(yàn)誤差協(xié)方差估計(jì)。

這兩個(gè)更新過(guò)程融合了先驗(yàn)估計(jì)（從過(guò)去的數(shù)據(jù)和模型推斷的系統(tǒng)狀態(tài)）和可能存在噪音的測(cè)量值，從而給出了系統(tǒng)最有可能的狀態(tài)（分布）。該方法的優(yōu)點(diǎn)在于，在測(cè)量和控制都不夠精確的情況下，給出結(jié)合二者數(shù)據(jù)的最佳估計(jì)。下面給出簡(jiǎn)單的python代碼及運(yùn)行結(jié)果供參考。

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):
        # 測(cè)量更新
        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)
        # 時(shí)間更新
        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):
        # 測(cè)量更新
        tmp = Pb*self._H
        K = tmp/(self._H*tmp + self._R)
        x = xb + K*(z - self._H*xb)
        P = (1 - K*self._H)*Pb
        # 時(shí)間更新
        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)   # 橫坐標(biāo)，時(shí)間
# ================= 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)))   # 真實(shí)值
z = real + noise   # 測(cè)量值
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     # 根據(jù)反饋進(jìn)行修正
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)  # 下面畫(huà)第一個(gè)圖，不帶反饋修正
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)    # 下面畫(huà)第二個(gè)圖，帶反饋修正
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)     # 下面畫(huà)第三個(gè)圖，比較帶反饋和不帶反饋的結(jié)果
matplotlib.pyplot.plot(t, x1, 'r')
matplotlib.pyplot.plot(t, real.T, 'g')
matplotlib.pyplot.plot(t, x2, 'b')
matplotlib.pyplot.show()

計(jì)算結(jié)果如下。可以看到，在大部分情況下，藍(lán)線（Kalman濾波結(jié)果）要比紅線（測(cè)量值）更加接近綠線（真實(shí)值）。在第二個(gè)圖中，對(duì)比了加入外部反饋以根據(jù)測(cè)量結(jié)果進(jìn)行修正和不加的情況。可以看到，增加反饋后在某些較大的值處給出比較好的結(jié)果，在0點(diǎn)附近震蕩更加均勻。但反饋無(wú)法在所有位置都改善結(jié)果。

感謝各位的閱讀，以上就是“python實(shí)現(xiàn)kalman濾波的方法”的內(nèi)容了，經(jīng)過(guò)本文的學(xué)習(xí)后，相信大家對(duì)python實(shí)現(xiàn)kalman濾波的方法這一問(wèn)題有了更深刻的體會(huì)，具體使用情況還需要大家實(shí)踐驗(yàn)證。這里是創(chuàng)新互聯(lián)，小編將為大家推送更多相關(guān)知識(shí)點(diǎn)的文章，歡迎關(guān)注！

當(dāng)前標(biāo)題：python實(shí)現(xiàn)kalman濾波的方法
URL分享：http://www.chinadenli.net/article10/igjpgo.html

成都網(wǎng)站建設(shè)公司_創(chuàng)新互聯(lián)，為您提供定制網(wǎng)站、品牌網(wǎng)站建設(shè)、網(wǎng)站維護(hù)、動(dòng)態(tài)網(wǎng)站、建站公司、網(wǎng)站策劃

廣告

聲明：本網(wǎng)站發(fā)布的內(nèi)容（圖片、視頻和文字）以用戶(hù)投稿、用戶(hù)轉(zhuǎn)載內(nèi)容為主，如果涉及侵權(quán)請(qǐng)盡快告知，我們將會(huì)在第一時(shí)間刪除。文章觀點(diǎn)不代表本網(wǎng)站立場(chǎng)，如需處理請(qǐng)聯(lián)系客服。電話(huà)：028-86922220；郵箱：631063699@qq.com。內(nèi)容未經(jīng)允許不得轉(zhuǎn)載，或轉(zhuǎn)載時(shí)需注明來(lái)源： 創(chuàng)新互聯(lián)