多传感器融合实验报告

　 非线性卡尔曼滤波与多传感器融合

　电信少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

　 %卡尔曼滤波

　 X_Kalman1(:,1)=[10500 110 9500 10]';

　 P1(:,:,1)=diag([10^6 10^4 10^6 10^4]);

　 X_Kalman2(:,1)=[10500 110 9500 10]';

　 P2(:,:,1)=diag([10^6 10^4 10^6 10^4]);

　 X_Kalman_Fusion(:,1)=[10500 110 9500 10]';

　 P_Fusion(:,:,1)=diag([10^6 10^4 10^6 10^4]);

　 for i=2:n

　 X_Kalman_pre1=F*X_Kalman1(:,i-1);

　 H1=[X_Kalman_pre1(1)/sqrt((X_Kalman_pre1(1))^2+(X_Kalman_pre1(3))^2) 0 X_Kalman_pre1(3)/sqrt((X_Kalman_pre1(1))^2+(X_Kalman_pre1(3))^2) 0

　 -X_Kalman_pre1(3)/(X_Kalman_pre1(1)^2+X_Kalman_pre1(3)^2) 0 X_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);

　 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))^2) 0 X_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)/((X_Kalman_pre2(1)-10000)^2+(X_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((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_Fusion_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=[H1

　 H2];

　 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_Fusion);

　 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*(Z(:,i)-Z_pre_Fusion);

　 P_Fusion(:,:,i)=P_pre_Fusion-W_Fusion*S_Fusion*W_Fusion';

　 end

　 for i=1:n

　 error_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))^2 + (sensor2(2,i)-X(3,i))^2;

　 error_Kalman2(i)=error_Kalman2(i)+ (X_Kalman2(1,i)-X(1,i))^2 + (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))^2;

　 error_v1(i)=error_v1(i)+ (X_Kalman1(2,i)-X(2,i))^2 + (X_Kalman1(4,i)-X(4,i))^2;

　 error_v2(i)=error_v2(i)+ (X_Kalman2(2,i)-X(2,i))^2 + (X_Kalman2(4,i)-X(4,i))^2;

　 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_Fusion_d(2,i)-X(2,i))^2 + (X_Kalman_Fusion_d(4,i)-X(4,i))^2;

　 end

　end

　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_Kalman2=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_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(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,error_mea2(i),'g',...

　 i,error_Kalman1(i),'r--',i,error_Kalman2(i),'g--');

　legend('雷达1量测距离均方根误差','雷达2量测距离均方根误差',...

　 '雷达1估计距离均方根估计误差','雷达2估计距离均方根估计误差');

　xlabel('时刻(秒)');ylabel('距离均方根误差（米）');

　%画图，真实位置，集中式融合估计，分布式融合估计比较

　figure;

　plot(X(1,i),X(3,i),'k',X_Kalman_Fusion(1,i), X_Kalman_Fusion(3,i),'r',X_Kalman_Fusion_d(1,i), X_Kalman_Fusion_d(3,i),'g');

　legend('真实位置','集中式融合估计','分布式融合估计');

　xlabel('X');ylabel('Y');

　%画图，融合前和融合后距离均方根误差比较

　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估计距离均方根估计误差','集中式融合距离均方根估计误差','分布式融合距离均方根估计误差');

　xlabel('时刻(秒)');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估计速度均方根估计误差','集中式融合速度均方根估计误差','分布式融合速度均方根估计误差');

　xlabel('时刻(秒)');ylabel('速度均方根误差（米/秒）');