《多传感器融合实验报告.doc》由会员分享，可在线阅读，更多相关《多传感器融合实验报告.doc（9页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、多传感器融合实验报告电信少41 刘星辰 2120406102根据题目中给出的量测方程，进行坐标变换，得以此坐标画图，结果如下：将非线性问题线性化，新的量测方程为其中，扩展卡尔曼滤波算法一个循环如下：将量测方程代入，由于题目中未给出滤波器初值，因此参考作业二中的初值，得到的两个雷达估计的目标状态如下图：距离均方根误差为将估计位置、量测位置分别代入上式，得到两个雷达量测和估计的距离均方差，如下图：可看出单个雷达量测的距离均方根误差是波动的，经过卡尔曼滤波后的误差是逐渐收敛的，且每一时刻都优于量测误差。（3）集中式融合算法：量测方程为过程与单个雷达进行EKF相同。简单凸组合分布式融合算法：即每个传感
2、器单独进行处理，将结果融合。融合估计如下图：将融合后的估计位置代入距离均方根误差，得融合前与融合后的距离均方根误差如下图：可看出融合后的效果优于任何一个雷达的估计效果。将分布式与集中式融合误差放大：可看出集中式融合的效果比简单凸组合分布式融合的效果要好，误差较小，这是因为集中式融合可以利用到所有的原始量测数据，无信息丢失，误差不会传递累计。（4）由距离均方根误差定义可得到速度均方根误差，即类似的可以得到两个雷达融合前以及融合后的速度均方根误差，如下图：放大之后：同样可看出融合后的效果优于融合前任何一个雷达的估计效果，而集中式融合效果优于简单凸组合分布式融合效果。感想：整个作业的完成不仅需要结合
3、课上所学知识和上的例程，也要加以理解、分析p ，理解例程的思想，才能写出正确的代码，得到理想的结果。代码只是工具，理解思路才是完成这次作业的重点。另外一些小的细节也不容忽略，例如弧度角度的转换等。代码如下：clc;clear;close all;参数设置_(:,1)=10000 -100 10000 0'sensor1(:,1)=0;0;sensor2(:,1)=10000;0;T=1;F_cv=1 T;0 1;F= F_cv zeros(2,2)zeros(2,2) F_cv;G=(T2)/2 T 0 0;0 0 (T2)/2 T'w_=1; 过程噪声标准差wy=1;Q=
4、diag(w_2 wy2);v_r=10; 量测噪声标准差v_theta=1_pi/180;R=v_r2 0;0 v_theta2;RR=R zeros(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(
5、1,n);error_v_Fusion=zeros(1,n);error_v_Fusion_d=zeros(1,n);for k=1:M生成真实状态和量测for i=2:nW=w_randn;wy_randn;_(:,i)=F_(:,i-1)+G_W;真实状态endfor i=1:n V=v_r_randn;v_theta_randn;雷达1z1(1,i)=sqrt(_(1,i)-0)2+(_(3,i)-0)2)+V(1);z1(2,i)=atan2(_(3,i)-0,_(1,i)-0)+V(2);sensor1(1,i)=z1(1,i)_cos(z1(2,i);sensor1(2,i)=z1
6、(1,i)_sin(z1(2,i);雷达2z2(1,i)=sqrt(_(1,i)-10000)2+(_(3,i)-0)2)+V(1);z2(2,i)=atan2(_(3,i)-0,_(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卡尔曼滤波_Kalman1(:,1)=10500 110 9500 10'P1(:,:,1)=diag(106 104 106 104);_Kalman2(:,1)=10500
7、 110 9500 10'P2(:,:,1)=diag(106 104 106 104);_Kalman_Fusion(:,1)=10500 110 9500 10'P_Fusion(:,:,1)=diag(106 104 106 104);for i=2:n_Kalman_pre1=F_Kalman1(:,i-1);H1=_Kalman_pre1(1)/sqrt(_Kalman_pre1(1)2+(_Kalman_pre1(3)2) 0 _Kalman_pre1(3)/sqrt(_Kalman_pre1(1)2+(_Kalman_pre1(3)2) 0-_Kalman_p
8、re1(3)/(_Kalman_pre1(1)2+_Kalman_pre1(3)2) 0 _Kalman_pre1(1)/(_Kalman_pre1(1)2+_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(_Kalman_pre1(1)2+_Kalman_pre1(3)2);atan2(_Kalman_pre1(3),_Kalman_pre1(1);_Kalman1(:,i)=_Kalman_pr
9、e1+W1_(z1(:,i)-Z_pre1);P1(:,:,i)=P_pre1-W1_S1_W1' _Kalman_pre2=F_Kalman2(:,i-1);H2=(_Kalman_pre2(1)-10000)/sqrt(_Kalman_pre2(1)-10000)2+(_Kalman_pre2(3)2) 0 _Kalman_pre2(3)/sqrt(_Kalman_pre2(1)-10000)2+_Kalman_pre2(3)2) 0-_Kalman_pre2(3)/(_Kalman_pre2(1)-10000)2+_Kalman_pre2(3)2) 0 (_Kalman_pre
10、2(1)-10000)/(_Kalman_pre2(1)-10000)2+(_Kalman_pre2(3)2) 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(_Kalman_pre2(1)-10000)2+_Kalman_pre2(3)2);atan2(_Kalman_pre2(3),(_Kalman_pre2(1)-10000);_Kalman2(:,i)=_Kalman_pre2+W2_(z2(:,i)-Z_pre2);P2(
11、:,:,i)=P_pre2-W2_S2_W2' P_Fusion_d(:,:,i)=inv(inv(P1(:,:,i)+inv(P2(:,:,i);_Kalman_Fusion_d(:,i)=P_Fusion_d(:,:,i)_(inv(P1(:,:,i)_Kalman1(:,i)+inv(P2(:,:,i)_Kalman2(:,i);H=H1H2;_Kalman_pre_Fusion=F_Kalman_Fusion(:,i-1);P_pre_Fusion=F_P_Fusion(:,:,i-1)_F'+G_Q_G'S_Fusion=H_P_pre_Fusion_H
12、'+RR;W_Fusion=P_pre_Fusion_H'_inv(S_Fusion);Z_pre_Fusion=sqrt(_Kalman_pre_Fusion(1)2+_Kalman_pre_Fusion(3)2);atan2(_Kalman_pre_Fusion(3),_Kalman_pre_Fusion(1);sqrt(_Kalman_pre_Fusion(1)-10000)2+_Kalman_pre_Fusion(3)2);atan2(_Kalman_pre_Fusion(3),(_Kalman_pre_Fusion(1)-10000);_Kalman_Fusion
13、(:,i)=_Kalman_pre_Fusion+W_Fusion_(Z(:,i)-Z_pre_Fusion);P_Fusion(:,:,i)=P_pre_Fusion-W_Fusion_S_Fusion_W_Fusion'endfor i=1:nerror_mea1(i)=error_mea1(i)+ (sensor1(1,i)-_(1,i)2 + (sensor1(2,i)-_(3,i)2;error_Kalman1(i)=error_Kalman1(i)+ (_Kalman1(1,i)-_(1,i)2 + (_Kalman1(3,i)-_(3,i)2;error_mea2(i)
14、=error_mea2(i)+ (sensor2(1,i)-_(1,i)2 + (sensor2(2,i)-_(3,i)2;error_Kalman2(i)=error_Kalman2(i)+ (_Kalman2(1,i)-_(1,i)2 + (_Kalman2(3,i)-_(3,i)2;error_Fusion(i)=error_Fusion(i)+ (_Kalman_Fusion(1,i)-_(1,i)2 + (_Kalman_Fusion(3,i)-_(3,i)2;error_Fusion_d(i)=error_Fusion_d(i)+ (_Kalman_Fusion_d(1,i)-_(
15、1,i)2 + (_Kalman_Fusion_d(3,i)-_(3,i)2;error_v1(i)=error_v1(i)+ (_Kalman1(2,i)-_(2,i)2 + (_Kalman1(4,i)-_(4,i)2;error_v2(i)=error_v2(i)+ (_Kalman2(2,i)-_(2,i)2 + (_Kalman2(4,i)-_(4,i)2;error_v_Fusion(i)=error_v_Fusion(i)+ (_Kalman_Fusion(2,i)-_(2,i)2 + (_Kalman_Fusion(4,i)-_(4,i)2;error_v_Fusion_d(i
16、)=error_v_Fusion_d(i)+ (_Kalman_Fusion_d(2,i)-_(2,i)2 + (_Kalman_Fusion_d(4,i)-_(4,i)2;endend error_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_Ka
17、lman2=sqrt(error_Kalman2);error_Fusion=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_
18、v_Fusion);error_v_Fusion_d=error_v_Fusion_d/M;error_v_Fusion_d=sqrt(error_v_Fusion_d);画图，真实轨迹和量测比较i=2:n;figure;plot(_(1,i),_(3,i),'k',sensor1(1,i),sensor1(2,i),'r',sensor2(1,i),sensor2(2,i),'g');legend('真实位置','雷达1量测值','雷达2量测值');_label(&apos
19、;_');ylabel('Y');画图，真实轨迹和雷达估计比较figure;plot(_(1,i),_(3,i),'k',_Kalman1(1,i),_Kalman1(3,i),'r',_Kalman2(1,i),_Kalman2(3,i),'g');legend('真实轨迹','雷达1估计','雷达2估计');_label('_(m)');ylabel('Y(m)');画图，量测距离均方根误差，估计距离均
20、方根误差比较figure;plot(i,error_mea1(i),'r',i,error_mea2(i),'g',i,error_Kalman1(i),'r-',i,error_Kalman2(i),'g-');legend('雷达1量测距离均方根误差','雷达2量测距离均方根误差','雷达1估计距离均方根估计误差','雷达2估计距离均方根估计误差');_label('时刻(秒)');ylabel('距离
21、均方根误差（米）');画图，真实位置，集中式融合估计，分布式融合估计比较figure;plot(_(1,i),_(3,i),'k',_Kalman_Fusion(1,i), _Kalman_Fusion(3,i),'r',_Kalman_Fusion_d(1,i), _Kalman_Fusion_d(3,i),'g');legend('真实位置','集中式融合估计','分布式融合估计');_label('_');ylabel('Y&ap
22、os;);画图，融合前和融合后距离均方根误差比较figure;plot(i,error_Kalman1(i),'r',i,error_Kalman2(i),'g',i,error_Fusion(i),'b',i,error_Fusion_d(i),'k');legend('雷达1估计距离均方根估计误差','雷达2估计距离均方根估计误差','集中式融合距离均方根估计误差','分布式融合距离均方根估计误差');_label('时刻
23、(秒)');ylabel('距离均方根误差（米）');画图，融合前和融合后速度均方根误差比较figure;plot(i,error_v1(i),'k',i,error_v2(i),'b',i,error_v_Fusion(i),'r',i,error_v_Fusion_d(i),'g');legend('雷达1估计速度均方根估计误差','雷达2估计速度均方根估计误差','集中式融合速度均方根估计误差','分布式融合速度均方根估计误差');_label('时刻(秒)');ylabel('速度均方根误差（米/秒）');第 9 页 共 9 页