卡爾曼濾波

維基百科,自由的百科全書
跳轉到: 導覽搜尋

卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含雜訊測量中,估計動態系統的狀態。

應用實例[編輯]

卡爾曼濾波的一個典型實例是從一組有限的,包含噪聲的,對物體位置的觀察序列(可能有偏差)預測出物體的位置的坐標速度。在很多工程應用(如雷達計算機視覺)中都可以找到它的身影。同時,卡爾曼濾波也是控制理論以及控制系統工程中的一個重要課題。

例如,對於雷達來說,人們感興趣的是其能夠跟蹤目標。但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響,得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波),也可以是對於將來位置的估計(預測),也可以是對過去位置的估計(插值或平滑)。

命名[編輯]

這種濾波方法以它的發明者魯道夫.E.卡爾曼(Rudolph E. Kalman)命名,但是根據文獻可知實際上Peter Swerling在更早之前就提出了一種類似的演算法。

斯坦利.施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時,發現他的方法對於解決阿波羅計劃的軌道預測很有用,後來阿波羅飛船的導航電腦便使用了這種濾波器。 關於這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發表。

目前,卡爾曼濾波已經有很多不同的實現.卡爾曼最初提出的形式現在一般稱為簡單卡爾曼濾波器。除此以外,還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環,它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。


以下的討論需要線性代數以及機率論的一般知識。


基本動態系統模型[編輯]

卡爾曼濾波建立在線性代數隱馬爾可夫模型(hidden Markov model)上。其基本動態系統可以用一個馬爾可夫鏈表示,該馬爾可夫鏈建立在一個被高斯噪聲(即常態分佈的噪聲)干擾的線性算子上的。系統的狀態可以用一個元素為實數的向量表示。 隨著離散時間的每一個增加,這個線性算子就會作用在當前狀態上,產生一個新的狀態,並也會帶入一些噪聲,同時系統的一些已知的控制器的控制信息也會被加入。同時,另一個受噪聲干擾的線性算子產生出這些隱含狀態的可見輸出。

為了從一系列有噪聲的觀察數據中用卡爾曼濾波器估計出被觀察過程的內部狀態,我們必須把這個過程在卡爾曼濾波的框架下建立模型。也就是說對於每一步k,定義矩陣Fk, Hk, Qk, Rk,有時也需要定義Bk,如下。

卡爾曼濾波器的模型。圓圈代表向量,方塊代表矩陣,星號代表高斯噪聲,其協方差矩陣在右下方標出。

卡爾曼濾波模型假設k時刻的真實狀態是從(k − 1)時刻的狀態演化而來,符合下式:

 \textbf{x}_{k} = \textbf{F}_{k} \textbf{x}_{k-1} + \textbf{B}_{k}\textbf{u}_{k}  + \textbf{w}_{k}

其中

  • Fk 是作用在xk−1上的狀態變換模型(/矩陣/矢量)。
  • Bk 是作用在控制器向量uk上的輸入-控制模型。
  • wk 是過程噪聲,並假定其符合均值為零,協方差矩陣Qk多元常態分佈
\textbf{w}_{k} \sim N(0, \textbf{Q}_k)

時刻k,對真實狀態 xk的一個測量zk滿足下式:

\textbf{z}_{k} = \textbf{H}_{k} \textbf{x}_{k} + \textbf{v}_{k}

其中Hk是觀測模型,它把真實狀態空間映射成觀測空間,vk 是觀測噪聲,其均值為零,協方差矩陣為Rk,且服從常態分佈

\textbf{v}_{k} \sim N(0, \textbf{R}_k)

初始狀態以及每一時刻的噪聲{x0, w1, ..., wk, v1 ... vk} 都認為是互相獨立的.

實際上,很多真實世界的動態系統都並不確切的符合這個模型;但是由於卡爾曼濾波器被設計在有噪聲的情況下工作,一個近似的符合已經可以使這個濾波器非常有用了。更多其它更複雜的卡爾曼濾波器的變種,在下邊討論中有描述。

卡爾曼濾波器[編輯]

卡爾曼濾波是一種遞歸的估計,即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值,因此不需要記錄觀測或者估計的歷史信息。卡爾曼濾波器與大多數濾波器不同之處,在於它是一種純粹的時域濾波器,它不需要像低通濾波器頻域濾波器那樣,需要在頻域設計再轉換到時域實現。

