最佳線性濾波理論起源于40年代美國科學家Wiener和前蘇聯(lián)科學家Ｋолмогоров等人的研究工作，后人統(tǒng)稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的數(shù)據(jù)，不適用于實時處理。為了克服這一缺點，60年代Kalman把狀態(tài)空間模型引入濾波理論，并導出了一套遞推估計算法，后人稱之為卡爾曼濾波理論?？柭鼮V波是以最小均方誤差為估計的最佳準則，來尋求一套遞推估計的算法，其基本思想是：采用信號與噪聲的狀態(tài)空間模型，利用前一時刻地估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)時刻的估計值。它適合于實時處理和計算機運算。

現(xiàn)設線性時變系統(tǒng)的離散狀態(tài)防城和觀測方程為：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分別是k時刻的狀態(tài)矢量和觀測矢量

F(k,k-1)為狀態(tài)轉(zhuǎn)移矩陣

U(k)為k時刻動態(tài)噪聲

T(k,k-1)為系統(tǒng)控制矩陣

H(k)為k時刻觀測矩陣

N(k)為k時刻觀測噪聲

則卡爾曼濾波的算法流程為：

1. 預估計X(k)^= F(k,k-1)·X(k-1)?
   
2. 計算預估計協(xié)方差矩陣
   C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
   Q(k) = U(k)×U(k)'?
   
3. 計算卡爾曼增益矩陣
   K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
   R(k) = N(k)×N(k)'?
   
4. 更新估計
   X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]?
   
5. 計算更新后估計協(xié)防差矩陣
   C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'?
   
6. X(k+1) = X(k)~
   C(k+1) = C(k)~
   重復以上步驟