卡爾曼濾波程序

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

2、時刻地估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變量的估計，求出現(xiàn)時刻的估計值。它適合于實時處理和計算機運算。現(xiàn)設(shè)線性時變系統(tǒng)的離散狀態(tài)防城和觀測方程為：X(k)=F(kk1)X(k1)T(kk1)U(k1)Y(k)=H(k)X(k)N(k)其中X(k)和Y(k)分別是k時刻的狀態(tài)矢量和觀測矢量F(kk1)為狀態(tài)轉(zhuǎn)移矩陣U(k)為k時刻動態(tài)噪聲T(kk1)為系統(tǒng)控制矩陣H(k)為k時刻觀測矩陣N(k)為k時刻觀測噪聲則卡爾曼濾波的算法流程為：p

3、[ii]=p[ii]f[inkk]a[kklj]f(ii=2ii=kii)f(i=0i=n1i)f(j=0j=m1j)jj=ilja[jj]=0.0f(kk=0kk=n1kk)a[jj]=a[jj]p[inkk]h[jnkk]f(i=0i=m1i)f(j=0j=m1j)jj=imje[jj]=r[jj]f(kk=0kk=n1kk)e[jj]=e[jj]h[inkk]a[kklj]js=rinv(em)if(js==0)free(e)fr

4、ee(a)free(b)return(js)f(i=0i=n1i)f(j=0j=m1j)jj=imjg[jj]=0.0f(kk=0kk=m1kk)g[jj]=g[jj]a[ilkk]e[jmkk]f(i=0i=n1i)jj=(ii1)nix[jj]=0.0f(j=0j=n1j)x[jj]=x[jj]f[inj]x[(ii2)nj]f(i=0i=m1i)jj=ilb[jj]=y[(ii1)mi]f(j=0j=n1j)b[jj]=b[jj]