卡爾曼濾波器預測原理

世事短如春夢，人情薄似秋雲。
不須計較苦勞心，萬事原來有命。
幸遇三杯酒好，況逢一朵花新。
片時歡笑且相親，明日陰晴未定。
——《西江月》 朱敦儒

1、引子

有一輛汽車在馬路上勻加速前進，隨着時間的推移，汽車的位置和速度都會發生變化，而在真實世界中，汽車的位置和速度跟理想狀態下是不同的，好比會受到風力影響，致使汽車的運動方式不是嚴格的勻加速運動。那麼在這種狀況下如何對汽車的運動狀態進行預測呢？沒錯，這個問題能夠用今天介紹的卡爾曼濾波器（Kalman Filter, KF）來解決。

圖1. 汽車運動圖示

2、卡爾曼錄波器原理

仍是以上述汽車勻加速行駛做爲例子來介紹卡爾曼濾波器的原理。假設如今有兩種方式能夠對汽車的運動狀態進行估計，即理論預測和實際測量。

圖2. 汽車運動狀態變換示意圖

2.1 理論預測

第一種是經過理論估計來預測汽車在 \(t\) 時刻的狀態，假如咱們已經知道汽車在 \(t-1\) 時刻的位置和速度，以及勻加速運動的加速度 \(a\)，那麼，咱們能夠根據勻加速運動定律對汽車在 \(t\) 時刻的位置和速度進行一個理想（沒有偏差）的估計：

\[{d_{t|t - 1}} = {d_{t - 1|t - 1}} + {v_{t - 1|t - 1}}\Delta t + \frac{1}{2}a\Delta {t^2}\quad\quad(1) \]

一樣的，咱們也能夠對 \(t\) 時刻的汽車速度作一個理想的（沒有偏差）估計：

\[{v_{t|t - 1}} = {v_{t - 1|t - 1}} + a\Delta t\quad\quad(2) \]

上面所述的位置 \(d\) 和速度 \(v\) 稱爲系統的狀態變量，若是還須要估計其餘的狀態變量，如汽車的耗油量、發動機的溫度等，均可以相似位置和速度同樣加進來，這裏僅以位置和速度爲例介紹。上面兩個狀態變量的理想估計能夠寫成緊湊的矩陣形式：

\[{{\pmb{x}}_{t|t - 1}} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right] \times \left[ {\begin{array}{*{20}{c}} {{d_{t - 1}}}\\ {{v_{t - 1}}} \end{array}} \right] + {\pmb{B}}\left[ {\begin{array}{*{20}{c}} {\frac{1}{2}\Delta {t^2}}\\ {\Delta t} \end{array}} \right] \times {{\pmb{u}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad\quad(3)\]

這裏 \({\pmb{F}} = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right]\) 稱爲狀態變換矩陣，\({\pmb{B}} = \left[ {\begin{array}{*{20}{c}} {\frac{1}{2}\Delta {t^2}}\\ {\Delta t} \end{array}} \right]\) 稱爲控制矩陣，\({{\pmb{u}}_t} = a\) 稱爲控制向量。

這裏 \({{\pmb{x}}_{t|t - 1}}\) 表示基於 \(t-1\) 時刻估計狀態下理論預測的 \(t\) 時刻的狀態，\({{\pmb{x}}_{t - 1|t - 1}}\) 表示在 \(t-1\) 時刻綜合理論預測和實際測量獲得的估計值。其餘的下標具備相似意思。

然而實際系統中總會存在噪聲，好比汽車的運動並不嚴格遵循勻加速運動。那麼汽車在 \(t\) 時刻的真實狀態能夠表示爲基於 \(t-1\) 時刻的真實狀態值通過 \(\Delta t\) 時間後加上噪聲項：

