myinit.m

文件包含有5项内容： 一、扩展卡尔曼滤波EKF 二、去偏转换卡尔曼滤波CMKF 三、最小二乘拟和的方法 四、最小二乘、EKF、CMKF的比较 五、野值剔除算法 用MATLAB实现了这些具体

%--------------------------------------------------------------------------
%XTrueZ 目标真实轨迹
%Z0 目标观测值
%T 采样周期
%Q 系统误差阵
%DeltaR 距离误差
%DeltaSita 方位角误差
%DeltaBeta 俯仰角误差
%totaltime 运动时间
%montimes 蒙特卡罗仿真次数
function [XTrue,Z,Z0,T,Q,DeltaR,DeltaSita,DeltaBeta,totaltime,montimes]=MYInit;
% function [XTrue,Z, Z2,T,Q,DeltaR,DeltaSita,DeltaBeta,DeltaR2,DeltaSita2,DeltaBeta2,totaltime,montimes]=MYInit;
%设置初始值
montimes=50;      %蒙特卡罗仿真次数
 T1=0.05;                %采样周期 2
 T=0.05;
%totaltime=2400;         %匀速运动阶段时间
totaltime=10;         %匀速运动阶段时间
%初始距离r0,sita0,beta0
r0=120000;            %目标初始斜距
sita0=pi/4;          %初始方位角
beta0=5*pi/180;          %初始俯仰角

Xstart=r0*cos(beta0)*cos(sita0);       %目标起始X坐标
Ystart=r0*cos(beta0)*sin(sita0);      %目标起始Y坐标
Zstart=r0*sin(beta0);                %目标起始Z坐标


vx=-800;             %X方向速度
vy=-230;          %Y方向速度
vz=0;             %Z方向速度


%采样率为1/3HZ的雷达的初始误差
DeltaR=3000;               %观测距离误差标准差
DeltaSita=1*pi/180;    %观测方位角角度误差标准差    此例对应红外
DeltaBeta=1*pi/180;    %观测俯仰角角度误差标准差   

% DeltaSita=1*pi/180;    %观测方位角角度误差标准差    此例对应雷达
% DeltaBeta=1*pi/180;    %观测俯仰角角度误差标准差   


q=0.007;                  % q是系统噪声标准差
Q=q^2*eye(3);              %系统各方向的状态噪声方差




% for i=1:times
% %    xt(i)=Z(1,1,i)*cos(Z(2,1,i))*cos(Z(3,1,i))-XTrue(1,1,i);
% %    yt(i)=Z(1,1,i)*sin(Z(2,1,i))*cos(Z(3,1,i))-XTrue(3,1,i);
% %    zt(i)=Z(1,1,i)*sin(Z(3,1,i))-XTrue(5,1,i);
%    xt(i)=Z(1,1,i)*cos(Z(2,1,i))*cos(Z(3,1,i));
%    yt(i)=Z(1,1,i)*sin(Z(2,1,i))*cos(Z(3,1,i));
%    zt(i)=Z(1,1,i)*sin(Z(3,1,i));
% end;
% plot(xt,zt);    

%－－－－－－－－计算目标在雷达2(采样间隔为40HZ)中的真实轨迹,初始化XTrue2－－－－－－－－－－－－－－－－－－－－
times2 = totaltime/T1+1;          %采样点数

XTrue2=zeros(6,1,times2);


XTrue=zeros(6,1,times2);       %%%%%
%真实轨迹作直线运动
for ii=1:times2
    XTrue2(:,:,ii)=[Xstart + vx*(ii-1)*T1;
                      vx;
                  Ystart + vy*(ii-1)*T1;
                      vy;
                  Zstart + vz*(ii-1)*T1;
                      vz;];
                XTrue(:,:,ii)= XTrue2(:,:,ii) ;  
end;



%－－－－－－－－极坐标下的模拟观测值，观测噪声距离方向上标准差DeltaR米，角度方位差为DeltaSita,DeltaBeta－－－－－－－－－－
Z=zeros(3,1,times2);
for ii=1:times2
    %模型观测距离
    range=(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2+XTrue2(5,1,ii)^2)^0.5+randn(1)*DeltaR;
    %模拟观测方位角度 
    azimuth=atan2(XTrue2(3,1,ii),XTrue2(1,1,ii))+randn(1)*DeltaSita;
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(XTrue2(5,1,ii)/(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2)^0.5)+randn(1)*DeltaBeta;
    
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z(:,:,ii)=[range;
              azimuth;
              pitching];
end;

% %－－－－－－－－计算目标在雷达1的真实轨迹,初始化XTrue－－－－－－－－－－－－－－－－－－－－
Z0=zeros(3,1,times2);
for ii=1:times2
    %模型观测距离
    range=(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2+XTrue2(5,1,ii)^2)^0.5;
    %模拟观测方位角度 
    azimuth=atan2(XTrue2(3,1,ii),XTrue2(1,1,ii));
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(XTrue2(5,1,ii)/(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2)^0.5);
    
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z0(:,:,ii)=[range;
              azimuth;
              pitching];
end;