source.m

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

function [Z,Z1,XTrue1,totaltime,T0]=source;
% function [XTrue,Z, Z2,T,Q,DeltaR,DeltaSita,DeltaBeta,DeltaR2,DeltaSita2,DeltaBeta2,totaltime,montimes]=MYInit;
%设置初始值
montimes=50;      %蒙特卡罗仿真次数
% T0=10;               %采样周期 1
%  T1=60;                %采样周期 2
% T=60;
 T0=0.05;               %采样周期 1


%totaltime=2400;         %匀速运动阶段时间
totaltime=20;         %匀速运动阶段时间
%初始距离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方向速度


%采样率为2HZ的雷达的初始误差
DeltaR1=3000;               %观测距离误差标准差
DeltaSita1=1.0*pi/180;    %观测方位角角度误差标准差    此例中方位角和俯仰角的标准差应相等
DeltaBeta1=1*pi/180;    %观测俯仰角角度误差标准差   

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

%－－－－－－－－计算目标在雷达1(采样间隔为10HZ)中的真实轨迹,初始化XTrue1－－－－－－－－－－－－－－－－－－－－
times1 = totaltime/T0+1;          %采样点数

XTrue1=zeros(6,1,times1);

%真实轨迹作直线运动
for i=1:times1
    XTrue1(:,:,i)=[Xstart + vx*(i-1)*T0;
                      vx;
                  Ystart + vy*(i-1)*T0;
                      vy;
                  Zstart + vz*(i-1)*T0;
                      vz;];
end;


%－－－－－－－－极坐标下的模拟观测值，观测噪声距离方向上标准差DeltaR米，角度方位差为DeltaSita,DeltaBeta－－－－－－－－－－
Z1=zeros(3,1,times1);
for i=1:times1
    %模型观测距离
    i
    range=(XTrue1(1,1,i)^2+XTrue1(3,1,i)^2+XTrue1(5,1,i)^2)^0.5+randn(1)*DeltaR1;

      
    %模拟观测方位角度 
    azimuth=atan(XTrue1(3,1,i)/XTrue1(1,1,i))+randn(1)*DeltaSita1;

%      
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(XTrue1(5,1,i)/(XTrue1(1,1,i)^2+XTrue1(3,1,i)^2)^0.5)+randn(1)*DeltaBeta1;
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z1(:,:,i)=[range;
              azimuth;
              pitching];
    
          
    %%%%%%增加几个野值以检验野值判断处理算法的正确性
%     Z1(2,1,189)=1.2;    Z1(2,1,18)=0.9;    Z1(2,1,54)=1.2;    Z1(2,1,34)=1.7;    Z1(2,1,89)=1.7;  Z1(2,1,389)=2.2;  %方位角度上的 
      Z1(1,1,189)=105000;    Z1(1,1,18)=86009;    Z1(1,1,54)=70000;    Z1(1,1,34)=69543;    Z1(1,1,89)=76765;  Z1(1,1,389)=35432;   % 距离上的 
end;
%－－－－－－－－极坐标下的模拟观测值，观测噪声距离方向上标准差DeltaR米，角度方位差为DeltaSita,DeltaBeta－－－－－－－－－－
Z=zeros(3,1,times1);
for i=1:times1
    %模型观测距离
    
 
      range=(XTrue1(1,1,i)^2+XTrue1(3,1,i)^2+XTrue1(5,1,i)^2)^0.5;
      
    %模拟观测方位角度 
 
    azimuth=atan(XTrue1(3,1,i)/XTrue1(1,1,i));
%      
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
   
  pitching=atan(XTrue1(5,1,i)/(XTrue1(1,1,i)^2+XTrue1(3,1,i)^2)^0.5);
%     
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z(:,:,i)=[range;
              azimuth;
              pitching];
          
end;