多传感器融合实验报告.docVIP

非线性卡尔曼滤波与多传感器融合 电信少41 刘星辰 2120406102 根据题目中给出的量测方程，进行坐标变换，得 以此坐标画图，结果如下： 将非线性问题线性化，新的量测方程为 其中， 扩展卡尔曼滤波算法一个循环如下： 将量测方程代入，由于题目中未给出滤波器初值，因此参考作业二中的初值，得到的两个雷达估计的目标状态如下图： 距离均方根误差为 将估计位置、量测位置分别代入上式，得到两个雷达量测和估计的距离均方差，如下图： 可看出单个雷达量测的距离均方根误差是波动的，经过卡尔曼滤波后的误差是逐渐收敛的，且每一时刻都优于量测误差。 （3） 集中式融合算法： 量测方程为 过程与单个雷达进行EKF相同。 简单凸组合分布式融合算法： 即每个传感器单独进行处理，将结果融合。 融合估计如下图： 将融合后的估计位置代入距离均方根误差，得融合前与融合后的距离均方根误差如下图： 可看出融合后的效果优于任何一个雷达的估计效果。 将分布式与集中式融合误差放大： 可看出集中式融合的效果比简单凸组合分布式融合的效果要好，误差较小，这是因为集中式融合可以利用到所有的原始量测数据，无信息丢失，误差不会传递累计。 （4） 由距离均方根误差定义可得到速度均方根误差，即 类似的可以得到两个雷达融合前以及融合后的速度均方根误差，如下图： 放大之后： 同样可看出融合后的效果优于融合前任何一个雷达的估计效果，而集中式融合效果优于简单凸组合分布式融合效果。 感想： 整个作业的完成不仅需要结合课上所学知识和PPT上的例程，也要加以理解、分析，理解例程的思想，才能写出正确的代码，得到理想的结果。代码只是工具，理解思路才是完成这次作业的重点。另外一些小的细节也不容忽略，例如弧度角度的转换等。 代码如下： clc;clear;close all; %参数设置 X(:,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=[(T^2)/2 T 0 0; 0 0 (T^2)/2 T]; wx=1; %过程噪声标准差 wy=1; Q=diag([wx^2 wy^2]); v_r=10; %量测噪声标准差 v_theta=1*pi/180; R=[v_r^2 0; 0 v_theta^2]; 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(1,n); error_v_Fusion=zeros(1,n); error_v_Fusion_d=zeros(1,n); for k=1:M %生成真实状态和量测 for i=2:n W=[wx*randn;wy*randn]; X(:,i)=F*X(:,i-1)+G*W;%真实状态 end for i=1:n V=[v_r*randn; v_theta*randn]; %雷达1 z1(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)*sin(z1(2,i)); %雷达2 z2(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 %