檢視卡爾曼濾波的原始碼

'''卡爾曼濾波'''（Kalman filter）是一種高效率的遞歸濾波器（自回歸濾波器）， 伊會當對系列無完全佮包含雜訊的測量內底，估計動態系統的狀態。卡爾曼濾波會根據各測量佇咧無仝時間下的值，考慮著各時間之下的聯合分布，再產生對無知變數的估計，因此會比只以單一測量為基礎的估計方式愛準。卡爾曼濾波得名自主要貢獻者之一的魯道夫 ・ 卡爾曼。

卡爾曼濾波佇技術領域有真濟應用。捷看的有飛行機佮太空船的導引、導航佮控制。卡爾曼濾波也真闊使用佇時間序列的分析中，譬如講信號處理佮計量經濟學中。卡爾曼濾波也是機器人運動規劃佮控制的重要主題之一，有時也包括佇軌跡最佳化。卡爾曼濾波也用佇中心神經系統運動控制的建模內。因為按給佮運動命令到收著神經的回授之間有時間差，使用卡爾曼濾波有幫贊建立符合實際的系統，估計運動系統的目前狀態，並且更新命令。

卡爾曼濾波的演算法是第二步的程序。佇咧估計步內底，卡爾曼濾波會產生有關係目前狀態的估計，其中嘛包括無確定性。只要觀察著後一个量測（其中一定有某一種程度的精差，包括隨機雜訊）。 會通過加權平均來更新估計值，確定性愈懸的量測加權比重也愈懸。做迵天的，會當佇實時控制系統當中執行，只需要目前的輸入量測、以往的計算值以及其無確定性矩陣，無需要其他往過的資訊。

使用卡爾曼濾波免假設誤差是常態分布，猶毋過所有的誤差攏是定態分布，卡爾曼濾波會使得著正確的條件機率估計。

嘛發展一寡擴展抑是廣義的卡爾曼濾波，譬如運作佇非線性系統的擴展卡爾曼濾波佮無跡卡爾曼濾波（unscented Kalman filter）。 底層的模型類似隱馬爾可夫模型，毋過藏佇咧變量的狀態空間是連紲的，而且所有潛在的變量佮會當觀測變數攏是常態分布。

==應用實例==

卡爾曼濾波的一个典型實例是對一組有限的，包含噪音的，通過對物體位置的觀察序列（可能有偏差）預測出物體的位置的坐標及速度。佇足濟工程應用（如雷達、機器視覺）中攏會當揣著伊的形影。同時，卡爾曼濾波也是控制理論佮控制系統工程內面一个重要的課題。

比如講，對雷達來講，人有興趣的是其會當綴佇目標。毋過目標的位置、速度、加速度的測量值往往佇任何時陣攏有噪音。卡爾曼濾波利用目標的動態信息，設法去落噪音的影響，得著一个關於目標的好的估計。這个估計會當是對當前目標位置的估計（濾波）， 嘛會當對將來位置的估計（預測）， 嘛會當對過去的估計（插值抑是平滑）。

==號名==

這種濾波方法以伊的發明者魯道夫 ・ 卡爾曼（Rudolph Kalman）號名，但是根據文獻會當知影講實際上 Peter Swerling 佇閣較早進前就提出一種類似的算法。

斯坦利 ・ 施密特（Stanley Schmidt）頭擺實現卡爾曼濾波器。卡爾曼在 NASA 埃姆斯研究中心訪問時，發現伊的方法對解決阿波羅計畫的鐵枝道預測誠有路用，後來阿波羅飛船的導航電腦使用這種濾波器。關於這種濾波器的論文 Swerling（一千九百五十八）、 Kalman ( 一千九百六十 ) 佮 Kalman and Bucy（一千九百六十一）發表。

目前，卡爾曼濾波已經有真濟無仝的實現。卡爾曼上頭先提出的形式這馬一般講是 _ 簡單 _ 卡爾曼濾波器。除了這以外，猶閣有施密特'''擴展'''濾波器、'''批信'''濾波器以及真濟 Bierman , Thornton 開發的'''平方根'''濾波器的變種。可能上捷看著的卡爾曼濾波器是鎖相環，伊咧收音機、計算機佮強欲任何視頻抑是通訊設備中廣泛存在。

---

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

---

==做概念==

卡爾曼濾波器使用系統的動態模型（比如講，運動的物理定律）， 該系統的已知控制輸入以及多個順序的測量值（譬論講來自傳感器的測量值）來形成對系統變化量（其狀態）閣較好的估計，其精度比只使用一種測量得著的估算值懸。伊是一種常見的感測器融合和數據融合算法。

感測器數據的雜訊，描述系統演化的方程式的近若像值以及無考慮所有的因素素的外部因素攏限制矣確定系統狀態的能力。卡爾曼濾波器有效地處理因為感測器數據雜訊引起的無確定性，閣佇一定程度上咧處理隨機外部因素。卡爾曼濾波器使用加權平均值生成系統狀態的估計值，做系統預測狀態佮新測量值的平均值。權重的目的是估計值有閣較好（即較細）的無確定性值會予人閣較濟「信任」。 權重是根據共變做異數來計算的，共變異數是對系統狀態預測的估計無確定性的度量。加權平均值的結果是介紹預測狀態佮測量狀態之間的新狀態估計，並且比任何一个狀態攏有閣較好的估計無確定性。佇每一个時間步重複此過程，新的估計值佮其共變異數共通知後續迵天中使用的預測。這意味對卡爾曼濾波器會當遞迴地工作，並且只需要系統狀態的最後「最佳猜測」，毋是規个歷史，就會當計算新的狀態。

