多傳感器融合實驗報告

1、00y-y)xcos(arctan(ki)+v)r,kx-xe,kkiy一y、ki-)x-x非線性卡爾曼濾波與多傳感器融合電信少41劉星辰2120406102（1）根據(jù)題目中給出的量測方程，進(jìn)行坐標(biāo)變換x(k)=rxcose=(：(x一x)2+(y一y)2+vk,ik,i*kiki+Ve丿y(k)=rxsine=Q(x-x)2+(y-y)2+v)xsin(arctan(k,ik,i*kikir,k以此坐標(biāo)畫圖，結(jié)果如下：（2）將非線性問題線性化，新的量測方程為Z=HX+vk,ik,ikr,k其中，kiHk,i(xk-xi)2+(yk-yi)2(x-x)ki:(x-x)2+(y-y)2TOC o

2、 1-5 h zkiki-(y-y)ki HYPERLINK l bookmark4 o Current Document ；(x-x)2+(y-y)2丿kiki(x-x)ki(x-x)2+(y-y)2kikiX=lxkkyk擴(kuò)展卡爾曼濾波算法一個循環(huán)如下：x(k+IIk+1)=x(k+IIk)+W(k+l)Ez(k+1)-Z(k+IIk)1P(k+IIk)=F(k)P(kIk)F(k)+Q(k)S(k+1)=R(k+1)+H(k+1)P(k+11k)H(k+1)W(k+1)=P(k+1Ik)H(k+1)S(k+1)-1P(k+1Ik+1)=P(k+1Ik)-W(k+1)S(k+1)W(k+1

3、)-1LFigure2最1.001.t61.02真實軌跡陽達(dá)2怙訂將量測方程代入，由于題目中未給出濾波器初值，因此參考作業(yè)二中的初值，得到的兩個雷達(dá)估計的目標(biāo)狀態(tài)如下圖：I1040.93L0FileE.ditViewInEiertToolsDesHopWindowHelpIIIIF200040)06000aOOO100002000X(ni)距離均方根誤差為RMSE(k)=蘭kx-x)2+(y-y)2positionkkkkIi=1將估計位置、量測位置分別代入上式，得到兩個雷達(dá)量測和估計的距離均方差如下圖：fileEditiewInsertlootsDesktopi門dow且呂p呂h釦氏|輒紳毀

4、曲|口|口凰|叨可看出單個雷達(dá)量測的距離均方根誤差是波動的，經(jīng)過卡爾曼濾波后的誤差是逐漸收斂的，且每一時刻都優(yōu)于量測誤差。（3）集中式融合算法：量測方程為z=tz）T（z）T.（z（N）T-TTOC o 1-5 h zk+1k+1k+1k+1H#H）T（H）T.（H（N）Tk+1Ik+1k+1Ik+1v=vTvT.v（N）Tk+1k+1k+1k+1過程與單個雷達(dá)進(jìn)行EKF相同。簡單凸組合分布式融合算法：X=送(Pi)1送(Pi)1XiLi=1i=1P=送(Pi)-11Li=1即每個傳感器單獨進(jìn)行處理，將結(jié)果融合。融合估計如下圖：nFigure4i.iy頁實鬥.首井擊弋邂令估訂FileEdity

5、iewInsertToolsDesktopWindowHelp0.9911111111101000200030004000&00060007OWBOOO90CK)1CKMX)X將融合后的估計位置代入距離均方根誤差，得融合前與融合后的距離均方根誤差如下圖：iFigue5FileEdit里iewInsertToolsDe-slctopWindowHelp電飩舒gQHI回253雷達(dá)1啟訃匪離均方恨佶訃遢淮常達(dá)2岀訃N審均方抿怙計謀揑耒中式融肯UB海均方根怙計謀娃彷布人遜館謂均方相i古訂舷010Z03040500D70ao93100時刻（秒）可看出融合后的效果優(yōu)于任何一個雷達(dá)的估計效果。將分布式與集中

