通過測量來細化估計值

我們可能有好幾個傳感器，它們一起提供有關系統狀態的信息。傳感器的作用不是我們關心的重點，它可以讀取位置，可以讀取速度，重點是，它能告訴我們關于狀態的間接信息——它是狀態下產生的一組讀數。

請注意，讀數的規模和狀態的規模不一定相同，所以我們把傳感器讀數矩陣設為

把這些分布轉換為一般形式

卡爾曼濾波的一大優點是擅長處理傳感器噪聲。換句話說，由于種種因素，傳感器記錄的信息其實是不準的，一個狀態事實上可以產生多種讀數。

我們將這種不確定性（即傳感器噪聲）的協方差設為

，讀數的分布均值設為

?，F在我們得到了兩塊高斯分布，一塊圍繞預測的均值，另一塊圍繞傳感器讀數。

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

，這兩種方法預測的狀態都有可能是準的，也都有可能是不準的。重點是我們怎么找到這兩個準確率。最簡單的方法是兩者相乘：

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

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

結合高斯

兩條高斯曲線相乘

按照一維方程進行擴展，可得

用k簡化一下

以上是一維的內容，如果是多維空間，把這個式子轉成矩陣格式

這個矩陣

就是我們說的卡爾曼增益

結合在一起

截至目前，我們有用矩陣

預測的分布，有用傳感器讀數

預測的分布。把它們代入上節的矩陣等式中：

相應的，卡爾曼增益就是：

考慮到

里還包含著一個

，我們再精簡一下上式

最后，

是我們的最佳估計值，我們可以把它繼續放進去做另一輪預測

4 代碼實現

In [9]

import matplotlib.pyplot as plt# 模擬數據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')  # 加入噪聲的值# 初始的估計的位置就直接用GPS測量的位置predicts = [position_noise[0]]position_predict = predicts[0]predict_var = 0odo_var = 120**2 #這是我們自己設定的位置測量儀器的方差，越大則測量值占比越低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 # 利用上個時刻的位置和速度預測當前位置    predict_var += v_std**2 # 更新預測數據的方差    # 下面是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()# 卡爾曼濾波將噪聲值（橙色線），濾波后（綠色線），盡量去擬合原值（藍色線）