卡爾曼濾波

卡爾曼濾波器會追蹤系統的估計狀態，以及估計的變異量或是不確定性。會透過狀狀態轉換模型以及其量測來更新估計值。 是指時間k時的估計，還沒有考慮第k次量測yk的資訊，為對應的不確定性
卡爾曼濾波（Kalman filter）是一種高效率的遞歸濾波器（自迴歸濾波器），它能夠從一系列的不完全及包含雜訊的測量中，估計動態系統的狀態。卡爾曼濾波會根據各測量量在不同時間下的值，考慮各時間下的聯合分佈，再產生對未知變數的估計，因此會比只以單一測量量為基礎的估計方式要準。卡爾曼濾波得名自主要貢獻者之一的魯道夫·卡爾曼。
卡爾曼濾波在技術領域有許多的應用。常見的有飛機及太空船的guidance, navigation, and control。卡爾曼濾波也廣為使用在時間序列的分析中，例如信號處理及計量經濟學中。卡爾曼濾波也是機器人運動規劃及控制的重要主題之一，有時也包括在trajectory optimization。卡爾曼濾波也用在中軸神經系統運動控制的建模中。因為從給與運動命令到收到感覺神經的回授之間有時間差，使用卡爾曼濾波有助於建立符合實際的系統，估計運動系統的目前狀態，並且更新命令。
卡爾曼濾波的演算法是二步驟的程序。在估計步驟中，卡爾曼濾波會產生有關目前狀態的估計，其中也包括不確定性。只要觀察到下一個量測（其中一定含有某種程度的誤差，包括隨機雜訊）。會通過加權平均來更新估計值，而確定性越高的量測加權比重也越高。演算法是迭代的，可以在實時控制系統中執行，只需要目前的輸入量測、以往的計算值以及其不確定性矩陣，不需要其他以往的資訊。
使用卡爾曼濾波不用假設誤差是正態分佈，不過若所有的誤差都是正態分佈，卡爾曼濾波可以得到正確的條件機率估計。
也發展了一些擴展或是廣義的卡爾曼濾波，例如運作在非線性系統的extended Kalman filter及無跡卡爾曼濾波（unscented Kalman filter）。底層的模型類似隱馬爾可夫模型，不過潛在變量的狀態空間是連續的，而且所有潛在變量及可觀測變數都是正態分佈。

**應用實例**
卡爾曼濾波的一個典型實例是從一組有限的，包含噪聲的，通過對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。在很多工程應用（如雷達、機器視覺）中都可以找到它的身影。同時，卡爾曼濾波也是控制理論以及控制系統工程中的一個重要課題。
例如，對於雷達來說，人們感興趣的是其能夠跟蹤目標。但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息，設法去掉噪聲的影響，得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計（濾波），也可以是對於將來位置的估計（預測），也可以是對過去位置的估計（插值或平滑）。

**命名**
這種濾波方法以它的發明者魯道夫·卡爾曼（Rudolph Kalman）命名，但是根據文獻可知實際上Peter Swerling在更早之前就提出了一種類似的算法。
（Stanley Schmidt）首次實現了卡爾曼濾波器。卡爾曼在NASA艾姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。關於這種濾波器的論文由Swerling（1958）、Kalman (1960)與Kalman and Bucy（1961）發表。
目前，卡爾曼濾波已經有很多不同的實現。卡爾曼最初提出的形式現在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton開發的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。
----
以下的討論需要線性代數以及概率論的一般知識。
----