卡爾曼濾波器的狀態由以下兩個變數表示:

  • \hat{\textbf{x}}_{k|k},在時刻k 的狀態的估計;
  • \textbf{P}_{k|k},誤差相關矩陣,度量估計值的精確程度。

卡爾曼濾波器的操作包括兩個階段:預測更新。在預測階段,濾波器使用上一狀態的估計,做出對當前狀態的估計。在更新階段,濾波器利用對當前狀態的觀測值優化在預測階段獲得的預測值,以獲得一個更精確的新估計值。

預測[編輯]

\hat{\textbf{x}}_{k|k-1} = \textbf{F}_{k}\hat{\textbf{x}}_{k-1|k-1} + \textbf{B}_{k} \textbf{u}_{k} (預測狀態)
\textbf{P}_{k|k-1} =  \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k} (預測估計協方差矩陣)

可參考:http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新[編輯]

首先要算出以下三個量:

\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1} (測量餘量,measurement residual)
\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k} (測量餘量協方差)
\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1} (最優卡爾曼增益)

然後用它們來更新濾波器變數xP

\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k} (更新的狀態估計)
\textbf{P}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1} (更新的協方差估計)

使用上述公式計算\textbf{P}_{k|k} 僅在最優卡爾曼增益的時候有效。使用其他增益的話,公式要複雜一些,請參見推導

不變數(Invariant)[編輯]

如果模型準確,而且\hat{\textbf{x}}_{0|0}\textbf{P}_{0|0} 的值準確的反映了最初狀態的分布,那麼以下不變數就保持不變:所有估計的誤差均值為零

  • \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k}] = \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}] = 0
  • \textrm{E}[\tilde{\textbf{y}}_k] = 0

協方差矩陣 準確的反映了估計的協方差:

  • \textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k})
  • \textbf{P}_{k|k-1} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})
  • \textbf{S}_{k} = \textrm{cov}(\tilde{\textbf{y}}_k)

請注意,其中\textrm{E}[\textbf{a}]表示{a}的期望值, \textrm{cov}(\textbf{a}) = \textrm{E}[\textbf{a}\textbf{a}^T]

實例[編輯]

考慮在無摩擦的、無限長的直軌道上的一輛車。該車最初停在位置 0 處,但時不時受到隨機的衝擊。我們每隔Δt秒即測量車的位置,但是這個測量是非精確的;我們想建立一個關於其位置以及速度的模型。我們來看如何推導出這個模型以及如何從這個模型得到卡爾曼濾波器。

因為車上無動力,所以我們可以忽略掉Bkuk。由於FHRQ 是常數,所以時間下標可以去掉。

車的位置以及速度(或者更加一般的,一個粒子的運動狀態)可以被線性狀態空間描述如下:

\textbf{x}_{k} = \begin{bmatrix} x \\ \dot{x} \end{bmatrix}

其中\dot{x}是速度,也就是位置對於時間的導數。

我們假設在(k − 1)時刻與k時刻之間,車受到ak的加速度,其符合均值為0,標準差為σa常態分佈。根據牛頓運動定律,我們可以推出

\textbf{x}_{k} = \textbf{F} \textbf{x}_{k-1} + \textbf{G}a_{k}

其中

\textbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}

\textbf{G} = \begin{bmatrix} \frac{\Delta t^{2}}{2} \\ \Delta t \end{bmatrix}

我們可以發現

 \textbf{Q} = \textrm{cov}(\textbf{G}a) = \textrm{E}[(\textbf{G}a)(\textbf{G}a)^{T}] = \textbf{G} \textrm{E}[a^2] \textbf{G}^{T} = \textbf{G}[\sigma_a^2]\textbf{G}^{T} = \sigma_a^2 \textbf{G}\textbf{G}^{T} (因為 σa 是一個純量).

在每一時刻,我們對其位置進行測量,測量受到噪聲干擾.我們假設噪聲服從常態分佈,均值為0,標準差為σz

\textbf{z}_{k} = \textbf{H x}_{k} + \textbf{v}_{k}

其中

\textbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}

\textbf{R} = \textrm{E}[\textbf{v}_k \textbf{v}_k^{T}] = \begin{bmatrix} \sigma_z^2 \end{bmatrix}