測量佮當前狀態估計的相對確定性是重要的考慮因素，通常根據卡爾曼濾波器的增益來討論濾波器的反應。卡爾曼增益是予測量值佮當前狀態估計值的相對權重，會當進行「調整」以得著特定的性能。增益懸的時陣，濾波器將閣較濟的精神囥佇咧上新的測量頂懸，所以反應速度閣較緊。增益較低的時，濾波器會閣較緊密地遵循模型預測。佇極端的狀況之下，接近一个懸增益共致使估計的軌跡閣較跳跳，去接近零的低增益欲消除雜訊，但是會降低反應的速度。

佇執行濾波器的實際計算時（如後所講）， 狀態估計值佮共變異數被編碼到矩陣中，以處理單個計算集中牽涉著的濟个維度。允准佇任何過渡模型抑是共變異數中表示無仝狀態變量（譬如講位置，速度佮加速度）之間的線性關係。

==基本動態系統模型==

卡爾曼濾波建立佇線性代數佮隱馬爾可夫模型（hidden Markov model）上。其基本動態系統會當用一个馬爾會當做夫鏈表示，該馬爾可夫鏈建立佇一个予高斯噪聲（即常態分布的噪聲）干擾的線性算子上的。系統的狀態會當用一个元素為實數的向量表示。隨著離散時間的每一个增加，這个線性算子就會作用咧做前狀態上，產生一个新的狀態，嘛會帶入一寡噪音，仝時陣系統的一寡已經知的控制器的控制信息嘛會予人加入。同時，另外一个受噪音干擾的線性算子產生出遮的隱含狀態的可見輸出。

為著自一系列有噪音的觀察數據中用卡爾曼濾波器估計出被觀察過程的內部狀態，必須共這个過程佇咧卡爾曼濾波的框殼下建立模型。也就是講對每一步 _ k _，定義矩陣'''F'''k ,'''H'''k ,'''Q'''k ,'''R'''k，有當時仔嘛需要定義'''B'''k，如下。

卡爾曼濾波模型假設 _ k _ 時刻的真實的狀態是對（_ k _ −  一）時刻的狀態演化來，符合下式：


: $ { \ textbf { x } } _ { k }={ \ textbf { F } } _ { k } { \ textbf { x } } _ { k 影一 } + { \ textbf { B } } _ { k } { \ textbf { u } } _ { k } + { \ textbf { w } } _ { k } $

其中

*'''F'''k 是作用佇咧'''x'''k− 一上的狀態變換模型（/ 矩陣 / 向量）。
*'''B'''k 是作用佇咧控制器向量'''u'''k 上的輸入－控制模型。
*'''w'''k 是過程噪聲，並假定其符合均值做零，協方差矩陣為'''Q'''k 的多元常態分布。


: $ { \ textbf { w } } _ { k } \ sim N ( 零 , { \ textbf { Q } } _ { k } ) $

時刻 _ k _，著真實狀態'''x'''k 的一个測量'''z'''k 滿足下式：


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

其中'''H'''k 是觀測模型，伊共真實狀態空間影射做觀測空間，'''v'''k 是觀測噪聲，其均值做零，協方差矩陣為'''R'''k , 而且服從常態分布。


: $ { \ textbf { v } } _ { k } \ sim N ( 零 , { \ textbf { R } } _ { k } ) $

初始狀態佮每一時刻的噪音聲 {'''x'''零 ,'''w'''一 , . . . ,'''w'''k ,'''v'''一 . . .'''v'''k } 攏認為講是互相獨立的。

實際上，真濟真實世界的動態系統攏並無確切的符合著這个模型；毋過因為卡爾曼濾波器被設計佇有噪音的情況下作穡，一个近若像的符合已經會使使使這个濾波器是真有路用。閣較濟其他閣較複雜的卡爾曼濾波器的變種，佇下跤咧討論內底伊有講著。

==卡爾曼濾波器==

卡爾曼濾波是一種遞歸的估計，也只要知影頂一時刻狀態的估計值佮當前狀態的觀測值就會當計算出當前狀態的估計值，所以無需要記錄觀測抑是估計的歷史信息。卡爾曼濾波器佮大多數濾波器無仝的所在，佇伊是一種純粹的時域濾波器，伊無需要像低通濾波器等等頻域濾波器彼種，需要佇頻域設計閣換著時域實現。

卡爾曼濾波器的狀態由以下兩个變量表示：

* $ { \ hat { \ textbf { x } } } _ { k | k } $，在時刻 _ k _ 的狀態的估計；
* $ { \ textbf { P } } _ { k | k } $，後驗估計精差協方差矩陣，度量估計值的精確程度。

卡爾曼濾波器的操作包括兩个階段：'''預測'''佮'''更新'''。佇預測階段，濾波器使用上一狀態的估計，做出對當前狀態的估計。佇更新階段，濾波器利用對當前狀態觀測值優化佇咧預測階段得著的預測值，閣較著一个閣較精確的新估計值。

===預測===


: $ { \ hat { \ textbf { x } } } _ { k | k 影一 }={ \ textbf { F } } _ { k } { \ hat { \ textbf { x } } } _ { k 影一 | k 影一 } + { \ textbf { B } } _ { k } { \ textbf { u } } _ { k } $（預測狀態）