**演算概念**
卡爾曼濾波器使用系統的動態模型（例如，運動的物理定律），該系統的已知控制輸入以及多個順序的測量值（例如來自傳感器的測量值）來形成對系統變化量（其狀態）更好的估計，其精度比僅使用一種測量獲得的估算值高。它是一種常見的感測器融合和Data fusion算法。
感測器數據的雜訊，描述系統演化的方程式的近似值以及未考慮所有因素的外部因素都限制了確定系統狀態的能力。卡爾曼濾波器有效地處理了由於感測器數據雜訊引起的不確定性，並在一定程度上處理了隨機外部因素。卡爾曼濾波器使用加權平均值生成系統狀態的估計值，作為系統預測狀態和新測量值的平均值。權重的目的是估計值具有更好（即較小）的不確定性的值會被更多“信任”。權重是根據共變異數來計算的，共變異數是對系統狀態預測的估計不確定性的度量。加權平均值的結果是介於預測狀態和測量狀態之間的新狀態估計，並且比任何一個狀態都有更好的估計不確定性。在每個時間步重複此過程，新的估計值及其共變異數將通知後續迭代中使用的預測。這意味著卡爾曼濾波器可以遞迴地工作，並且只需要系統狀態的最後“最佳猜測”，而不是整個歷史，就可以計算新狀態。
測量和當前狀態估計的相對確定性是重要的考慮因素，通常根據卡爾曼濾波器的增益來討論濾波器的反應。卡爾曼增益是賦予測量值和當前狀態估計值的相對權重，可以進行“調整”以獲得特定的性能。增益高時，濾波器將更多的精力放在最新的測量上，因此反應速度更快。增益較低時，濾波器會更緊密地遵循模型預測。在極端情況下，接近1的高增益將導致估計的軌跡更加跳躍，而接近零的低增益將消除雜訊，但會降低反應速度。
在執行濾波器的實際計算時（如下所述），狀態估計值和共變異數被編碼到矩陣中，以處理單個計算集中涉及的多個維度。這允許在任何過渡模型或共變異數中表示不同狀態變量（例如位置，速度和加速度）之間的線性關係。

**基本動態系統模型**
卡爾曼濾波建立在線性代數和隱馬爾可夫模型（hidden Markov model）上。其基本動態系統可以用一個馬爾可夫鏈表示，該馬爾可夫鏈建立在一個被高斯噪聲（即正態分佈的噪聲）干擾的線性算子上的。系統的狀態可以用一個元素為實數的向量表示。隨著離散時間的每一個增加，這個線性算子就會作用在當前狀態上，產生一個新的狀態，並也會帶入一些噪聲，同時系統的一些已知的控制器的控制信息也會被加入。同時，另一個受噪聲干擾的線性算子產生出這些隱含狀態的可見輸出。
為了從一系列有噪聲的觀察數據中用卡爾曼濾波器估計出被觀察過程的內部狀態，必須把這個過程在卡爾曼濾波的框架下建立模型。也就是說對於每一步k，定義矩陣Fk, Hk, Qk, Rk，有時也需要定義Bk，如下。
卡爾曼濾波器的模型。圓圈代表向量，方塊代表矩陣，星號代表高斯噪聲，其協方差矩陣在右下方標出。
卡爾曼濾波模型假設k時刻的真實狀態是從（k − 1）時刻的狀態演化而來，符合下式：

其中
*Fk是作用在xk−1上的狀態變換模型（/矩陣/向量）。- Bk是作用在控制器向量uk上的輸入－控制模型。- wk是過程噪聲，並假定其符合均值為零，協方差矩陣為Qk的多元正態分佈。

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

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

初始狀態以及每一時刻的噪聲{x0, w1, ..., wk,  v1 ... vk}都認為是互相獨立的。
實際上，很多真實世界的動態系統都並不確切的符合這個模型；但是由於卡爾曼濾波器被設計在有噪聲的情況下工作，一個近似的符合已經可以使這個濾波器非常有用了。更多其它更復雜的卡爾曼濾波器的變種，在下邊討論中有描述。

**卡爾曼濾波器**
卡爾曼濾波是一種遞歸的估計，即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史信息。卡爾曼濾波器與大多數濾波器不同之處，在於它是一種純粹的時域濾波器，它不需要像低通濾波器等頻域濾波器那樣，需要在頻域設計再轉換到時域實現。
卡爾曼濾波器的狀態由以下兩個變量表示：
*，在時刻k的狀態的估計；
*，後驗估計誤差協方差矩陣，度量估計值的精確程度。
卡爾曼濾波器的操作包括兩個階段：預測與更新。在預測階段，濾波器使用上一狀態的估計，做出對當前狀態的估計。在更新階段，濾波器利用對當前狀態的觀測值優化在預測階段獲得的預測值，以獲得一個更精確的新估計值。