\[{{\pmb{x}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t} + {{\pmb{w}}_t}\quad\quad(4) \]

這裏 \({\pmb{x}_{t}}\) 表示 \(t\) 時刻的狀態真實值，\({{\pmb{x}}_{t - 1}}\) 表示汽車在 \(t-1\) 時刻的狀態真實值。噪聲 \({{\pmb{w}}_t}\) 爲服從均值爲0，協方差爲 \({{\pmb{Q}}_t}\) 的二元獨立高斯分佈，\({{\pmb{Q}}_t} = {\mathop{\rm cov}} ({{\pmb{w}}_t})\)。

那麼如何度量理論估計值 \(\pmb{x}_{t|t - 1}\) 與真實狀態 \({\pmb{x}_{t}}\) 之間的差距呢？也就是說理論估計值的估計偏差有多少？這能夠用協方差矩陣來表示：

\[{{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\quad\quad(5) \]

(5)式通過推導得：

\[\begin{array}{l} {{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\\ {\rm{ \quad\quad\:\:= cov(}}{\pmb{F}}{{\bf{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t}{\rm{ + }}{{\pmb{w}}_t} - {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} - {\pmb{B}}{{\pmb{u}}_t}{\rm{)}}\\ {\rm{ \quad\quad\:\:= cov}}\left( {{\pmb{F}}({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){\rm{ + }}{{\pmb{w}}_t}} \right)\\ \quad\quad\:\:= {\pmb{F}}{\mathop{\rm cov}} ({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){{\pmb{F}}^{\rm T}} + {\mathop{\rm cov}} ({{\pmb{w}}_t})\\ \quad\quad\:\:= {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\pmb{Q}}_t} \end{array}\quad\quad(6)\]

2.2 實際測量

第二種是經過各類傳感器或其餘手段來測量狀態變量，好比力傳感器，加速度傳感器等。一樣的，經過傳感器測量獲得的準確值能夠表示爲：

\[{{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad\quad(7) \]

這裏 \(\pmb{H}\) 也是一個變換矩陣，做用是將真實狀態空間映射到測量空間，好比說前面理論估計的是位置和速度，若是傳感器測量的是力或者加速度，那麼須要經過變換矩陣 \(\pmb{H}\) 將位置和加速度映射到力或加速度相同的空間中來。噪聲 \({{\pmb{v}}_t}\) 爲服從均值爲0，協方差爲 \({{\pmb{R}}_t}\) 的二元獨立高斯分佈，\({{\pmb{R}}_t} = {\mathop{\rm cov}} ({{\pmb{v}}_t})\)。

相似於理論預測中用協方差來度量理論估計值與真實值之間的估計偏差，一樣的，理論估計值與測量值之間的偏差也能夠用協方差矩陣來表示：

\[{{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\quad\quad(8) \]

(8)式通過推導得：

\[\begin{array}{l} {{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\\ \quad= {\mathop{\rm cov}} ({\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t} - {\pmb{H}}{{\bf{x}}_{t|t - 1}})\\ \quad= {\mathop{\rm cov}} \left( {{\pmb{H}}({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}}){{\pmb{H}}^{\rm T}}} \right) + {\mathop{\rm cov}} ({{\pmb{v}}_t})\\ \quad= {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t} \end{array}\quad(9)\]

2.3 理論預測與實際測量融合

如今已經知道了 \(t\) 時刻的理論預測值和實際測量值，並知道了理論預測值與 \(t\) 時刻真實值及測量值之間的估計偏差，那麼根據理論預測值和實際測量值獲得 \(t\) 時刻的估計值呢？卡爾曼濾波的思想是分別給理論預測值和實際測量值一個權重，經過理論預測值與實際測量值的加權線性組合來獲得估計值，即：

\[{{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(10) \]

這裏 \({{\pmb{K}}_t}\) 稱爲卡爾曼增益，
那麼這個權重怎麼肯定呢？

咱們的目標是使得 \(t\) 時刻加權後的估計值與系統的真實值之間的偏差最小，也就是 \({{\pmb{x}}_{t|t}}\) 與 \({{\pmb{x}}_t}\) 之間的距離最小化。在這個前提下求得的權重因子（卡爾曼增益）就是最佳的。那麼\({{\pmb{x}}_{t|t}}\) 與 \({{\pmb{x}}_t}\) 之間的距離最小化能夠描述爲：

\[\mathop {\min }\limits_{{{\pmb{K}}_t}} {\left\| {{{\pmb{x}}_t} - {{\pmb{x}}_{t|t}}} \right\|^2} \Leftrightarrow \mathop {\min }\limits_{{{\pmb{K}}_t}} {\rm{Tr}}\left( {{\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t}})} \right) = \mathop {\min }\limits_{{{\pmb{K}}_t}} {\mathop{\rm Tr}\nolimits} ({{\pmb{P}}_{t|t}})\quad(11) \]

這一位置只需到展開 \({{\pmb{P}}_{t|t}}\)，而後對其求一階導，令導數等於0，便可獲得卡爾曼增益：

\[{{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad\quad\quad(12) \]

獲得卡爾曼增益後，將 \({{\pmb{K}}_t}\) 代回 \({{\pmb{P}}_{t|t}}\) 中便可獲得關於 \({{\pmb{K}}_t}\) 的加權估計下的協方差矩陣:

\[{{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad\quad\quad(13) \]

2.4 卡爾曼濾波器迭代過程

綜上所述，卡爾曼濾波器的迭代過程能夠總結爲：

\[\begin{array}{l} {{\pmb{x}}_{t|t - 1}} = {\pmb{F}}{{\bf{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad(1)\\ {{\pmb{P}}_{t|t - 1}} = {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\bf{Q}}_t}\quad(2)\\ {{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad(3)\\ {{\pmb{S}}_t} = {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t}\quad(4)\\ {{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(5)\\ {{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad(6)\\ {{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad(7) \end{array}\]

3、討論

最小二乘（Least Squares）是優化方法中的一種特殊狀況，而卡爾曼濾波又是最小二乘法的一種特殊狀況。 古典最小二乘中，假設了每一次測量的權重相同，但事實上這樣並不合理，後來演化爲加權最小二乘法，至此最小二乘估計所作的都是批處理（Batch），這樣比較佔內存，不符合動態系統狀態估計的須要，即每一次更新輸入時，都要重新計算以前全部的記錄值。然後，提出遞推最小二乘法，模型就不用每次都從新計算了。與遞歸最小二乘類似，卡爾曼濾波加入了系統內部變化的考慮。即利用process model對系統在下一時刻的狀態進行預測。

當對於系統不夠了解時，使用最小二乘法比較合適，而對於系統瞭解比較多時，能夠採用Kalman濾波。改變量測噪聲、系統噪聲都會對Kalman濾波的效果產生影響，而不會對最小二乘濾波產生影響，而改變最小二乘的階數會對其產生影響。

往期精選：

• 新基建新機遇！100頁PPT

• 毫米波大規模陣列中的AOA估計

• 線性降維：主成分分析PCA原理分析與仿真驗證

• 5G+AI：有哪些新的研究方向和新範式？

• 簡述3D點雲配准算法

• 5G爲人工智能與工業互聯網賦能|79頁高清PPT

• 智能算法|以動物命名的算法

• 一份超全面的機器學習公共數據集

• 矩陣填充|奇異值閾值算法

• 可重構/大規模智能反射表面reconfigurable/large intelligent surface綜述

• 迭代硬閾值類算法總結||IHT/NIHT/CGIHT/HTP

• 軟閾值迭代算法（ISTA）和快速軟閾值迭代算法（FISTA）

• 伍德伯裏矩陣恆等式（Woodbury matrix identity）

• 壓縮感知：一種新型亞採樣技術

• 最大比率傳輸

更多精彩內容請關注訂閱號優化與算法和加入QQ討論羣1032493483獲取更多資料