: $ { \ textbf { P } } _ { k | k 影一 }={ \ textbf { F } } _ { k } { \ textbf { P } } _ { k 影一 | k 影一 } { \ textbf { F } } _ { k } ^ { T } + { \ textbf { Q } } _ { k } $（預測估計協方差矩陣）

會當參考：http : / / www . cs . unc . edu / ~ welch / media / pdf / kalman \ _ intro . pdf

會當參考：http : / / web . mit . edu / kirtley / kirtley / binlustuff / literature / control / Kalman % 二十 filter . pdf

===更新===

首先欲算出以下三个量：


: $ { \ tilde { \ textbf { y } } } _ { k }={ \ textbf { z } } _ { k }-{ \ textbf { H } } _ { k } { \ hat { \ textbf { x } } } _ { k | k 影一 } $（測量殘差）


: $ { \ textbf { S } } _ { k }={ \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } + { \ textbf { R } } _ { k } $（測量殘差協方差）


: $ { \ textbf { K } } _ { k }={ \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } { \ textbf { S } } _ { k } ^ { 影一 } $（上優卡爾曼增益）

紲落用𪜶來更新濾波器變量'''x'''佮'''P'''：


: $ { \ hat { \ textbf { x } } } _ { k | k }={ \ hat { \ textbf { x } } } _ { k | k 影一 } + { \ textbf { K } } _ { k } { \ tilde { \ textbf { y } } } _ { k } $（更新的狀態估計）


: $ { \ textbf { P } } _ { k | k }=( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) { \ textbf { P } } _ { k | k 影一 } $（更新的協方差估計）

使用的講公式計算 $ { \ textbf { P } } _ { k | k } $ 干焦佇上優卡爾曼增益的時陣有效。使用其他增益的話，公式愛複雜一寡，請參見推導。

===不變量（Invariant）===

若模型準確實，而且 $ { \ hat { \ textbf { x } } } _ { 零 | 零 } $ 佮 $ { \ textbf { P } } _ { 零 | 零 } $ 的值準確實的反映矣上蓋起初的狀態的分布，以下遮袂變量就保持袂變：所有估計的精差攏值做零

* $ { \ textrm { E } } [{ \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k }]={ \ textrm { E } } [{ \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k 影一 }]=零 $
* $ { \ textrm { E } } [{ \ tilde { \ textbf { y } } } _ { k }]=零 $

而且協方差矩陣準確的反映估計的協方差：

* $ { \ textbf { P } } _ { k | k }={ \ textrm { cov } } ( { \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k } ) $
* $ { \ textbf { P } } _ { k | k 影一 }={ \ textrm { cov } } ( { \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k 影一 } ) $
* $ { \ 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 }] $。

==實例==

考慮在無摩擦的、無限長的直軌道頂懸的一台車。該車上早停佇位零處，時不時咧受隨機的衝擊。每隔 _ Δt _ 隨測量車的位置，但是這个測量是非精確的啦；想欲建立一个關於著其位置佮速度的模型。來看如何推導出這个模型以及如何對這个模型得著卡爾曼濾波器。

因為車頂攏無動力，所以會使失覺察掉'''B'''k 和'''u'''k。因為'''F'''、'''H'''、'''R'''和'''Q'''是常數，所以時間落標會使去掉。

車的位置佮速度（抑是閣較一般的，一粒仔的運動狀態）會當予線性狀態空間對這欲按怎：


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

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

準講佇（_ k _ −  一）時刻佮 _ k _ 時刻之間，車受著 _ a _ k 的加速度，其符合均值做零，標準差為 _ σ _ a 的常態分布。根據牛頓運動定律，會當推出講


: $ { \ textbf { x } } _ { k }={ \ textbf { F } } { \ textbf { x } } _ { k 影一 } + { \ textbf { G } } a _ { k } $

其中


: $ { \ textbf { F } }={ \ begin { bmatrix } 一 & \ Delta t \ \ 零 & 一 \ end { bmatrix } } $

而且


: $ { \ textbf { G } }={ \ begin { bmatrix } { \ frac { \ Delta t ^ { 二 } } { 二 } } \ \ \ 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 ^ { 二 }] { \ textbf { G } } ^ { T }={ \ textbf { G } } [\ sigma _ { a } ^ { 二 }] { \ textbf { G } } ^ { T }=\ sigma _ { a } ^ { 二 } { \ textbf { G } } { \ textbf { G } } ^ { T } $（因為乎 _ σ _ a 是一个純量）。

佇每一時刻，著其位置來進行測量，測量受著噪聲干擾。假使噪聲服對常態分布，均值為零，標準差為 _ σ _ z。


: $ { \ textbf { z } } _ { k }={ \ textbf { Hx } } _ { k } + { \ textbf { v } } _ { k } $

其中


: $ { \ textbf { H } }={ \ begin { bmatrix } 一 & 零 \ end { bmatrix } } $

而且


: $ { \ textbf { R } }={ \ textrm { E } } [{ \ textbf { v } } _ { k } { \ textbf { v } } _ { k } ^ { T }]={ \ begin { bmatrix } \ sigma _ { z } ^ { 二 } \ end { bmatrix } } $

若知影有夠精確的車上早的位置，咱嘛會使初初化


