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

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

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

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

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

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

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

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

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

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

，這兩種方法預(yù)測的狀態(tài)都有可能是準(zhǔn)的，也都有可能是不準(zhǔn)的。重點(diǎn)是我們怎么找到這兩個準(zhǔn)確率。最簡單的方法是兩者相乘：

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

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

結(jié)合高斯

兩條高斯曲線相乘

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

用k簡化一下

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

這個矩陣

就是我們說的卡爾曼增益

結(jié)合在一起

截至目前，我們有用矩陣

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

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

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

考慮到

里還包含著一個

，我們再精簡一下上式

最后，

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

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測量的位置predicts = [position_noise[0]]position_predict = predicts[0]predict_var = 0odo_var = 120**2 #這是我們自己設(shè)定的位置測量儀器的方差，越大則測量值占比越低v_std = 50 # 測量儀器的方差for i in range(1,t.shape[0]):      dv =  (position[i]-position[i-1]) + np.random.normal(0,50) # 模擬從慣性測量單元IMU讀取出的速度    position_predict = position_predict + dv # 利用上個時刻的位置和速度預(yù)測當(dāng)前位置    predict_var += v_std**2 # 更新預(yù)測數(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)色線）