IMM算法卡尔曼滤波matlab仿真

clear;
clc;

tic

N=400;
x=[60000-17240000246]‘;
v=sqrt（x（2）^2+x（4）^2）;
T=1;
g=9.8;
f_cv=[1T00;
      0100;
      001T;
      0001];
syms w;
f_tr=[1sin（w*T）/w0（-1+cos（w*T））/w;
      0cos（w*T）0-sin（w*T）;
      0（1-cos（w*T））/w1sin（w*T）/w;
      0sin（w*T）0cos（w*T）];
%% 真实轨迹
x_real=zeros（4N）;
x_real（:1）=x;
for i=2:55
    x_real（:i）=f_cv*x_real（:i-1）;
end
for i=56:150
    a=g;
    w=a/v;
    x_real（:i）=eval（f_tr）*x_real（:i-1）;
end
for i=151:181
     x_real（:i）=f_cv*x_real（:i-1）;
end
for i=182:245
    a=-1.5*g;
    w=a/v;
    x_real（:i）=eval（f_tr）*x_real（:i-1）;
end
for i=246:284
     x_real（:i）=f_cv*x_real（:i-1）;
end
for i=285:314
    a=3*g;
    w=a/v;
    x_real（:i）=eval（f_tr）*x_real（:i-1）;
end
for i=315:344
     x_real（:i）=f_cv*x_real（:i-1）;
end
for i=345:379
    a=-2.5*g;
    w=a/v;
    x_real（:i）=eval（f_tr）*x_real（:i-1）;
end
for i=380:400
     x_real（:i）=f_cv*x_real（:i-1）;
end
%plot（x_real（1:）x_real（3:））
%% 滤波器初始化

M=25;
xMSE=zeros（MN-2）;
yMSE=zeros（MN-2）;
eMSE=zeros（MN）;
xGJ=zeros（MN-2）;
yGJ=zeros（MN-2）;
for m=1:M           %蒙特卡洛
    
deltax=100;
deltay=100;
zx=x_real（1:）+deltax*randn（1N）;
zy=x_real（3:）+deltax*randn（1N）;
z=[zx;zy];
x2=[zx（2）zx（2）-zx（1）zy（2）zy（2）-zy（1）];
p2=[deltax^2deltax^200;
    deltax^22*deltax^200;
    00deltay^2deltay^2;
    00deltay^22*deltay^2];
niu=[0.030.030.030.920.030.030.03];
pai=[0.90.100000;
    0.10.80.10000;
    00.10.80.1000;
    000.10.80.100;
    0000.10.80.10;
    00000.10.80.1;
    000000.10.9];
tao=[T^2/20;
    T0;
    0T^2/2;
    0T];
C=[1000;
   0010];
R=[100^20;
   0100^2];
qcv=1.8^2*eye（22）;
qtr=2.5^2*eye（22）;
xesti_i=zeros（47N）;
for i=1:7
    xesti_i（:i2）=x2;
end
pesti_i=zeros（447N）;
for i=1:7
pesti_i（::i2）=p2;
end

%% KF

    
xesti_0j=zeros（47）;
x_esti=zeros（4N）;  %存放全局估计
p0j=zeros（447）;
for k=3:N
    for i=1:7       %归一化常数c
        c（i）=0;