: $ { \ hat { \ textbf { x } } } _ { 零 | 零 }={ \ begin { bmatrix } 零 \ \ 零 \ end { bmatrix } } $

並且，若予濾波器知影確切初位置，會當予出一个協方差矩陣：


: $ { \ textbf { P } } _ { 零 | 零 }={ \ begin { bmatrix } 零 & 零 \ \ 零 & 零 \ end { bmatrix } } $

若無確切的知影上頭仔的位置和速度，遐爾仔協方差矩陣會當初初開始化做一个對角線的元素是 _ B _ 矩陣，_ B _ 取一个合的較大的數。


: $ { \ textbf { P } } _ { 零 | 零 }={ \ begin { bmatrix } B & 零 \ \ 零 & 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 影一 } + { \ 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 影一 } + { \ textbf { K } } _ { k } ( { \ textbf { z } } _ { k }-{ \ textbf { H } } _ { k } { \ hat { \ textbf { x } } } _ { k | k 影一 } ) ) ) $

佮 $ { \ textbf { z } } _ { k } $


: $ { \ textbf { P } } _ { k | k }={ \ textrm { cov } } ( { \ textbf { x } } _ { k }-( { \ hat { \ textbf { x } } } _ { k | k 影一 } + { \ textbf { K } } _ { k } ( { \ textbf { H } } _ { k } { \ textbf { x } } _ { k } + { \ textbf { v } } _ { k }-{ \ textbf { H } } _ { k } { \ hat { \ textbf { x } } } _ { k | k 影一 } ) ) ) $

整理精差向量，得


: $ { \ textbf { P } } _ { k | k }={ \ textrm { cov } } ( ( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) ( { \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k 影一 } )-{ \ textbf { K } } _ { k } { \ textbf { v } } _ { k } ) $

因為測量是誤差'''v'''k 佮其他的項是非相關的，所以有


: $ \ mathbf { P } _ { k | k }={ \ textrm { cov } } ( ( I-\ mathbf { K } _ { k } \ mathbf { H } _ { k } ) ( \ mathbf { x } _ { k }-{ \ hat { \ mathbf { x } } } _ { k \ mid k 影一 } ) ) + { \ textrm { cov } } ( \ mathbf { K } _ { k } \ mathbf { v } _ { k } ) $

利用協方差矩陣的性質，此式會當寫作


: $ { \ textbf { P } } _ { k | k }=( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) { \ textrm { cov } } ( { \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k 影一 } ) ( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) ^ { T } + { \ textbf { K } } _ { k } { \ textrm { cov } } ( { \ textbf { v } } _ { k } ) { \ textbf { K } } _ { k } ^ { T } $

使用不變量'''P'''k | _ k _ 鋪排咧'''R'''k 的定義這項會當寫作
：

$ $
{ \ textbf { P } } _ { k | k }=( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) { \ textbf { P } } _ { k | k 影一 } ( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) ^ { T } + { \ textbf { K } } _ { k } { \ textbf { R } } _ { k } { \ textbf { K } } _ { k } ^ { T }
$ $

這一公式對任何卡爾曼增益'''K'''k 攏成立。若是'''K'''k 是最優卡爾曼增益，會當進一步簡省，請見下文。

===上優卡爾曼增益的推導===

卡爾曼濾波器是一个上小均方精差估計器，後驗狀態精差估計（英文：_ a posteriori _ state estimate）是


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

上小化這个矢量幅度平方的期望值，$ { \ textrm { E } } [| { \ textbf { x } } _ { k }-{ \ hat { \ textbf { x } } } _ { k | k } | ^ { 二 }] $，這等仝款佇最小化後驗估計協方差矩陣'''P'''k | _ k _ 的跡（trace）。 共頂懸行程內底的項做展開、抵消，得著：


:

當矩陣導數是零的時陣得著'''P'''k | _ k _ 的跡（trace）的上小值：


: $ { \ frac { d \ ; { \ textrm { tr } } ( { \ textbf { P } } _ { k | k } ) } { d \ ; { \ textbf { K } } _ { k } } }=鋪二 ( { \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 } ) ^ { T } + 二 { \ textbf { K } } _ { k } { \ textbf { S } } _ { k }=零 $

這个所在愛用著一个常用的式，如下：

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

對這个方面解出卡爾曼增益'''K'''k：


: $ { \ textbf { K } } _ { k } { \ textbf { S } } _ { k }=( { \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 } ) ^ { T }={ \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } $


: $ { \ textbf { K } } _ { k }={ \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } { \ textbf { S } } _ { k } ^ { 影一 } $

這个增益稱做 _ 上優卡爾曼增益 _，咧使用的時陣得著上小均方精差。

===後驗誤差協方差公式化的簡===

佇咧卡爾曼增益等於頂頭導出的上優值的時陣，計算後驗協方差的公式會當進行簡化。佇卡爾曼增益公式兩爿的正爿攏乘以'''S'''k'''K'''kT 得著


: $ { \ textbf { K } } _ { k } { \ textbf { S } } _ { k } { \ textbf { K } } _ { k } ^ { T }={ \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } { \ textbf { K } } _ { k } ^ { T } $

根據頂頭後驗誤差協方差展開公式，


: $ { \ textbf { P } } _ { k | k }={ \ textbf { P } } _ { k | k 影一 }-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 }-{ \ textbf { P } } _ { k | k 影一 } { \ 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 影一 }-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 }=( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) { \ textbf { P } } _ { k | k 影一 } $ .

