卡曼濾波器實(shí)現(xiàn)多目標(biāo)跟蹤解析 2

通過測(cè)量來細(xì)化估計(jì)值

我們可能有好幾個(gè)傳感器，它們一起提供有關(guān)系統(tǒng)狀態(tài)的信息。傳感器的作用不是我們關(guān)心的重點(diǎn)，它可以讀取位置，可以讀取速度，重點(diǎn)是，它能告訴我們關(guān)于狀態(tài)的間接信息——它是狀態(tài)下產(chǎn)生的一組讀數(shù)。

請(qǐng)注意，讀數(shù)的規(guī)模和狀態(tài)的規(guī)模不一定相同，所以我們把傳感器讀數(shù)矩陣設(shè)為

把這些分布轉(zhuǎn)換為一般形式

卡爾曼濾波的一大優(yōu)點(diǎn)是擅長處理傳感器噪聲。換句話說，由于種種因素，傳感器記錄的信息其實(shí)是不準(zhǔn)的，一個(gè)狀態(tài)事實(shí)上可以產(chǎn)生多種讀數(shù)。

我們將這種不確定性（即傳感器噪聲）的協(xié)方差設(shè)為

，讀數(shù)的分布均值設(shè)為

?，F(xiàn)在我們得到了兩塊高斯分布，一塊圍繞預(yù)測(cè)的均值，另一塊圍繞傳感器讀數(shù)。

如果要生成靠譜預(yù)測(cè)，模型必須調(diào)和這兩個(gè)信息。也就是說，對(duì)于任何可能的讀數(shù)

，這兩種方法預(yù)測(cè)的狀態(tài)都有可能是準(zhǔn)的，也都有可能是不準(zhǔn)的。重點(diǎn)是我們?cè)趺凑业竭@兩個(gè)準(zhǔn)確率。最簡(jiǎn)單的方法是兩者相乘：

兩塊高斯分布相乘后，我們可以得到它們的重疊部分，這也是會(huì)出現(xiàn)最佳估計(jì)的區(qū)域。換個(gè)角度看，它看起來也符合高斯分布：

事實(shí)證明，當(dāng)你把兩個(gè)高斯分布和它們各自的均值和協(xié)方差矩陣相乘時(shí)，你會(huì)得到一個(gè)擁有獨(dú)立均值和協(xié)方差矩陣的新高斯分布。最后剩下的問題就不難解決了：我們必須有一個(gè)公式來從舊的參數(shù)中獲取這些新參數(shù)！

結(jié)合高斯

兩條高斯曲線相乘

按照一維方程進(jìn)行擴(kuò)展，可得

用k簡(jiǎn)化一下

以上是一維的內(nèi)容，如果是多維空間，把這個(gè)式子轉(zhuǎn)成矩陣格式

這個(gè)矩陣

就是我們說的卡爾曼增益

結(jié)合在一起

截至目前，我們有用矩陣

預(yù)測(cè)的分布，有用傳感器讀數(shù)

預(yù)測(cè)的分布。把它們代入上節(jié)的矩陣等式中：

相應(yīng)的，卡爾曼增益就是：

考慮到

里還包含著一個(gè)

，我們?cè)倬?jiǎn)一下上式

最后，

是我們的最佳估計(jì)值，我們可以把它繼續(xù)放進(jìn)去做另一輪預(yù)測(cè)

4 代碼實(shí)現(xiàn)

In [9]

import matplotlib.pyplot as plt# 模擬數(shù)據(jù)t = np.linspace(1,100,100)# print(t)a = 0.5position = (a * t**2)/2# print(position)position_noise = position+np.random.normal(0,120,size=(t.shape[0])) plt.plot(t,position,label='truth position')  # 原值plt.plot(t,position_noise,label='only use measured position')  # 加入噪聲的值# 初始的估計(jì)的位置就直接用GPS測(cè)量的位置predicts = [position_noise[0]]position_predict = predicts[0]predict_var = 0odo_var = 120**2 #這是我們自己設(shè)定的位置測(cè)量?jī)x器的方差，越大則測(cè)量值占比越低v_std = 50 # 測(cè)量?jī)x器的方差for i in range(1,t.shape[0]):      dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模擬從慣性測(cè)量單元IMU讀取出的速度    position_predict = position_predict + dv # 利用上個(gè)時(shí)刻的位置和速度預(yù)測(cè)當(dāng)前位置    predict_var += v_std**2 # 更新預(yù)測(cè)數(shù)據(jù)的方差    # 下面是Kalman濾波    position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)    predict_var = (predict_var * odo_var)/(predict_var + odo_var)**2    predicts.append(position_predict)    plt.plot(t,predicts,label='kalman filtered position')  # 濾波后的值plt.legend()plt.show()# 卡爾曼濾波將噪聲值（橙色線），濾波后（綠色線），盡量去擬合原值（藍(lán)色線）