www.pudn.com > data_fusion.rar > fusion_asyn.m
% % 融合
% % 同步后数据的融合
% % 时间 2003 6 18
% % 秦玉亮
% % %-----------------------------------------------------------------------------------------------
function y=fusion_asyn;
clear;
clc;
[ZTrue]=source;
Tf=0.020;%融合周期
TimesOut=fix(10/Tf);
%-------------误差计算-----------------------------------
ErrSquareSita=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w=zeros(TimesOut,1); %俯仰角速度误差平方和变量
ErrSquareSita1=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w1=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta1=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w1=zeros(TimesOut,1); %俯仰角速度误差平方和变量
ErrSquareSita2=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w2=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta2=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w2=zeros(TimesOut,1); %俯仰角速度误差平方和变量
mont=10;
for m=1:mont
[Xasyn_Radar,Zasyn_Radar,Pasyn_Radar]=Asynchronism_Radar;
[Xasyn_IR,Zasyn_IR,Pasyn_IR]=Asynchronism_IR;
Xfusion=zeros(6,1,TimesOut);
for i=1:TimesOut
for j=1:6
Xfusion(j,1,i)=(Pasyn_IR(j,j,i)*Xasyn_Radar(j,1,i)+Pasyn_Radar(j,j,i)*Xasyn_IR(j,1,i))/(Pasyn_Radar(j,j,i)+Pasyn_IR(j,j,i));
end
end
Zfusion=zeros(4,1,TimesOut);
%-------------转换为极坐标下值---------------------------------------------------------------
for i=1:TimesOut
%模型观测距离
% range=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;
%模拟观测方位角度
azimuth=atan(Xfusion(1,1,i)/Xfusion(3,1,i)); %%%%% 这里采用的是孙忠康的雷达数据处理中323页的公式 注意其方位角的公式与一般不同!!!!!!!
%
if azimuth>2*pi
azimuth=azimuth-2*pi;
else if azimuth < 0
azimuth=azimuth+2*pi;
end;
end;
%观测俯仰角度
pitching=atan(Xfusion(5,1,i)/(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2)^0.5);
if pitching>pi/2
pitching=pitching-pi;
else if pitching<-pi/2
pitching=pitching+pi;
end;
end;
%观测方位角速度
azimuth_w=(Xfusion(3,1,i)*Xfusion(2,1,i)-Xfusion(1,1,i)*Xfusion(4,1,i))/(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2) ;
%观测俯仰角角速度
R_Rs=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2)^0.5;
R_R=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2+Xfusion(5,1,i)^2)^0.5;
R_V=(Xfusion(1,1,i)*Xfusion(2,1,i)+Xfusion(3,1,i)*Xfusion(4,1,i)+Xfusion(5,1,i)*Xfusion(6,1,i) )/R_R;
pitching_w=( Xfusion(6,1,i)*R_R-Xfusion(5,1,i)*R_V)/(R_R*R_Rs) ;
Zfusion(:,:,i)=[%range;
azimuth;
pitching;
azimuth_w;
pitching_w];
end;
%-------------转换为极坐标系下值结束-------------
for i=1:TimesOut
ErrSquareSita(i)=(ZTrue(1,1,i)-Zfusion(1,1,i)).^2+ErrSquareSita(i);
ErrSquareBeta(i)=(ZTrue(2,1,i)-Zfusion(2,1,i)).^2+ErrSquareBeta(i);
ErrSquareSita_w(i)=(ZTrue(3,1,i)-Zfusion(3,1,i)).^2+ErrSquareSita_w(i);
ErrSquareBeta_w(i)=(ZTrue(4,1,i)-Zfusion(4,1,i)).^2+ErrSquareBeta_w(i);
ErrSquareSita1(i)=(ZTrue(1,1,i)-Zasyn_Radar(1,1,i)).^2+ErrSquareSita1(i);
ErrSquareBeta1(i)=(ZTrue(2,1,i)-Zasyn_Radar(2,1,i)).^2+ErrSquareBeta1(i);
ErrSquareSita_w1(i)=(ZTrue(3,1,i)-Zasyn_Radar(3,1,i)).^2+ErrSquareSita_w1(i);
ErrSquareBeta_w1(i)=(ZTrue(4,1,i)-Zasyn_Radar(4,1,i)).^2+ErrSquareBeta_w1(i);
ErrSquareSita2(i)=(ZTrue(1,1,i)-Zasyn_IR(1,1,i)).^2+ErrSquareSita2(i);
ErrSquareBeta2(i)=(ZTrue(2,1,i)-Zasyn_IR(2,1,i)).^2+ErrSquareBeta2(i);
ErrSquareSita_w2(i)=(ZTrue(3,1,i)-Zasyn_IR(3,1,i)).^2+ErrSquareSita_w2(i);
ErrSquareBeta_w2(i)=(ZTrue(4,1,i)-Zasyn_IR(4,1,i)).^2+ErrSquareBeta_w2(i);
end
end
% for i=2:TimesOut
% A(i)=sqrt(ErrSquareSita(i)/mont);
% B(i)=sqrt(ErrSquareBeta(i)/mont);
% C(i)=sqrt(ErrSquareSita_w(i)/mont);
% D(i)=sqrt(ErrSquareBeta_w(i)/mont);
%
% A1(i)=sqrt(ErrSquareSita1(i)/mont);
% B1(i)=sqrt(ErrSquareBeta1(i)/mont);
% C1(i)=sqrt(ErrSquareSita_w1(i)/mont);
% D1(i)=sqrt(ErrSquareBeta_w1(i)/mont);
%
% A2(i)=sqrt(ErrSquareSita2(i)/mont);
% B2(i)=sqrt(ErrSquareBeta2(i)/mont);
% C2(i)=sqrt(ErrSquareSita_w2(i)/mont);
% D2(i)=sqrt(ErrSquareBeta_w2(i)/mont);
%
% end
for i=2:TimesOut
A(i-1)=sqrt(ErrSquareSita(i)/mont);
B(i-1)=sqrt(ErrSquareBeta(i)/mont);
C(i-1)=sqrt(ErrSquareSita_w(i)/mont);
D(i-1)=sqrt(ErrSquareBeta_w(i)/mont);
A1(i-1)=sqrt(ErrSquareSita1(i)/mont);
B1(i-1)=sqrt(ErrSquareBeta1(i)/mont);
C1(i-1)=sqrt(ErrSquareSita_w1(i)/mont);
D1(i-1)=sqrt(ErrSquareBeta_w1(i)/mont);
A2(i-1)=sqrt(ErrSquareSita2(i)/mont);
B2(i-1)=sqrt(ErrSquareBeta2(i)/mont);
C2(i-1)=sqrt(ErrSquareSita_w2(i)/mont);
D2(i-1)=sqrt(ErrSquareBeta_w2(i)/mont);
end
%-------------误差计算结束------------------------------
p=2:TimesOut;
plot(p,A,'r',p,A1,p,A2);
title('方位角误差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合结果','雷达滤波值','红外滤波值');
figure;
plot(p,B,'r',p,B1,p,B2);
title('俯仰角误差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合结果','雷达滤波值','红外滤波值');
figure;
plot(p,C,'r',p,C1,p,C2);
title('方位角角速率误差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合结果','雷达滤波值','红外滤波值');
figure;
plot(p,D,'r',p,D1,p,D2);
title('俯仰角角速率误差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合结果','雷达滤波值','红外滤波值');
% for i=1:TimesOut
% A(i)=Zasyn_Radar(1,1,i);
% B(i)=ZTrue(1,1,i);
% C(i)=Zasyn_IR(1,1,i);
% end
% plot(p,A,p,B,'r',p,C);