第二章 濾波方法介紹
2.2 卡爾曼濾波演算法之介紹
2.2 卡爾曼濾波演算法之介紹
1960 年，卡爾曼(Rudolph E. Kalman)發表了以遞歸方法解決離散數據線性濾波問 題的論文，故此濾波方法以卡爾曼命名。卡爾曼濾波(Kalman filter；以下簡稱卡氏濾 波器) 由一系列的遞歸數學公式描述，是一種高效率的遞歸濾波器(亦稱自回歸濾波器)
，它能夠從一系列的不完全及包含噪訊的測量(measurement)中，估計動態系統的狀態。
一般而言，量測點的位移、速度、加速度的測量值往往都會有噪訊，卡氏濾波器利用 量測點的動態資訊，從一組包含噪聲的序列，設法除去噪聲的影響，預測其位移及速 度，良好地估計出量測點的狀態。
簡單的說，就是以最小均方誤差為估計的最佳準則，來尋求一套遞迴估算的演算 法，當輸入為高斯白噪聲(Gaussian white noise，簡稱白噪，即常態分佈的隨機噪訊)時，
使得期望的量測值和實際的輸出值之間的均方根誤差達到最小。採用訊號與噪訊的狀 態空間模型，利用前一時刻的估計值和當前時刻的觀測值來更新對狀態變數的估計，
求出當前時刻的估計值。
卡氏濾波器在許多工程應用(如雷達、動態定位系統、GPS、經濟學、時間序列模 型……等)中都有良好的發展亦是控制理論以及控制系統工程中的一個重要課題。
5
2.2.1 卡氏濾波器的基本動態系統模型
卡氏濾波器建立在隱馬爾可夫模型(Hidden Markov Model)上，其模型假設當 t 時刻 的真實狀態是從(t − 1)時刻的狀態演化而來，可用下式表示之：
𝒙𝑡 = 𝑭𝑡𝒙𝑡−1+ 𝑩𝑡𝒖𝒕+ 𝒘𝑡 (2.1) 其中𝑭𝑡為作用在𝒙𝑡−1的狀態變換模型；𝑩𝑡為作用在控制器向量𝒖𝑡的輸入控制模型；𝒘𝑡 為符合均值為零的過程噪聲(process noise)且協方差矩陣為𝑸𝑡的多元常態分佈，𝒘𝑡~ N(0, 𝑸𝑡)。
對於一個真實狀態𝐱𝑡的測量𝒛𝑡，在時刻為 t 時滿足下式：
𝒛𝑡= 𝑯𝑡𝒙𝑡+ 𝒗𝑡 (2.2)
其中𝑯𝑡為觀測模型，本研究在濾波演算過程中將其設為𝑯𝑡= [ 1 0 ]；而 𝒗𝑡 為均值為 零的觀測噪聲(observation noise)，且協方差矩陣為𝑹𝑡並滿足𝒗𝑡~N(0, 𝑹𝑡)。假設𝒘𝑡與𝒗𝑡 皆為互相獨立且正態分佈的白色噪聲；實際的應用中， 𝒘𝑡與𝒗𝑡通常為有色噪聲，兩者 亦可能存在互相關的情況，而𝑸𝑡與𝑹𝑡則可能會隨著疊代計算而產生變化，但在這裡我 們將𝑸𝑡與𝑹𝑡假設為常數[8]。在本研究中，我們假設𝑸𝑡為 0.001；且在此稱 GR 值為「猜 測的噪訊值」，在濾波之前，先假設 GR 值分別為 10%、20%、30%、40%，其中 GR 值即代表式中的 ，濾波過程中我們將 假設為 0.1、0.2、0.3、0.4，欲觀察猜測的噪 訊含量是否與實際上的噪訊含量一致，而此過程對於過濾噪訊與系統識別的結果又將 影響與否。
卡氏濾波器模型請參考圖 2.2，其中圓圈代表向量，方塊代表矩陣，星號代表高斯 噪聲，其協方差矩陣在右下方標示。
2.2.2 卡氏濾波器的推導及運算
在線性的條件之下，卡氏濾波器的狀態可由以下兩個變數表示：
𝒙̂ 𝑡 | 𝑡：當時刻為 t 時的狀態估算，
𝑷 𝑡 | 𝑡：誤差的協方差矩陣(covariance matrix)，用來度量估計值的精確程度。
6
基本型的卡氏濾波器的運算包含預測及更新兩個階段：
(1) 預測：依式(2.1)，利用前一刻狀態的估算，做出對當前時刻狀態的估算
預測狀態： 𝒙̂ 𝑡 | 𝑡−1 = 𝑭𝑡𝒙̂ 𝑡−1 | 𝑡−1+ 𝑩𝑡𝒖𝒕−𝟏 (2.3)
預測估計協方差矩陣： 𝑷 𝑡 | 𝑡−1 = 𝑭𝑡𝑷 𝑡−1 | 𝑡−1𝑭𝑡𝑇+ 𝑸𝑡 (2.4)
(2) 更新：
𝒙̂ 𝑡 |𝑡 = 𝒙̂ 𝑡 | 𝑡−1+ 𝑲𝑡𝒚̃𝑡 (2.5)
𝑷𝑡|𝑡= (𝑰 − 𝑲𝑡𝑯𝑡)𝑷𝑡|𝑡−1 (2.6)
其中，
𝒚̃𝑡 = 𝒛𝑡− 𝑯𝑡𝒙̂𝑡|𝑡−1 (2.7) 𝑲𝑡 = 𝑷𝑡|𝑡−1𝑯𝑡𝑇𝑺𝑡−1 (2.8)
= | −1 + (2.9)
𝒚̃𝑡為測量餘量(measurement residual)；𝑺𝑡為測量餘量的協方差矩陣；𝑲𝑡為卡爾曼增益；
𝒙̂ 𝑡 |𝑡為更新的狀態估算；𝑷𝑡|𝑡為更新的協方差矩陣之估算，只適用於最優卡爾曼增益。
假設模型準確，且𝒙̂0|0和𝑷0|0反映了最初狀態的分佈，那麼可知協方差矩陣亦準確 地反映了估算的協方差，即
𝑷𝑡|𝑡= 𝑐𝑜𝑣(𝒙𝑡− 𝒙̂ 𝑡|𝑡) (2.10) 𝑷𝑡|𝑡−1= 𝑐𝑜𝑣(𝒙𝑡− 𝒙̂ 𝑡|𝑡−1) (2.11)
𝑺𝑡= 𝑐𝑜𝑣(𝒚̃𝑡) (2.12)
其中𝑐𝑜𝑣(𝑎) = [𝑎𝑎𝑇]， [ ]代表 的期望值。
欲推導後驗( posteriori)協方差矩陣，首先將式(2.5)代入式(2.10)，可得：
𝑷𝑡|𝑡= 𝑐𝑜𝑣[𝒙𝑡− (𝒙̂ 𝑡 | 𝑡−1+ 𝑲𝑡𝒚̃𝑡)] (2.13)
7 再將式(2.2)及(2.7)代入式(2.13)，得：
𝑷𝑡|𝑡= 𝑐𝑜𝑣{𝒙𝑡− [𝒙̂ 𝑡 | 𝑡−1+ 𝑲𝑡(𝑯𝑡𝒙𝑡+ 𝒗𝑡− 𝑯𝑡𝒙̂𝑡|𝑡−1)]} (2.14)
整理可得：
𝑷𝑡|𝑡= 𝑐𝑜𝑣[(𝑰 − 𝑲𝑡𝑯𝑡)(𝒙𝑡− 𝒙̂𝑡|𝑡−1) − 𝑲𝑡𝒗𝑡] (2.15) 由於測量誤差𝒗𝑡與其他項不相關，因此(2.13)式可改寫為：
𝑷𝑡|𝑡= 𝑐𝑜𝑣[(𝑰 − 𝑲𝑡𝑯𝑡)(𝒙𝑡− 𝒙̂𝑡|𝑡−1)] + 𝑐𝑜𝑣(𝑲𝑡𝒗𝑡) (2.16) 利用協方差矩陣的性質，式(2.14)可以改寫為：
𝑷𝑡|𝑡= (𝑰 − 𝑲𝑡𝑯𝑡)𝑐𝑜𝑣(𝒙𝑡− 𝒙̂𝑡|𝑡−1)(𝑰 − 𝑲𝑡𝑯𝑡)𝑇+ 𝑲𝑡𝑐𝑜𝑣(𝒗𝑡)𝑲𝑡𝑇 (2.17) 最後，將式(2.11)代入(2.15)，並令𝑹𝑡 = 𝑐𝑜𝑣(𝒗𝑡)，整理可得下式：
𝑷𝑡|𝑡= (𝑰 − 𝑲𝑡𝑯𝑡)𝑷𝑡|𝑡−1(𝑰 − 𝑲𝑡𝑯𝑡)𝑇+ 𝑲𝑡𝑹𝑡𝑲𝑡𝑇 (2.18) 上式對於任一卡爾曼增益𝑲𝑡皆成立。卡氏濾波器的分析流程圖請參考圖 2.3。