6、式融合誤差放大：Figure53FileEditViewInsertToolsDesktopWindowHelpuah|fez-|a|n|Q15雷達(dá)1怙i|趾宵切方根怙計許差-雷達(dá)2估計距盹方根怙引讖差集中式融合梔離均方棍估訃課差井布式RL會梔商均片報估訃課差363-73B39404142時刻（秒）可看出集中式融合的效果比簡單凸組合分布式融合的效果要好，誤差較小，這是因為集中式融合可以利用到所有的原始量測數(shù)據(jù)，無信息丟失，誤差不會傳遞累計。（4）由距離均方根誤差定義可得到速度均方根誤差，即Rmse（k）=史lxx）2+6y）2vMkkkki=1類似的可以得到兩個雷達(dá)融合前以及融合后的速度均方根

7、誤差，如下圖：X三2eiEditViewInseirtToolsDe&ldopWindowHelp當(dāng)Dada%耳寥凰搖”S1Z|Q250第估計速厘均方總曲計泯養(yǎng)豚這左怙計堆度減1方柚佔計說繪200-集中式6k甘遠(yuǎn)度均方恨怙訃型差-井布式度均方恨估計罠魁0102D30J050607080W100時刻砂.1OT.150U65.566妬.56767.5bB時細(xì)秒）Figure6FilsEditViewInsertToolsDesktopWindoiA*旦Elp門-.JM.5笛豈兇放大之后：*=同樣可看出融合后的效果優(yōu)于融合前任何一個雷達(dá)的估計效果，而集中式融合效果優(yōu)于簡單凸組合分布式融合效果。感想：

8、整個作業(yè)的完成不僅需要結(jié)合課上所學(xué)知識和PPT上的例程，也要加以理解、分析，理解例程的思想，才能寫出正確的代碼，得到理想的結(jié)果。代碼只是工具，理解思路才是完成這次作業(yè)的重點。另外一些小的細(xì)節(jié)也不容忽略，例如弧度角度的轉(zhuǎn)換等。代碼如下：clc;clear;closeall;%參數(shù)設(shè)置X(:,1)=10000-100100000;sensor1(:,1)=0;0;sensor2(:,1)=10000;0;T=1;F_cv=1T;01;F=F_cvzeros(2,2)zeros(2,2)F_cv;G=(TA2)/2T00;00(TA2)/2T;wx=1;%過程噪聲標(biāo)準(zhǔn)差wy=1;Q=diag(wx2

9、wyA2);v_r=10;%量測噪聲標(biāo)準(zhǔn)差v_theta=1*pi/180;R=v_rA20;0v_thetaA2;RR=Rzeros(2,2)zeros(2,2)R;n=100;M=500;%Monte-Carlo仿真次數(shù)error_mea1=zeros(1,n);error_Kalman1=zeros(1,n);error_mea2=zeros(1,n);error_Kalman2=zeros(1,n);error_Fusion=zeros(1,n);error_Fusion_d=zeros(1,n);error_v1=zeros(1,n);error_v2=zeros(1,n);erro

