卡爾曼濾波器預測原理詳解+推導

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

一、引子

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

圖1. 汽車運動圖示

二、卡爾曼錄波器原理

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

圖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{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)$

實際測量

第二種是通過各種傳感器或其他手段來測量狀態變量，比如力傳感器，加速度傳感器等。同樣的，通過傳感器測量得到的準確值可以表示爲：

${{\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)$

理論預測與實際測量融合

現在已經知道了 $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)$

卡爾曼濾波器迭代過程

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

$\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}$

討論

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

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

往期精選：

• 毫米波大規模MIMO的AOA估計

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

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

• 簡述3D點雲配准算法

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

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

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

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

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

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

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

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

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

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