交互多模型的滤波器

为交互多模型的滤波器设计，采用当前统计模型，跟踪效果比较理想

clc;
close all;
%%%%%%%%%%%%%%%仿真目标真实航迹%%%%%%%%%%%%%%%
npts=2500;
sigma_r=100;
sigma_theta=1.7*pi/180;
vx0=0.8*340*cos（60*pi/180）;
vy0=0.8*340*sin（60*pi/180）;
XXX（:1） = [0 vx0 0 12000 vy0 0]‘;
g=10;
Pmj=3.5*g;%%%%%%火箭助推器推力
alphf=0.0/180*pi;%%%%%%%%%%%目标攻击角
lou=1.225;
beta=1.422e-004;
S=8.5;%%%%%%%%%%%%%%目试当前统计模型算法程序
%标参考面积
Cx=0.02;%%%%%阻力系数
Cy=0.025;%%%%%升力系数
Rr=6371004;%%%%%%%%%%%%地球半径
ts=10;
T=1;%%%%%%%%%%%采样时间间隔
t=0:ts:2500;%%%%%%%%%%%%爬升段飞行时间
N=length（t）;%%%%%采样点数
Hm=29.00*1000;%%%%%%%%%%爬升段末点高度，即为巡航高度
V=zeros（1N）;
h=zeros（1N）;
X=zeros（1N）;
Y=zeros（1N）;
Wd=zeros（1N）;
Jd=zeros（1N）;
Pj=zeros（1N）;
Mws=zeros（1N）;
sigma=zeros（1N）;
midu（1）=lou*exp（-beta*12000）;%%%%%初始大气密度
V（1）=0.8*340;%%%%%%%%目标的初始速度
h（1）=12000;%%%%%%%%目标初始高度
X（1）=0.5*midu（1）*V（1）^2*S*Cx;%%%%%%%%%%%%目标的初始阻力
Y（1）=0.5*midu（1）*V（1）^2*S*Cy;%%%%%目标的初始升力
Wd（1）=52.7*pi/180;%%%%%%%%%目标初始纬度
Jd（1）=174.1*pi/180;%%%%%%%目标初始经度
Pj（1）=-85.0/180*pi;%%%%%%%%%%%%目标航迹的初始方位角
Mws（1）=3510;%%%%%%%%%%%%%目标的总质量（目标本身加助推器加燃料等等）
sigma=60/180*pi;%%%%%%%%%%%%%%%目标的轨道倾角
for i=1:N
    V（i+1）=V（i）+（Pmj*Mws（i）*cos（alphf）-X（i）-Mws（i）*g*sin（sigma））/Mws（i）*ts;
%     sigma（i+1）=sigma（i）-（sigma（1）*V（i）/（Hm-h（1））*sin（sigma（i）））*ts;
    h（i+1）=h（i）+V（i）*sin（sigma）*ts;
    midu（i+1）=lou*exp（-beta*h（i））;
     X（i+1）=0.5*midu（i）*V（i）^2*S*Cx;%%%%%%%%%%%%阻力
     Pj（i+1）=Pj（i）+V（i）*tan（Wd（i））*cos（sigma）*sin（Pj（i））/（Rr+h（i））*ts;
    Jd（i+1）=Jd（i）+V（i）*cos（sigma）*sin（Pj（i））/cos（Wd（i））/（Rr+h（i））*ts;
    Wd（i+1）=Wd（i）+V（i）*cos（sigma）*cos（Pj（i））/（Rr+h（i））*ts;
    Mws（i+1）=Mws（1）-（h（i）-h（1））/（Hm-h（1））*1785;
XXX（2i+1）=V（i）*cos（60*pi/180）;
XXX（1i+1）=XXX（1i）+XXX（2i+1）*ts;
XXX（4i+1）=h（i+1）; 
    if h（i+1）>=Hm
        break;
    end 
end
i

% V（i）
% figure（5）
% plot（V）
% plot（Jd.*180./piWd.*180./pi）
N1=i;
Prj=0.5*g;
Mws（i）=1387;
Irjs=4000;
Cx1=0.007;
S1=2.5;
h1=h（i）;
midu1=lou*exp（-beta*h1）;
sigma1=0;
vm=7.0*340;
for i=N1:N
    V（i+1）=V（i）+（Prj*Mws（i）*cos（alphf）-X（i）-Mws（i）*g*sin（sigma1））./Mws（i）*ts;
    h（i+1）=h（i）;
     X（i+1）=0.5*midu1*V（i）^2*S1*Cx1;%%%%%%%%%%%%阻力
     Pj（i+1）=Pj（i）+V（i）*tan（Wd（i））*sin（Pj（i））./（Rr+h1）*ts;
    Jd（i+1）=Jd（i）+V（i）*sin（Pj（i））./cos（Wd（i））./（Rr+h1）*ts;
    Wd（i+1）=Wd（i）+V（i）*cos（Pj（i））./（Rr+h1）*ts;
    Mws（i+1）=Mws（i）-Prj*Mws（i）./Irjs*ts;
XXX（2i+1）=V（i）;
XXX（1i+1）=XXX（1i）+XXX（2i+1）*ts;
XXX（4i+1）=h（i+1）;
    if V（i+1）>=vm
        break;
    end
end

i

N2=i;
V1=V（i）;
P=X（i）/cos（alphf）;
Wmin=560
for i=N2:N
    V（i+1）=V（i）;
    h（i+1）=h（i）;
     Pj（i+1）=Pj（i）+V1*tan（Wd（i））*sin（Pj（i））/（Rr+h1）*ts;
    Jd（i+1）=Jd（i）+V1*sin（Pj（i））/cos（Wd（i））/（Rr+h1）*ts;
    Wd（i+1）=Wd（i）+V1*cos（Pj（i））/（Rr+h1）*ts;
    Mws（i+1）=Mws（i）-P/Irjs*ts;
XXX（2i+1）=V（i）;
XXX（1i+1）=XXX（1i）+XXX（2i+1）*ts;
XXX（4i+1）=h（i+1）;
    if Mws（i+1）<=Wmin
        break;
    end
end
i

N3=i;
Cx2=0.01;%%%%%阻力系数
Cy2=0.0023;%%%%%升力系数
midu（i）=lou*exp（-beta*h（i））;%%%%%初始大气密度
X（i）=0.5*midu（i）*V（i）^2*S1*Cx2;%%%%%%%%%%%%目标

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-08-01 16:23  15 交互多模型（CS）\
     文件        9027  2012-08-01 17:01  15 交互多模型（CS）\CS_IMMnew.m