卡爾曼實驗實驗報告

Kalman濾波實驗報告一、實驗內(nèi)容介紹卡爾曼濾波是以最小均方誤差為預(yù)計的最佳準則，來謀求一套遞推預(yù)計的算法，其實質(zhì)是由量測值重構(gòu)系統(tǒng)的狀態(tài)向量。它以“預(yù)測—實測—修正”的次序遞推，根據(jù)系統(tǒng)的量測值來消除隨機干擾，運用計算機軟件編程實現(xiàn)卡爾曼濾波?？柭^濾中信號和噪聲是用狀態(tài)方程和測量方程來表達的。設(shè)計卡爾曼濾波器規(guī)定已知狀態(tài)方程和測量方程。它不需要懂得全部過去的數(shù)據(jù)，采用遞推的辦法計算，它既能夠用于平穩(wěn)和不平穩(wěn)的隨機過程，同時也能夠應(yīng)用解決非時變和時變系統(tǒng)，將計算機模擬實驗成果與理論分析成果相比較。采用信號與噪聲的狀態(tài)空間模型，運用前一時刻地預(yù)計值和現(xiàn)時刻的觀察值來更新對狀態(tài)變量的預(yù)計，求出現(xiàn)時刻的預(yù)計值，它適合于實時解決和計算機運算。分析影響卡爾曼效果的多個因素，從而加深對卡爾曼濾波的理解。二、自編實驗2.1自編仿真程序%定義仿真的長度nlen=20;%Definethesystem.a=1;h=3;%噪聲協(xié)方差Q=0.01;R=2;%全部陣列的預(yù)分派內(nèi)存%1)thestate.%2)theapriorierror.先驗誤差初步預(yù)計x=zeros(1,nlen);z=zeros(1,nlen);xapriori=zeros(1,nlen);xaposteriori=zeros(1,nlen);residual=zeros(1,nlen);papriori=ones(1,nlen);paposteriori=ones(1,nlen);k=zeros(1,nlen);%計算過程和測量誤差w1=randn(1,nlen);v1=randn(1,nlen);%運行該腳本一次生成多個含有相似%.噪聲w=w1*sqrt(Q);v=v1*sqrt(R);%初始條件對狀態(tài)、xx_0=1.0;%現(xiàn)狀和后驗概率方差的初始猜想xaposteriori_0=1.5;paposteriori_0=1;%計算基于初始狀態(tài)和后驗概率方差的全部值的第一種預(yù)計,將計算在一種循環(huán)中的其它環(huán)節(jié)。%Calculatethestateandtheoutput計算狀態(tài)和輸出x(1)=a*x_0+w(1);z(1)=h*x(1)+v(1);%預(yù)測方程xapriori(1)=a*xaposteriori_0;residual(1)=z(1)-h*xapriori(1);papriori(1)=a*a*paposteriori_0+Q;%校正方程k(1)=h*papriori(1)/(h*h*papriori(1)+R);paposteriori(1)=papriori(1)*(1-h*k(1));xaposteriori(1)=xapriori(1)+k(1)*residual(1);%計算值的預(yù)計forj=2:nlen,%計算狀態(tài)和輸出x(j)=a*x(j-1)+w(j);z(j)=h*x(j)+v(j);%預(yù)測方程xapriori(j)=a*xaposteriori(j-1);residual(j)=z(j)-h*xapriori(j);papriori(j)=a*a*paposteriori(j-1)+Q;%校正方程k(j)=h*papriori(j)/(h*h*papriori(j)+R);paposteriori(j)=papriori(j)*(1-h*k(j));xaposteriori(j)=xapriori(j)+k(j)*residual(j);endj=1:nlen;%狀態(tài)以及狀態(tài)預(yù)計subplot(221);h1=stem(j+0.25,xapriori,'b');holdonh2=stem(j+0.5,xaposteriori,'g');h3=stem(j,x,'r');holdoff%設(shè)計的格式legend([h1(1)h2(1)h3(1)],'apriori','aposteriori','exact');title('Statewithaprioriandaposteriorielements');ylabel('State,x');xlim=[0length(j)+1];set(gca,'XLim',xlim);%協(xié)方差subplot(222);h1=stem(j,papriori,'b');holdon;h2=stem(j,paposteriori,'g');holdofflegend([h1(1)h2(1)],'apriori','aposteriori');title('Calculatedaprioriandaposterioricovariance');%輸出標題先驗和后驗概率計算協(xié)方差ylabel('Covariance');set(gca,'XLim',xlim);%第一種圖形設(shè)立同樣限制%輸出誤差subplot(223);h1=stem(j,x-xapriori,'b');holdonh2=stem(j,x-xaposteriori,'g');holdofflegend([h1(1)h2(1)],'apriori','aposteriori');title('Actualaprioriandaposteriorierror');%實際的先驗和后驗誤差ylabel('Errors');set(gca,'XLim',xlim);%Plotkalmangain,k卡爾曼增益subplot(224);h1=stem(j,k,'b');legend([h1(1)],'kalmangain');title('Kalmangain');ylabel('Kalmangain,k');set(gca,'XLim',xlim);%Setlimitsthesameasfirstgraph2.2自編仿真實驗成果2.3實驗成果分析由實驗成果分析可得通過先驗誤差概率預(yù)計、后驗誤差概率預(yù)計和設(shè)立的預(yù)計的比較，測得其協(xié)方差和彼此之間的誤差來分析卡爾曼濾波的特性。噪聲方差一定的狀況下，一定范疇內(nèi)，采樣間隔性能越好，會引發(fā)系統(tǒng)的臨時不穩(wěn)定。有關(guān)參數(shù)的影響，初始條件、噪聲統(tǒng)