基于Matlab的卡爾曼濾波算法仿真

PAGE基于Matlab的卡爾曼濾波算法仿真卡爾曼濾波器原理卡爾曼濾波是解決以均方誤差最小為準(zhǔn)則的最佳線(xiàn)性濾波問(wèn)題，它根據(jù)前一個(gè)估計(jì)值和最近一個(gè)觀(guān)察數(shù)據(jù)來(lái)估計(jì)信號(hào)的當(dāng)前值。它是用狀態(tài)方程和遞推方法進(jìn)行估計(jì)的，而它的解是以估計(jì)值（常常是狀態(tài)變量的估計(jì)值）的形式給出其信號(hào)模型是從狀態(tài)方程和量測(cè)方程得到的。

卡爾曼濾波中信號(hào)和噪聲是用狀態(tài)方程和測(cè)量方程來(lái)表示的。因此設(shè)計(jì)卡爾曼濾波器要求已知狀態(tài)方程和測(cè)量方程。它不需要知道全部過(guò)去的數(shù)據(jù)，采用遞推的方法計(jì)算，它既可以用于平穩(wěn)和不平穩(wěn)的隨機(jī)過(guò)程，同時(shí)也可以應(yīng)用解決非時(shí)變和時(shí)變系統(tǒng)，因而它比維納過(guò)濾有更廣泛的應(yīng)用?？柭鼛讉€(gè)重要公式：s?(n|n)=as?(n-1|n-1)+Gn[x(n)–acs?(n-1|n-1)](1)P(n)=a2ξ(n-1)+Q(2)Gn=cP(n)R+ξ(n)=RcGn=(1–這組方程的遞推計(jì)算過(guò)程如圖1所示。圖1.卡爾曼濾波器遞推運(yùn)算流程圖卡爾曼濾波過(guò)程實(shí)際上是獲取維納解的遞推運(yùn)算過(guò)程，這一過(guò)程從某個(gè)初始狀態(tài)啟動(dòng)，經(jīng)過(guò)迭代運(yùn)算，最終到達(dá)穩(wěn)定狀態(tài)，即維納濾波狀態(tài)。遞推計(jì)算按圖1所示進(jìn)行。假設(shè)已經(jīng)有了初始值s?(0|0)和ξ（0），這樣便可由式（2）計(jì)算P(1)，由式（3）計(jì)算G1，由式（4）計(jì)算ξ（1），由式（1）計(jì)算s?(1|1)。ξ（1）和s?(1|1)便成為下一輪迭代運(yùn)算的已知數(shù)據(jù)。在遞推運(yùn)算過(guò)程中，隨著迭代次數(shù)n的增加，ξ（n）將逐漸下降，知道最終趨近于某個(gè)穩(wěn)定值ξ0。這時(shí)ξ（n）=ξ（n-1）=ξ0為求得這個(gè)穩(wěn)定值，將式（3）和式（2）代入式（4），得到ξ02+1-解此方程即可求出ξ0?；贛atlab的卡爾曼濾波器的仿真Matlab代碼如下：clearN=200;w(1)=0;x(1)=5;a=1;c=1;%過(guò)程噪聲Q1=randn(1,N)*1;%測(cè)量噪聲Q2=randn(1,N);%狀態(tài)矩陣fork=2:N;x(k)=a*x(k-1)+Q1(k-1);endfork=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;fort=2:N;Rww=cov(Q1(1:t));Rvv=cov(Q2(1:t));p1(t)=a.^