如果我們知道足夠精確的車最初的位置,那麼我們可以初始化

\hat{\textbf{x}}_{0|0} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}

並且,我們告訴濾波器我們知道確切的初始位置,我們給出一個協方差矩陣:

\textbf{P}_{0|0} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}

如果我們不確切的知道最初的位置與速度,那麼協方差矩陣可以初始化為一個對角線元素是B的矩陣,B取一個合適的比較大的數。

\textbf{P}_{0|0} = \begin{bmatrix} B & 0 \\ 0 & B \end{bmatrix}

此時,與使用模型中已有信息相比,濾波器更傾向於使用初次測量值的信息。

推導[編輯]

推導後驗協方差矩陣[編輯]

按照上邊的定義,我們從誤差協方差\textbf{P}_{k|k}開始推導如下:

\textbf{P}_{k|k}  = \textrm{cov}(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k})

代入\hat{\textbf{x}}_{k|k}

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k\tilde{\textbf{y}}_{k}))

再代入 \tilde{\textbf{y}}_k

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{z}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))

\textbf{z}_{k}

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{H}_k\textbf{x}_k + \textbf{v}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))

整理誤差向量,得

\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k})(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}) - \textbf{K}_k \textbf{v}_k )

因為測量誤差vk 與其他項是非相關的,因此有

\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k})(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}))  + \textrm{cov}(\textbf{K}_k \textbf{v}_k )

利用協方差矩陣的性質,此式可以寫作

\textbf{P}_{k|k} = (I - \textbf{K}_k \textbf{H}_{k})\textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})(I - \textbf{K}_k \textbf{H}_{k})^{T}  + \textbf{K}_k\textrm{cov}(\textbf{v}_k )\textbf{K}_k^{T}

使用不變數Pk|k-1以及Rk的定義 這一項可以寫作 :\textbf{P}_{k|k}=(I - \textbf{K}_k \textbf{H}_{k}) \textbf{P}_{k|k-1} (I - \textbf{K}_k \textbf{H}_{k})^T +\textbf{K}_k \textbf{R}_k \textbf{K}_k^T 這一公式對於任何卡爾曼增益Kk都成立。如果Kk是最優卡爾曼增益,則可以進一步簡化,請見下文。

最優卡爾曼增益的推導[編輯]

卡爾曼濾波器是一個最小均方誤差估計器,後驗狀態誤差估計(英文:a posteriori state estimate)是

\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}

我們最小化這個矢量幅度平方的期望值,\textrm{E}[|\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}|^2],這等同於最小化後驗估計協方差矩陣 Pk|k跡(trace)。將上面方程中的項展開、抵消,得到:

 \textbf{P}_{k|k}  = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} - \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k (\textbf{H}_k \textbf{P}_{k|k-1} \textbf{H}_k^T + \textbf{R}_k) \textbf{K}_k^T
 = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} - \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k\textbf{K}_k^T

矩陣導數是 0 的時候得到Pk|k跡(trace)的最小值:

\frac{d \; \textrm{tr}(\textbf{P}_{k|k})}{d \;\textbf{K}_k} = -2 (\textbf{H}_k \textbf{P}_{k|k-1})^T + 2 \textbf{K}_k \textbf{S}_k  = 0

此處須用到一個常用的式子, 如下:

   \frac{d \; \textrm{tr}(\textbf{BAC})}{d \;\textbf{A}} = B^TC^T

從這個方程解出卡爾曼增益Kk

\textbf{K}_k \textbf{S}_k = (\textbf{H}_k \textbf{P}_{k|k-1})^T = \textbf{P}_{k|k-1} \textbf{H}_k^T
 \textbf{K}_{k} = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{S}_k^{-1}

這個增益稱為最優卡爾曼增益,在使用時得到最小均方誤差

後驗誤差協方差公式的化簡[編輯]

在卡爾曼增益等於上面導出的最優值時,計算後驗協方差的公式可以進行簡化。在卡爾曼增益公式兩側的右邊都乘以 SkKkT 得到

\textbf{K}_k \textbf{S}_k \textbf{K}_k^T = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T

根據上面後驗誤差協方差展開公式,

 \textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1}  - \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k \textbf{K}_k^T

最後兩項可以抵消,得到

 \textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}.