這个公式的計算較簡單，所以實際中總是使用這个公式，但是需要注意這公式干焦佇咧使用上優卡爾曼增益的時陣伊才成立。若是算術精度總是真低毋過致使數值穩定性出現問題，抑是刁工使用非上優卡爾曼增益，遐爾仔就袂使得用這个簡省；著愛使用頂懸導出的後驗誤差協方差公式。

==佮遞歸貝葉斯估計之間的關係==

準講真正的狀態是無法度觀察的馬爾會當夫過程，測量結果是對隱性馬爾可夫模型觀察著的狀態。


:

根據馬爾可夫假使，真正的狀態干焦受著最近一个狀態影響而佮其他較早狀態無關係。


: $ p ( { \ textbf { x } } _ { k } | { \ textbf { x } } _ { 零 } , \ dots , { \ textbf { x } } _ { k 影一 } )=p ( { \ textbf { x } } _ { k } | { \ textbf { x } } _ { k 影一 } ) $

佮這个類似，在時刻 _ k _ 測量干焦佮當前狀態有關係。


: $ p ( { \ textbf { z } } _ { k } | { \ textbf { x } } _ { 零 } , \ dots , { \ textbf { x } } _ { k } )=p ( { \ textbf { z } } _ { k } | { \ textbf { x } } _ { k } ) $

根據遮的假設，隱性馬爾可夫模型所有的狀態的概率分佈會當較簡化為：


: $ p ( { \ textbf { x } } _ { 零 } , \ dots , { \ textbf { x } } _ { k } , { \ textbf { z } } _ { 一 } , \ dots , { \ textbf { z } } _ { k } )=p ( { \ textbf { x } } _ { 零 } ) \ prod _ { i=一 } ^ { k } p ( { \ textbf { z } } _ { i } | { \ textbf { x } } _ { i } ) p ( { \ textbf { x } } _ { i } | { \ textbf { x } } _ { i 影一 } ) $

毋過，當卡爾曼濾波器用來估計狀態 _'''x'''_ 時，興趣的機率分布，是因為這馬目前為止所有測量值來得著的當前狀態之機率分布


: $ p ( { \ textbf { x } } _ { k } | { \ textbf { Z } } _ { k 影一 } )=\ int p ( { \ textbf { x } } _ { k } | { \ textbf { x } } _ { k 影一 } ) p ( { \ textbf { x } } _ { k 影一 } | { \ textbf { Z } } _ { k 影一 } ) \ , d { \ textbf { x } } _ { k 影一 } $

==信息濾波器==

佇信息濾波器抑是逆共變異數濾波器內底，估計共變異數佮估計狀態分別由信息矩陣佮信息向量代替。遮的定義做：


: $ { \ begin { aligned } \ mathbf { Y } _ { k \ mid k } &=\ mathbf { P } _ { k \ mid k } ^ { 影一 } \ \ { \ hat { \ mathbf { y } } } _ { k \ mid k } &=\ mathbf { P } _ { k \ mid k } ^ { 影一 } { \ hat { \ mathbf { x } } } _ { k \ mid k } \ end { aligned } } $

仝款，預測的共變異數佮狀態有等效的信息形式，定義做：


: $ { \ begin { aligned } \ mathbf { Y } _ { k \ mid k 影一 } &=\ mathbf { P } _ { k \ mid k 影一 } ^ { 影一 } \ \ { \ hat { \ mathbf { y } } } _ { k \ mid k 影一 } &=\ mathbf { P } _ { k \ mid k 影一 } ^ { 影一 } { \ hat { \ mathbf { x } } } _ { k \ mid k 影一 } \ end { aligned } } $

佮測量共變異數佮測量向量，𪜶定義共：


: $ { \ begin { aligned } \ mathbf { I } _ { k } &=\ mathbf { H } _ { k } ^ { \ textsf { T } } \ mathbf { R } _ { k } ^ { 影一 } \ mathbf { H } _ { k } \ \ \ mathbf { i } _ { k } &=\ mathbf { H } _ { k } ^ { \ textsf { T } } \ mathbf { R } _ { k } ^ { 影一 } \ mathbf { z } _ { k } \ end { aligned } } $

新聞更新這馬變甲微微仔道囉。


: $ { \ begin { aligned } \ mathbf { Y } _ { k \ mid k } &=\ mathbf { Y } _ { k \ mid k 影一 } + \ mathbf { I } _ { k } \ \ { \ hat { \ mathbf { y } } } _ { k \ mid k } &={ \ hat { \ mathbf { y } } } _ { k \ mid k 影一 } + \ mathbf { i } _ { k } \ end { aligned } } $

信息過濾器的主要優點是，只需要共測量信息矩陣佮向量相加就會當佇每一个時間行長過濾 N 彼个測量值。


: $ { \ begin { aligned } \ mathbf { Y } _ { k \ mid k } &=\ mathbf { Y } _ { k \ mid k 影一 } + \ sum _ { j=一 } ^ { N } \ mathbf { I } _ { k , j } \ \ { \ hat { \ mathbf { y } } } _ { k \ mid k } &={ \ hat { \ mathbf { y } } } _ { k \ mid k 影一 } + \ sum _ { j=一 } ^ { N } \ mathbf { i } _ { k , j } \ end { aligned } } $

