多传感器融合实验报告

1、00y-y)xcos(arctan(ki)+v)r,kx-xe,kkiy一y、ki-)x-x非线性卡尔曼滤波与多传感器融合电信少41刘星辰2120406102（1）根据题目中给出的量测方程，进行坐标变换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以此坐标画图，结果如下：（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扩展卡尔曼滤波算法一个循环如下：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真实轨迹陽达2怙订将量测方程代入，由于题目中未给出滤波器初值，因此参考作业二中的初值，得到的两个雷达估计的目标状态如下图：I1040.93L0FileE.ditViewInEiertToolsDesHopWindowHelpIIIIF200040)06000aOOO100002000X(ni)距离均方根误差为RMSE(k)=兰kx-x)2+(y-y)2positionkkkkIi=1将估计位置、量测位置分别代入上式，得到两个雷达量测和估计的距离均方差如下图：fileEditiewInsertlootsDesktopi门dow且吕p吕h釦氏|輒紳毀

4、曲|口|口凰|叨可看出单个雷达量测的距离均方根误差是波动的，经过卡尔曼滤波后的误差是逐渐收敛的，且每一时刻都优于量测误差。（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过程与单个雷达进行EKF相同。简单凸组合分布式融合算法：X=送(Pi)1送(Pi)1XiLi=1i=1P=送(Pi)-11Li=1即每个传感器单独进行处理，将结果融合。融合估计如下图：nFigure4i.iy頁实鬥.首井击弋邂令估订FileEdity

5、iewInsertToolsDesktopWindowHelp0.9911111111101000200030004000&00060007OWBOOO90CK)1CKMX)X将融合后的估计位置代入距离均方根误差，得融合前与融合后的距离均方根误差如下图：iFigue5FileEdit里iewInsertToolsDe-slctopWindowHelp電饨舒gQHI回253雷达1启讣匪离均方恨佶讣遢淮常达2岀讣N审均方抿怙计谋揑耒中式融肯UB海均方根怙计谋娃彷布人逊館謂均方相i古订舷010Z03040500D70ao93100时刻（秒）可看出融合后的效果优于任何一个雷达的估计效果。将分布式与集中

6、式融合误差放大：Figure53FileEditViewInsertToolsDesktopWindowHelpuah|fez-|a|n|Q15雷达1怙i|趾宵切方根怙计许差-雷达2估计距盹方根怙引谶差集中式融合梔离均方棍估讣课差井布式RL會梔商均片报估讣课差363-73B39404142时刻（秒）可看出集中式融合的效果比简单凸组合分布式融合的效果要好，误差较小，这是因为集中式融合可以利用到所有的原始量测数据，无信息丢失，误差不会传递累计。（4）由距离均方根误差定义可得到速度均方根误差，即Rmse（k）=史lxx）2+6y）2vMkkkki=1类似的可以得到两个雷达融合前以及融合后的速度均方根

7、误差，如下图：X三2eiEditViewInseirtToolsDe&ldopWindowHelp當Dada%耳寥凰搖”S1Z|Q250第估计速厘均方總曲计泯養豚这左怙计堆度减1方柚佔计说绘200-集中式6k甘遠度均方恨怙讣型差-井布式度均方恨估计罠魁0102D30J050607080W100时刻砂.1OT.150U65.566妬.56767.5bB时細秒）Figure6FilsEditViewInsertToolsDesktopWindoiA*旦Elp门-.JM.5笛岂凶放大之后：*=同样可看出融合后的效果优于融合前任何一个雷达的估计效果，而集中式融合效果优于简单凸组合分布式融合效果。感想：

8、整个作业的完成不仅需要结合课上所学知识和PPT上的例程，也要加以理解、分析，理解例程的思想，才能写出正确的代码，得到理想的结果。代码只是工具，理解思路才是完成这次作业的重点。另外一些小的细节也不容忽略，例如弧度角度的转换等。代码如下：clc;clear;closeall;%参数设置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;%过程噪声标准差wy=1;Q=diag(wx2

9、wyA2);v_r=10;%量测噪声标准差v_theta=1*pi/180;R=v_rA20;0v_thetaA2;RR=Rzeros(2,2)zeros(2,2)R;n=100;M=500;%Monte-Carlo仿真次数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%生成真实状态和量测fori=2:nW=wx*randn;wy*randn;X(:,i)=F*X(:,i-1)+G*W%真实状态endfori=1:nV=v_r*randn;v_theta*randn;%雷达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);%雷达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(真实位置,雷达1量测值，雷达2量测值);xlabel(X);ylabel(Y);%画图，真实轨迹和雷达估计比较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(真实轨迹,雷达1估计,雷达2估计);xlabel(X(m);ylabel(Y(m);%画图，量测距离均方根误差，估计距离均方根误差比较figure;plot(i,error_mea1(i),r,i,err