10、r_v_Fusion=zeros(1,n);error_v_Fusion_d=zeros(1,n);fork=1:M%生成真實狀態(tài)和量測fori=2:nW=wx*randn;wy*randn;X(:,i)=F*X(:,i-1)+G*W%真實狀態(tài)endfori=1:nV=v_r*randn;v_theta*randn;%雷達(dá)1z1(1,i)=sqrt(X(1,i)-0)入2+(X(3,i)-0)入2)+V(1);z1(2,i)=atan2(X(3,i)-0,X(1,i)-0)+V(2);sensor1(1,i)=z1(1,i)*cos(z1(2,i);sensor1(2,i)=z1(1,i)*s

11、in(z1(2,i);%雷達(dá)2z2(1,i)=sqrt(X(1,i)-10000)入2+(X(3,i)-0)入2)+V(1);z2(2,i)=atan2(X(3,i)-0,X(1,i)-10000)+V(2);sensor2(1,i)=z2(1,i)*cos(z2(2,i)+10000;sensor2(2,i)=z2(1,i)*sin(z2(2,i);Z(:,i)=z1(:,i);z2(:,i);end%卡爾曼濾波X_Kalman1(:,1)=10500110950010;Pl(:,:,1)=diag(10A610A410A610A4);X_Kalman2(:,1)=1050011095001

12、0;P2(:,:,1)=diag(10A610A410A610A4);X_Kalman_Fusion(:,1)=10500110950010;P_Fusion(:,:,1)=diag(10A610A410A610A4);fori=2:nX_Kalman_pre1=F*X_Kalman1(:,i-1);Hl=X_Kalman_pre1(1)/sqrt(X_Kalman_pre1(1)入2+(X_Kalman_pre1(3)入2)0X_Kalman_pre1(3)/sqrt(X_Kalman_pre1(1)入2+(X_Kalman_pre1(3)入2)0-X_Kalman_pre1(3)/(X_K

13、alman_pre1(1)入2+X_Kalman_pre1(3)入2)0X_Kalman_pre1(1)/(X_Kalman_pre1(1)入2+X_Kalman_pre1(3)入2)0;P_pre1=F*P1(:,:,i-1)*F+G*Q*G;S1=H1*P_pre1*H1+R;W1=P_pre1*H1*inv(S1);Z_pre1=sqrt(X_Kalman_pre1(1)入2+X_Kalman_pre1(3)入2);atan2(X_Kalman_pre1(3),X_Kalman_pre1(1);X_Kalman1(:,i)=X_Kalman_pre1+W1*(z1(:,i)-Z_pre1

14、);P1(:,:,i)=P_pre1-W1*S1*W1;X_Kalman_pre2=F*X_Kalman2(:,i-1);H2=(X_Kalman_pre2(1)-10000)/sqrt(X_Kalman_pre2(1)-10000)入2+(X_Kalman_pre2(3)A2)0X_Kalman_pre2(3)/sqrt(X_Kalman_pre2(1)-10000)入2+X_Kalman_pre2(3)入2)0-X_Kalman_pre2(3)/(X_Kalman_pre2(1)-10000)入2+X_Kalman_pre2(3)入2)0(X_Kalman_pre2(1)-10000)/(

15、X_Kalman_pre2(1)-10000)入2+(X_Kalman_pre2(3)A2)0;P_pre2=F*P2(:,:,i-1)*F+G*Q*G;S2=H2*P_pre2*H2+R;W2=P_pre2*H2*inv(S2);Z_pre2=sqrt(X_Kalman_pre2(1)-10000)入2+X_Kalman_pre2(3)入2);atan2(X_Kalman_pre2(3),(X_Kalman_pre2(1)-10000);X_Kalman2(:,i)=X_Kalman_pre2+W2*(z2(:,i)-Z_pre2);P2(:,:,i)=P_pre2-W2*S2*W2;P_F

16、usion_d(:,:,i)=inv(inv(P1(:,:,i)+inv(P2(:,:,i);X_Kalman_Fusion_d(:,i)=P_Fusion_d(:,:,i)*(inv(P1(:,:,i)*X_Kalman1(:,i)+inv(P2(:,:,i)*X_Kalman2(:,i);H=H1H2;X_Kalman_pre_Fusion=F*X_Kalman_Fusion(:,i-1);P_pre_Fusion=F*P_Fusion(:,:,i-1)*F+G*Q*G;S_Fusion=H*P_pre_Fusion*H+RR;W_Fusion=P_pre_Fusion*H*inv(S_F

17、usion);Z_pre_Fusion=sqrt(X_Kalman_pre_Fusion(1)入2+X_Kalman_pre_Fusion(3)入2);atan2(X_Kalman_pre_Fusion(3),X_Kalman_pre_Fusion(1);sqrt(X_Kalman_pre_Fusion(1)-10000)入2+X_Kalman_pre_Fusion(3)入2);atan2(X_Kalman_pre_Fusion(3),(X_Kalman_pre_Fusion(1)-10000);X_Kalman_Fusion(:,i)=X_Kalman_pre_Fusion+W_Fusion

18、*(Z(:,i)-Z_pre_Fusion);P_Fusion(:,:,i)=P_pre_Fusion-W_Fusion*S_Fusion*W_Fusion;endfori=1:nerror_mea1(i)=error_mea1(i)+(sensor1(1,i)-X(1,i)入2+(sensor1(2,i)-X(3,i)八2;error_Kalman1(i)=error_Kalman1(i)+(X_Kalman1(1,i)-X(1,i)入2+(X_Kalman1(3,i)-X(3,i)入2;error_mea2(i)=error_mea2(i)+(sensor2(1,i)-X(1,i)A2+(

19、sensor2(2,i)-X(3,i)A2;error_Kalman2(i)=error_Kalman2(i)+(X_Kalman2(1,i)-X(1,i)A2+(X_Kalman2(3,i)-X(3,i)入2;error_Fusion(i)=error_Fusion(i)+(X_Kalman_Fusion(1,i)-X(1,i)入2+(X_Kalman_Fusion(3,i)-X(3,i)入2;error_Fusion_d(i)=error_Fusion_d(i)+(X_Kalman_Fusion_d(1,i)-X(1,i)入2+(X_Kalman_Fusion_d(3,i)-X(3,i)入

20、2;error_v1(i)=error_v1(i)+(X_Kalman1(2,i)-X(2,i)A2+(X_Kalman1(4,i)-X(4,i)入2;error_v2(i)=error_v2(i)+(X_Kalman2(2,i)-X(2,i)A2+(X_Kalman2(4,i)-X(4,i)A2;error_v_Fusion(i)=error_v_Fusion(i)+(X_Kalman_Fusion(2,i)-X(2,i)入2+(X_Kalman_Fusion(4,i)-X(4,i)入2;error_v_Fusion_d(i)=error_v_Fusion_d(i)+(X_Kalman_Fu

21、sion_d(2,i)-X(2,i)入2+(X_Kalman_Fusion_d(4,i)-X(4,i)入2;endenderror_mea1=error_mea1/M;error_mea1=sqrt(error_mea1);error_mea2=error_mea2/M;error_mea2=sqrt(error_mea2);error_Kalman1=error_Kalman1/M;error_Kalman1=sqrt(error_Kalman1);error_Kalman2=error_Kalman2/M;error_Kalman2=sqrt(error_Kalman2);error_Fu

22、sion=error_Fusion/M;error_Fusion=sqrt(error_Fusion);error_Fusion_d=error_Fusion_d/M;error_Fusion_d=sqrt(error_Fusion_d);error_v1=error_v1/M;error_v1=sqrt(error_v1);error_v2=error_v2/M;error_v2=sqrt(error_v2);error_v_Fusion=error_v_Fusion/M;error_v_Fusion=sqrt(error_v_Fusion);error_v_Fusion_d=error_v

23、_Fusion_d/M;error_v_Fusion_d=sqrt(error_v_Fusion_d);%畫圖，真實軌跡和量測比較i=2:n;figure;plot(X(1,i),X(3,i),k,sensor1(1,i),sensor1(2,i),r,sensor2(1,i),sensor2(2,i),g);legend(真實位置,雷達(dá)1量測值，雷達(dá)2量測值);xlabel(X);ylabel(Y);%畫圖，真實軌跡和雷達(dá)估計比較figure;plot(X(1,i),X(3,i),k,X_Kalman1(1,i),X_Kalman1(3,i),r,X_Kalman2(1,i),X_Kalman2(3,i),g);legend(真實軌跡,雷達(dá)1估計,雷達(dá)2估計);xlabel(X(m);ylabel(Y(m);%畫圖，量測距離均方根誤差，估計距離均方根誤差比較figure;plot(i,error_mea1(i),r,i,err