卡爾曼濾波程序

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

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