卡爾曼濾波的原理和實現

卡爾曼濾波是一種遞迴的估計，即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值。它是一種純粹的時域濾波器。卡爾曼濾波在技術領域有許多的應用，比如飛行導航控制，機械人運動規劃等控制領域。

卡爾曼濾波適用於如下系統控制模型：

x(k) = ax(k-1) + bu(k-1) + w(k-1);

z(k) = hx(k) + v(k);

其中a是作用在x(k−1)上的狀態變換模型（矩陣/向量）。

b是作用在控制器向量u(k)上的輸入－控制模型。

w(k)是過程雜訊，並假定其符合均值為零，協方差矩陣為q(k)的多元正態分佈。

h是觀測模型，它把真實狀態空間對映成觀測空間。

v(k)是觀測雜訊，其均值為零，協方差矩陣為r(k),且服從正態分佈。

對於不滿足如上模型的控制系統，可以使用其他的卡爾曼濾波器的變種，下面我們只討論基本的卡爾曼濾波器。

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

**階段的方程如下：

x(k|k-1) = ax(k-1|k-1) + bu(k-1)；（**狀態）

p(k|k-1) = ap(k-1|k-1) a』 + q；（**估計協方差矩陣）

校正階段的方程如下：

kg(k)= p(k|k-1) h』 / (hp(k|k-1) h』 + r) (卡爾曼增益)

x(k|k)= x(k|k-1)+kg(k) (z(k)-hx(k|k-1)) （校正後的狀態估計）

p(k|k)= (i-kg(k)h)p(k|k-1) （更新後的協方差估計）

通過上述5個方程就能實現乙個卡爾曼濾波器。下面我們來看下如何具體實現乙個卡爾曼濾波器，需要求解哪些量。

上面5個方程中a、b和h都是乙個固定的量，在乙個控制系統中，這3個都是確定已知的常量。

r是測量雜訊協方差，它是乙個數值，這是和測量相關的乙個特性，作為已知條件輸入濾波器，我們通常通過實驗確定這個值。

q 表示過程激勵雜訊的協方差，它是狀態轉移矩陣與實際過程之間的誤差。這個矩陣是卡爾曼濾波中比較難確定的乙個量，但是在某些穩定的過程可以假定它是固定的矩陣，通過尋找最優的q值使濾波器獲得更好的效能。