為著預測信息過濾器，會當共信息矩陣佮向量轉換轉去𪜶的狀態空間遮的效項，抑是會當使用信息空間預測。


: $ { \ begin { aligned } \ mathbf { M } _ { k } &=\ left [\ mathbf { F } _ { k } ^ { 影一 } \ right] ^ { \ textsf { T } } \ mathbf { Y } _ { k 影一 \ mid k 影一 } \ mathbf { F } _ { k } ^ { 影一 } \ \ \ mathbf { C } _ { k } &=\ mathbf { M } _ { k } \ left [\ mathbf { M } _ { k } + \ mathbf { Q } _ { k } ^ { 影一 } \ right] ^ { 影一 } \ \ \ mathbf { L } _ { k } &=\ mathbf { I }-\ mathbf { C } _ { k } \ \ \ mathbf { Y } _ { k \ mid k 影一 } &=\ mathbf { L } _ { k } \ mathbf { M } _ { k } \ mathbf { L } _ { k } ^ { \ textsf { T } } + \ mathbf { C } _ { k } \ mathbf { Q } _ { k } ^ { 影一 } \ mathbf { C } _ { k } ^ { \ textsf { T } } \ \ { \ hat { \ mathbf { y } } } _ { k \ mid k 影一 } &=\ mathbf { L } _ { k } \ left [\ mathbf { F } _ { k } ^ { 影一 } \ right] ^ { \ textsf { T } } { \ hat { \ mathbf { y } } } _ { k 影一 \ mid k 影一 } \ end { aligned } } $

若是 F 和 Q 是非時變的，會當共遮的值勻仔存起來，並且 F 和 Q 著愛是可逆的。

==頻率加權卡爾曼濾波器==

佇一九三空年代，Fletcher 和 Munson 進行了有關無仝頻率的聲音感知的開創性研究。𪜶的工課致使了佇咧工業雜訊佮聽力損失調查中加權測得的聲音水平的標準方法。此後，已經佇咧濾波器佮控制器設計當中使用了頻率加權，以管理目標頻段內的性能。

通常，頻率整形函數用佇加權指定頻段中誤差頻譜密度的平均功率。令 $ \ mathbf { y }-{ \ hat { \ mathbf { y } } } $ 表示由傳統的卡爾曼濾波器。仝款，予 $ \ mathbf { W } $ 表示因果頻率加權傳遞函數。上小化 $ \ mathbf { W } \ left ( \ mathbf { y }-{ \ hat { \ mathbf { y } } } \ right ) $ 是通過簡單地構建 $ \ mathbf { W } ^ { 影一 } { \ hat { \ mathbf { y } } } $ 來產生的。

$ \ mathbf { W } $ 的設計猶原是一个懸而未決的問題。一種進行方式是識別產生估計精差的系統，並將 $ \ mathbf { W } $ 設置為等於這个系統的倒去。會當重複執行此過程，以提懸均方誤差為代價，增加濾波器的階數。會當共仝的技術應用佇平滑器。

==非線性濾波器==

基本卡爾曼濾波器（The basic Kalman filter）是限制佇線性的假使之下。毋過，大部份非平凡的（non-trivial）的系統攏是非線性的系統。內底的「非線性質」（non-linearity）可能是伴隨存在過程模型（process model）抑是觀測模型（observation model）中，抑是兩个有去之間。

===擴展卡爾曼濾波器===

佇擴展卡爾曼濾波器（Extended Kalman Filter，簡稱 EKF）中狀態轉換佮觀測模型毋免是狀態的線性函數，會當替換做（可微）函數。


: $ { \ textbf { x } } _ { k }=f ( { \ textbf { x } } _ { k 影一 } , { \ textbf { u } } _ { k } , { \ textbf { w } } _ { k } ) $


: $ { \ textbf { z } } _ { k }=h ( { \ textbf { x } } _ { k } , { \ textbf { v } } _ { k } ) $

函數 _ f _ 會當用來對過去的估計值中計算預測的狀態，相仝的，函數 _ h _ 會當用來預測的狀態計算預測的測量值。毋過 _ f _ 和 _ h _ 袂當直接應用佇協方差當中，取代的是計算偏導矩陣（Jacobian）。

佇每一步內底使用當前的估計狀態計算 Jacobian 矩陣，這幾个矩陣會當用佇卡爾曼濾波器的方程當中。這个過程，實質上將非線性的函數佇咧當前估計值處線性化。

按呢來喔，卡爾曼濾波器的等式為：

'''預測'''


: $ { \ hat { \ textbf { x } } } _ { k | k 影一 }=f ( { \ textbf { x } } _ { k 影一 } , { \ textbf { u } } _ { k } , 零 ) $


: $ { \ textbf { P } } _ { k | k 影一 }={ \ textbf { F } } _ { k } { \ textbf { P } } _ { k 影一 | k 影一 } { \ textbf { F } } _ { k } ^ { T } + { \ textbf { Q } } _ { k } $

'''使用 Jacobians 矩陣更新形體'''


: $ { \ textbf { F } } _ { k }=\ left . { \ frac { \ partial f } { \ partial { \ textbf { x } } } } \ right \ vert _ { { \ hat { \ textbf { x } } } _ { k 影一 | k 影一 } , { \ textbf { u } } _ { k } } $


: $ { \ textbf { H } } _ { k }=\ left . { \ frac { \ partial h } { \ partial { \ textbf { x } } } } \ right \ vert _ { { \ hat { \ textbf { x } } } _ { k | k 影一 } } $