這個公式的計算比較簡單,所以實際中總是使用這個公式,但是需注意這公式僅在使用最優卡爾曼增益的時候它才成立。如果算術精度總是很低而導致數值穩定性出現問題,或者特意使用非最優卡爾曼增益,那麼就不能使用這個簡化;必須使用上面導出的後驗誤差協方差公式。

與遞歸Bayesian估計之間的關係[編輯]

假設真正的狀態是無法觀察的馬爾可夫過程,測量結果是從隱性馬爾可夫模型觀察到的狀態。

Hidden Markov Model

根據馬爾可夫假設,真正的狀態僅受最近一個狀態影響而與其它以前狀態無關。

p(\textbf{x}_k|\textbf{x}_0,\dots,\textbf{x}_{k-1}) = p(\textbf{x}_k|\textbf{x}_{k-1})

與此類似,在時刻 k 測量只與當前狀態有關而與其它狀態無關。

p(\textbf{z}_k|\textbf{x}_0,\dots,\textbf{x}_{k}) = p(\textbf{z}_k|\textbf{x}_{k} )

根據這些假設,隱性馬爾可夫模型所有狀態的機率分布可以簡化為:

p(\textbf{x}_0,\dots,\textbf{x}_k,\textbf{z}_1,\dots,\textbf{z}_k) = p(\textbf{x}_0)\prod_{i=1}^k p(\textbf{z}_i|\textbf{x}_i)p(\textbf{x}_i|\textbf{x}_{i-1})

然而,當卡爾曼濾波器用來估計狀態x的時候,我們感興趣的機率分布,是基於目前為止所有個測量值來得到的當前狀態之機率分布

 p(\textbf{x}_k|\textbf{Z}_{k-1}) = \int p(\textbf{x}_k | \textbf{x}_{k-1}) p(\textbf{x}_{k-1} | \textbf{Z}_{k-1} )  \, d\textbf{x}_{k-1}

信息濾波器[編輯]

非線性濾波器[編輯]

基本卡爾曼濾波器(The basic Kalman filter)是限制在線性的假設之下。然而,大部份非平凡的(non-trivial)的系統都是非線性系統。其中的「非線性性質」(non-linearity )可能是伴隨存在過程模型(process model)中或觀測模型(observation model)中,或者兩者兼有之。

擴展卡爾曼濾波器[編輯]

在擴展卡爾曼濾波器(Extended Kalman Filter,簡稱EKF)中狀態轉換和觀測模型不需要是狀態的線性函數,可替換為(可微的)函數。

\textbf{x}_{k} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, \textbf{w}_{k})
\textbf{z}_{k} = h(\textbf{x}_{k}, \textbf{v}_{k})

函數 f 可以用來從過去的估計值中計算預測的狀態,相似的,函數 h可以用來以預測的狀態計算預測的測量值。然而 fh 不能直接的應用在協方差中,取而代之的是計算偏導矩陣(Jacobian)。

在每一步中使用當前的估計狀態計算Jacobian矩陣,這幾個矩陣可以用在卡爾曼濾波器的方程中。這個過程,實質上將非線性的函數在當前估計值處線性化了。

這樣一來,卡爾曼濾波器的等式為:

預測

\hat{\textbf{x}}_{k|k-1} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, 0)
 \textbf{P}_{k|k-1} =  \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}

使用Jacobians矩陣更新模型

 \textbf{F}_{k} = \left . \frac{\partial f}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k-1|k-1},\textbf{u}_{k}}
 \textbf{H}_{k} = \left . \frac{\partial h}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1}}

更新

\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - h(\hat{\textbf{x}}_{k|k-1}, 0)
\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}
\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}
\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}
 \textbf{P}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}

預測

如同擴展卡爾曼濾波器(EKF)一樣, UKF的預測過程可以獨立於UKF的更新過程之外,與一個線性的(或者確實是擴展卡爾曼濾波器的)更新過程合併來使用;或者,UKF的預測過程與更新過程在上述中地位互換亦可。

應用[編輯]

參見[編輯]

外部連接[編輯]

參考文獻[編輯]

  • Gelb A., editor. Applied optimal estimation. MIT Press, 1974.
  • Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960)
  • Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961)
  • [JU97] Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997.
  • Harvey, A.C. Forecasting, Structural Time Series Models and the Kalman Filter. Cambridge University Press, Cambridge, 1989.