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;