'''更新'''


: $ { \ tilde { \ textbf { y } } } _ { k }={ \ textbf { z } } _ { k }-h ( { \ hat { \ textbf { x } } } _ { k | k 影一 } , 零 ) $


: $ { \ textbf { S } } _{ k }={ \ textbf { H } } _ { k } { \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } + { \ textbf { R } } _ { k } $


: $ { \ textbf { K } } _ { k }={ \ textbf { P } } _ { k | k 影一 } { \ textbf { H } } _ { k } ^ { T } { \ textbf { S } } _ { k } ^ { 影一 } $


: $ { \ hat { \ textbf { x } } } _ { k | k }={ \ hat { \ textbf { x } } } _ { k | k 影一 } + { \ textbf { K } } _ { k } { \ tilde { \ textbf { y } } } _ { k } $


: $ { \ textbf { P } } _ { k | k }=( I-{ \ textbf { K } } _ { k } { \ textbf { H } } _ { k } ) { \ textbf { P } } _ { k | k 影一 } $

'''預測'''

親像擴展卡爾曼濾波器（EKF）仝款，UKF 的預測過程會當獨立於 UKF 的換新過程以外，佮一个線性的（抑是確實是擴展卡爾曼濾波器的）更新過程合併來使用；抑是講，UKF 的預測過程佮更新過程咧喝中國地位相換亦可。

==卡爾曼-布西濾波器==

卡爾曼-布西濾波器（Kalman-Bucy filter，以 Richard Snowden Bucy 號名）是 Kalman 濾波器的連紲時間版本。

伊因為狀態空間模型


: $ { \ begin { aligned } { \ frac { d } { dt } } \ mathbf { x } ( t ) &=\ mathbf { F } ( t ) \ mathbf { x } ( t ) + \ mathbf { B } ( t ) \ mathbf { u } ( t ) + \ mathbf { w } ( t ) \ \ \ mathbf { z } ( t ) &=\ mathbf { H } ( t ) \ mathbf { x } ( t ) + \ mathbf { v } ( t ) \ end { aligned } } $

其中 $ \ mathbf { Q } ( t ) $ 和 $ \ mathbf { R } ( t ) $ 分別代表兩个白噪音聲 $ \ mathbf { w } ( t ) $ 和 $ \ mathbf { v } ( t ) $ 的強度（抑是閣較準確來講：功率譜密度-PSD-矩陣）。

該濾波器由兩个微分方程組成，用一个狀態估計，一个用於共變異數：


: $ { \ begin { aligned } { \ frac { d } { dt } } { \ hat { \ mathbf { x } } } ( t ) &=\ mathbf { F } ( t ) { \ hat { \ mathbf { x } } } ( t ) + \ mathbf { B } ( t ) \ mathbf { u } ( t ) + \ mathbf { K } ( t ) \ left ( \ mathbf { z } ( t )-\ mathbf { H } ( t ) { \ hat { \ mathbf { x } } } ( t ) \ right ) \ \ { \ frac { d } { dt } } \ mathbf { P } ( t ) &=\ mathbf { F } ( t ) \ mathbf { P } ( t ) + \ mathbf { P } ( t ) \ mathbf { F } ^ { \ textsf { T } } ( t ) + \ mathbf { Q } ( t )-\ mathbf { K } ( t ) \ mathbf { R } ( t ) \ mathbf { K } ^ { \ textsf { T } } ( t ) \ end { aligned } } $

卡爾曼增益由


: $ \ mathbf { K } ( t )=\ mathbf { P } ( t ) \ mathbf { H } ^ { \ textsf { T } } ( t ) \ mathbf { R } ^ { 影一 } ( t ) $

注意，遮表達式當中，對於 $ \ mathbf { K } ( t ) $，觀察雜訊 $ \ mathbf { R } ( t ) $ 的共變異數同時表示預測誤差（抑是創新）$ { \ tilde { \ mathbf { y } } } ( t )=\ mathbf { z } ( t )-\ mathbf { H } ( t ) { \ hat { \ mathbf { x } } } ( t ) $ 的共變異數；遮的共變異數干焦連紲時間的情況下才相等。

離散的時間卡爾曼濾波的預測步驟佮更新勢之間的區別佇咧連紲時間內不存在。

用佇咧共變異數的第二个微分方程是 Riccati 方程的一个示例。

==混合型卡爾曼濾波器==

大多數物理系統表示做連紲時間模型，離散時間測量就會當定通過數字處理器來進行狀態估計。所以，系統模型佮測量模型由下式予出：


: $ { \ begin { aligned } { \ dot { \ mathbf { x } } } ( t ) &=\ mathbf { F } ( t ) \ mathbf { x } ( t ) + \ mathbf { B } ( t ) \ mathbf { u } ( t ) + \ mathbf { w } ( t ) , & \ mathbf { w } ( t ) & \ sim N \ left ( \ mathbf { 零 } , \ mathbf { Q } ( t ) \ right ) \ \ \ mathbf { z } _ { k } &=\ mathbf { H } _ { k } \ mathbf { x } _ { k } + \ mathbf { v } _ { k } , & \ mathbf { v } _ { k } & \ sim N ( \ mathbf { 零 } , \ mathbf { R } _ { k } ) \ end { aligned } } $

當


: $ \ mathbf { x } _ { k }=\ mathbf { x } ( t _ { k } ) $ .

