www.pudn.com > data_fusion.rar > EKF_Radar.m
% % 子程序
% % 采用扩展卡尔曼滤波算法
% % 时间 2003 6 17
% % 秦玉亮
% % %-----------------------------------------------------------------------------------------------
function [X2,Zfilt,P2,ZRadar]=EKF_Radar;
%丛SourceRadar.m中调用所有用到的参数
[ZTrueRadar,ZRadar,XTrueRadar,totaltimeRadar,T0Radar,Tao0_Radar,t_Radar]=sourceRadar;
%采样点数：总运动时间/采样周期＋1
T=T0Radar;
times=totaltimeRadar/T+1;
%计算参数准备:
%状态方程X(k+1)=phi*X(k)+G*W(k) ;E[W(k)W(k)']=Q
%X(k)=[x volx y voly z volz]';
phi=[1 T 0 0 0 0;
0 1 0 0 0 0;
0 0 1 T 0 0;
0 0 0 1 0 0;
0 0 0 0 1 T;
0 0 0 0 0 1];
G=[T^2/2 0 0;
T 0 0;
0 T^2/2 0;
0 T 0;
0 0 T^2/2;
0 0 T];
%-----------------------------------------------------------------------------------------------
DeltaR=3000; %观测距离误差标准差
DeltaSita=1*pi/180; %观测方位角角度误差标准差 此例中方位角和俯仰角的标准差应相等
DeltaBeta=1*pi/180; %观测俯仰角角度误差标准差
DeltaSita_w=.2*pi/180; %观测方位角角速度误差标准差 此例中方位角和俯仰角角速度的标准差应相等
DeltaBeta_w=.2*pi/180; %观测俯仰角角速度误差标准差
q=0.01; % q是系统噪声标准差
Q=q^2*eye(3); % 系统各方向的状态噪声方差
%-------------------
X2=zeros(6,1,times);
P2=zeros(6,6,times);
P0=zeros(6,6,times);
nCount=0; %计数 每个融合周期内被动雷达滤波次数的计数
j=0; %融合周期计数
%--------------------EKF滤波方法，结果为X2－－－－－－－－－－－－－－
X2(:,:,1)=XTrueRadar(:,:,1);
%极坐标观测噪声协方差R2
R2=[
DeltaSita^2 0 0 0;
0 DeltaBeta^2 0 0;
0 0 DeltaSita_w ^2 0 ;
0 0 0 DeltaBeta_w^2];
%以Z(:,:,1),Z(:,:,2)(极坐标系的测量值），建立模型的初始估计xx，
r2=12000;
r1=r2;
% r1=Z1(1,1,1);
sita1=ZRadar(1,1,1); beta1=ZRadar(2,1,1);
% r2=ZRadar(1,1,2);
sita2=ZRadar(1,1,2); beta2=ZRadar(2,1,2);
%设定初始值和初始状态
X2(:,:,2)=[r2*cos(beta2)*sin(sita2);
(r2*cos(beta2)*sin(sita2)-r1*cos(beta1)*sin(sita1))/T;
r2*cos(beta2)*cos(sita2);
(r2*cos(beta2)*cos(sita2)-r1*cos(beta1)*cos(sita1))/T;
r2*sin(beta2);
(r2*sin(beta2)-r1*sin(beta1))/T];
%使用真值计算初值估计误差方差
P0=(XTrueRadar(:,:,2)-X2(:,:,2))*(XTrueRadar(:,:,2)-X2(:,:,2))';
P2(:,:,2)=P0;
P2(:,:,1)=P0;
for i=2:times-1
Xest=phi*X2(:,:,i);
Q1=G*Q*G';
Ppre=phi*P2(:,:,i)*phi'+Q1';
%计算 H'[X(k+1|k)]
%临时变量xk,yk,zk; xkf, ykf ,zkf; RR, Rz, rrf
xk=Xest(1,1); yk=Xest(3,1); zk=Xest(5,1); xkf=Xest(2,1); ykf=Xest(4,1); zkf=Xest(6,1);
RR=(xk^2+yk^2+zk^2)^0.5;
Rz=(xk^2+yk^2)^0.5;
rrf= (xk*xkf+yk*ykf+zk*zkf)/RR;
h11=xk/RR;
h13=yk/RR;
h15=zk/RR;
h21=yk/Rz^2;
h23=- xk/Rz^2;
h31=-xk*zk/(RR^ 2*Rz);
h33=-yk*zk/(RR^2*Rz);
h35=Rz/RR^2;
h41=(2*xk*(xk*ykf-yk*xkf)-ykf*Rz^2)/Rz^4;
h42=yk/Rz^2;
h43=(xkf*Rz^2-2*yk*(yk*xkf-xk*ykf))/Rz^4;
h44=-xk/Rz^2;
h51=((xk*zkf-zk*xkf)*RR*Rz^2-(zkf*RR-rrf*zk)*(2*xk*Rz^2+zk^2*xk))/(RR^3*Rz^3);
h52=-zk*xk/(RR^2*Rz);
h53=((yk*zkf-zk*ykf)*Rz^2*RR-(zkf*RR-rrf*zk)*(2*yk*Rz^2+yk*zk^2))/(RR^3*Rz^3);
h54=-zk*yk/(RR^2*Rz);
h55=(-rrf*RR^2*Rz-2*(zkf*RR-zk*rrf)*zk*Rz)/(RR^3*Rz^3);
h56=RR/Rz^2;
H2=[%h11 0 h13 0 h15 0;
h21 0 h23 0 0 0;
h31 0 h33 0 h35 0;
h41 h42 h43 h44 0 0;
h51 h52 h53 h54 h55 h56];
K=Ppre*H2'*(H2*Ppre*H2'+R2)^(-1);
P2(:,:,i+1)=Ppre-K*H2*Ppre;
Hest=[%(xk^2+yk^2+zk^2)^0.5;
atan2(xk,yk);
atan(zk/(xk^2+yk^2)^0.5);
(yk*xkf-xk*ykf)/(xk^2+yk^2);
% (zkf*Rz^2-zk*(xkf+ykf))/(RR*Rz)];
(zkf*RR-zk*rrf)/(RR*Rz)];
%
%-------------对数据进行判断--------------
if (ZRadar(1,1,i+1)==0)&(ZRadar(2,1,i+1)==0)&(ZRadar(3,1,i+1)==0)&(ZRadar(4,1,i+1)==0) %如果数据不存在 则进行推测
X2(:,:,i+1)=Xest;
else
X2(:,:,i+1)=Xest+K*(ZRadar(:,:,i+1)-Hest);
end
%------------------------
end
Zfilt=zeros(4,1,times);
for i=1:times
%模型观测距离
% range=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;
%模拟观测方位角度
azimuth=atan(X2(1,1,i)/X2(3,1,i)); %%%%% 这里采用的是孙忠康的雷达数据处理中323页的公式 注意其方位角的公式与一般不同!!!!!!!
%
if azimuth>2*pi
azimuth=azimuth-2*pi;
else if azimuth < 0
azimuth=azimuth+2*pi;
end;
end;
%观测俯仰角度
pitching=atan(X2(5,1,i)/(X2(1,1,i)^2+X2(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=-(X2(3,1,i)*X2(2,1,i)-X2(1,1,i)*X2(4,1,i))/(X2(1,1,i)^2+X2(3,1,i)^2) ;
%观测俯仰角角速度
R_Rs=(X2(1,1,i)^2+X2(3,1,i)^2)^0.5;
R_R=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;
R_V=( X2(1,1,i)*X2(2,1,i)+X2(3,1,i)*X2(4,1,i)+X2(5,1,i)*X2(6,1,i) )/R_R;
pitching_w=( X2(6,1,i)*R_R-X2(5,1,i)*R_V)/(R_R*R_Rs) ;
Zfilt(:,:,i)=[%range;
azimuth;
pitching;
azimuth_w;
pitching_w];
end;