**預測**
（預測狀態）
（預測估計協方差矩陣）
可參考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf date=20210126065856 
可參考：http://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf date=20200923102207 

**更新**
首先要算出以下三個量：
（測量殘差）
（測量殘差協方差）
（最優卡爾曼增益）
然後用它們來更新濾波器變量x與P：
（更新的狀態估計）
（更新的協方差估計）
使用上述公式計算僅在最優卡爾曼增益的時候有效。使用其他增益的話，公式要複雜一些，請參見推導。

**不變量（Invariant）**
如果模型準確，而且與的值準確的反映了最初狀態的分佈，那麼以下不變量就保持不變：所有估計的誤差均值為零
且協方差矩陣準確的反映了估計的協方差：
請注意，其中表示的期望值, 。

**實例**
考慮在無摩擦的、無限長的直軌道上的一輛車。該車最初停在位置0處，但時不時受到隨機的衝擊。每隔Δt秒即測量車的位置，但是這個測量是非精確的；想建立一個關於其位置以及速度的模型。來看如何推導出這個模型以及如何從這個模型得到卡爾曼濾波器。
因為車上無動力，所以可以忽略掉Bk和uk。由於F、H、R和Q是常數，所以時間下標可以去掉。
車的位置以及速度（或者更加一般的，一個粒子的運動狀態）可以被線性狀態空間描述如下：

其中是速度，也就是位置對於時間的導數。
假設在（k − 1）時刻與k時刻之間，車受到ak的加速度，其符合均值為0，標準差為σa的正態分佈。根據牛頓運動定律，可以推出

其中

且

可以發現
（因為σa是一個標量）。
在每一時刻，對其位置進行測量，測量受到噪聲干擾。假設噪聲服從正態分佈，均值為0，標準差為σz。

其中

且

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

並且，若讓濾波器知道確切的初始位置，可給出一個協方差矩陣：

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

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

**推導**

**推導後驗協方差矩陣**
按照上邊的定義，從誤差協方差開始推導如下：

代入

再代入  

與

整理誤差向量，得

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

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

使用不變量Pk|k-1以及Rk的定義這一項可以寫作
：
這一公式對於任何卡爾曼增益Kk都成立。如果Kk是最優卡爾曼增益，則可以進一步簡化，請見下文。

**最優卡爾曼增益的推導**
卡爾曼濾波器是一個最小均方誤差估計器，後驗狀態誤差估計（英文：a posteriori state estimate）是

最小化這個矢量幅度平方的期望值，，這等同於最小化後驗估計協方差矩陣Pk|k的跡（trace）。將上面方程中的項展開、抵消，得到：
當矩陣導數是0的時候得到Pk|k的跡（trace）的最小值：

此處須用到一個常用的式子，如下：

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


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

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

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

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

**與遞歸貝葉斯估計之間的關係**
假設真正的狀態是無法觀察的馬爾可夫過程，測量結果是從隱性馬爾可夫模型觀察到的狀態。
Hidden Markov Model
根據馬爾可夫假設，真正的狀態僅受最近一個狀態影響而與其它以前狀態無關。

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

根據這些假設，隱性馬爾可夫模型所有狀態的概率分佈可以簡化為：

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


**信息濾波器**
在信息濾波器或逆共變異數濾波器中，估計的共變異數和估計狀態分別由信息矩陣和信息向量代替。 這些定義為：

同樣，預測的共變異數和狀態具有等效的信息形式，定義為：

以及測量共變異數和測量向量，它們定義為：

信息更新現在變得微不足道了。

信息過濾器的主要優點是，只需將其測量信息矩陣和向量相加即可在每個時間步長過濾N個測量值。

為了預測信息過濾器，可以將信息矩陣和向量轉換回它們的狀態空間等效項，或者可以使用信息空間預測。