===初初值===


: $ { \ hat { \ mathbf { x } } } _ { 零 \ mid 零 }=E \ left [\ mathbf { x } ( t _ { 零 } ) \ right] , \ mathbf { P } _ { 零 \ mid 零 }=\ operatorname { Var } \ left [\ mathbf { x } \ left ( t _ { 零 } \ right ) \ right] $

===預測===


: $ { \ begin { aligned } { \ dot { \ hat { \ mathbf { x } } } } ( t ) &=\ mathbf { F } ( t ) { \ hat { \ mathbf { x } } } ( t ) + \ mathbf { B } ( t ) \ mathbf { u } ( t ) { \ text { , with } } { \ hat { \ mathbf { x } } } \ left ( t _ { k 影一 } \ right )={ \ hat { \ mathbf { x } } } _ { k 影一 \ mid k 影一 } \ \ \ Rightarrow { \ hat { \ mathbf { x } } } _ { k \ mid k 影一 } &={ \ hat { \ mathbf { x } } } \ left ( t _ { k } \ right ) \ \ { \ dot { \ mathbf { P } } } ( t ) &=\ mathbf { F } ( t ) \ mathbf { P } ( t ) + \ mathbf { P } ( t ) \ mathbf { F } ( t ) ^ { \ textsf { T } } + \ mathbf { Q } ( t ) { \ text { , with } } \ mathbf { P } \ left ( t _ { k 影一 } \ right )=\ mathbf { P } _ { k 影一 \ mid k 影一 } \ \ \ Rightarrow \ mathbf { P } _ { k \ mid k 影一 } &=\ mathbf { P } \ left ( t _ { k } \ right ) \ end { aligned } } $

預測方程式是對連紲的時間卡爾曼濾波器的方程式推導得著，無需要進行測量來更新，比如講 : $ \ mathbf { K } ( t )=零 $。預測狀態佮共變異數分別通過求解一組初初二值等於上一步估計值的微分方程組來計算。

線性非常時變系統的情況下，會當使用矩陣指數會連紲時間動態精確地離散化為離散時間系統。

===更新===


: $ { \ begin { aligned } \ mathbf { K } _ { k } &=\ mathbf { P } _ { k \ mid k 影一 } \ mathbf { H } _ { k } ^ { \ textsf { T } } \ left ( \ mathbf { H } _ { k } \ mathbf { P } _ { k \ mid k 影一 } \ mathbf { H } _ { k } ^ { \ textsf { T } } + \ mathbf { R } _ { k } \ right ) ^ { 影一 } \ \ { \ hat { \ mathbf { x } } } _ { k \ mid k } &={ \ hat { \ mathbf { x } } } _ { k \ mid k 影一 } + \ mathbf { K } _ { k } \ left ( \ mathbf { z } _ { k }-\ mathbf { H } _ { k } { \ hat { \ mathbf { x } } } _ { k \ mid k 影一 } \ right ) \ \ \ mathbf { P } _ { k \ mid k } &=\ left ( \ mathbf { I }-\ mathbf { K } _ { k } \ mathbf { H } _ { k } \ right ) \ mathbf { P } _ { k \ mid k 影一 } \ end { aligned } } $

更新方程佮離散的時間卡爾曼濾波器的更新方程仝款。

==應用==

* 自動駕駛儀
* 動態定位系統
* 經濟學，特別是宏觀經濟學，時間序列模型，佮計量經濟學
* 慣性導航系統
* 雷達佮蹤器
* 衛星導航系統

==參見==

* 即時定位佮地圖構建
* 快速卡爾曼濾波
* 比較：維納濾波佮粒仔濾波器
* 施密特–卡爾曼濾波器
* 濾波問題
* 滾動的時陣估計
* 無影變換

==外部連結==

* An Introduction to the Kalman Filter , SIGGRAPH 兩千空一 Course , Greg Welch and Gary Bishop
* Kalman filtering chapter from _ Stochastic Models , Estimation _ , by Peter Maybeck
* Kalman Filter webpage , with lots of links
* Kalman Filtering
* The unscented Kalman filter for nonlinear estimation
* _ Kalman Filters _ , thorough introduction to several types , together with applications to _ Robot Localization _

==參考文獻==

* Gelb A . , editor . Applied optimal estimation . MIT Press , 一千九百七十四 .
* Kalman , R . E . A New Approach to Linear Filtering and Prediction Problems , _ Transactions of the ASME-Journal of Basic Engineering _ Vol . 八十二 : pp . 三十五孵四十五 ( 一千九百六十 )
* Kalman , R . E . , Bucy R . S . , New Results in Linear Filtering and Prediction Theory , _ Transactions of the ASME-Journal of Basic Engineering _ Vol . 八十三 : pp . 九十五孵一百空七 ( 一千九百六十一 )
* [JU 九十七喔] Julier , Simon J . and Jeffery K . Uhlmann . A New Extension of the Kalman Filter to nonlinear Systems . In The Proceedings of AeroSense : The 十一 th International Symposium on Aerospace / Defense Sensing , Simulation and Controls , Multi Sensor Fusion , Tracking and Resource Management II , SPIE , 一千九百九十七 .
* Harvey , A . C . Forecasting , Structural Time Series Models and the Kalman Filter . Cambridge University Press , Cambridge , 一千九百八十九 .

[[分類: 待校正]]