如果F和Q是非時變的，則可以將這些值緩存起來，並且F和Q必須是可逆的。

**頻率加權卡爾曼濾波器**
在1930年代，Fletcher和Munson進行了有關不同頻率的聲音感知的開創性研究。他們的工作導致了在工業雜訊和聽力損失調查中加權測得的聲音水平的標準方法。此後，已在濾波器和控制器設計中使用了頻率
加權，以管理目標頻段內的性能。
通常，頻率整形函數用於加權指定頻段中誤差頻譜密度的平均功率。 令  表示由 傳統的卡爾曼濾波器。 同樣，讓  表示因果頻率加權傳遞函數。 最小化  是通過簡單地構建  而產生的。
的設計仍然是一個懸而未決的問題。 一種進行方式是識別產生估計誤差的系統，並將  設置為等於該系統的倒數。 可以重複執行此過程，以提高均方誤差為代價，增加濾波器階數。可以將相同的技術應用於平滑器。

**非線性濾波器**

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

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


函數f可以用來從過去的估計值中計算預測的狀態，相似的，函數h可以用來以預測的狀態計算預測的測量值。然而f和h不能直接的應用在協方差中，取而代之的是計算偏導矩陣（Jacobian）。
在每一步中使用當前的估計狀態計算Jacobian矩陣，這幾個矩陣可以用在卡爾曼濾波器的方程中。這個過程，實質上將非線性的函數在當前估計值處線性化了。
這樣一來，卡爾曼濾波器的等式為：
預測


使用Jacobians矩陣更新模型


更新

 



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

**卡爾曼-布西濾波器**
卡爾曼-布西濾波器（Kalman-Bucy filter，以Richard Snowden Bucy命名）是Kalman濾波器的連續時間版本。
它基於狀態空間模型

其中  和  分別代表兩個白噪聲項  和  的強度（或更準確地說：功率譜密度-PSD-矩陣）。
該濾波器由兩個微分方程組成，一個用於狀態估計，一個用於共變異數：

卡爾曼增益由

注意，在此表達式中，對於  ，觀察雜訊  的共變異數同時表示預測誤差（或創新） 的共變異數； 這些共變異數僅在連續時間的情況下才相等。
離散時間卡爾曼濾波的預測步驟和更新步驟之間的區別在連續時間內不存在。
用於共變異數的第二個微分方程是Riccati方程的一個示例。

**混合型卡爾曼濾波器**
大多數物理系統表示為連續時間模型，而離散時間測量則經常通過數字處理器進行狀態估計。 因此，系統模型和測量模型由下式給出：

當
.

**初始值**


**預測**

預測方程式是從連續時間卡爾曼濾波器的方程式推導而得，而無需進行測量更新，例如:  。預測狀態和共變異數分別通過求解一組初始值等於上一步估計值的微分方程組來計算。
在線性非時變系統的情況下，可以使用矩陣指數將連續時間動態精確地離散化為離散時間系統。

**更新**

更新方程與離散時間卡爾曼濾波器的更新方程相同。

**應用**
自動駕駛儀
*Dynamic positioning
*經濟學，特別是宏觀經濟學，時間序列模型，以及計量經濟學
*慣性導航系統
*Radar tracker
*衛星導航系統

**參見**
 即時定位與地圖構建- Fast Kalman filter- 比較：維納濾波及粒子濾波器- 施密特–卡爾曼濾波器- 濾波問題- 滾動時域估計- 無跡變換

**參考文獻**
 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.

**外部鏈接**
  An Introduction to the Kalman Filter date=20210224114008 , SIGGRAPH 2001 Course, Greg Welch and Gary Bishop-  Kalman filtering chapter from Stochastic Models, Estimation, by Peter Maybeck-  Kalman Filterdate=20070408165701  webpage, with lots of links-  Kalman Filtering-  The unscented Kalman filter for nonlinear estimation date=20210225013227 -  Kalman Filters, thorough introduction to several types, together with applications to Robot Localization date=20181010160514