clear;
clc;
close?all;

T=2;
N=900/T;
N1=400/T;
N2=600/T;
N3=610/T;
N4=660/T;

x=zeros（N1）;
y=zeros（N1）;
zx=zeros（N1）;
zy=zeros（N1）;

M?=?100;?%?仿真次數(shù)

Delta0=10;
Delta1=150;
Delta2=150;
Delta12=2;


?
%產(chǎn)生真實(shí)軌跡
t=2:T:400;??%沿y軸勻速直線
x0=1000+0*t‘;
y0=12000-15*t‘;
t=402:T:600;??%慢轉(zhuǎn)彎
x1=x0（N1）+0.075*（（t‘-400）.^2）/2;
y1=y0（N1）-15*（t‘-400）+0.075*（（t‘-400）.^2）/2;
t=602:T:610??%勻速
vx=0.075*（600-400）;
x2=x1（N2-N1）+vx*（t‘-600）;
y2=y1（N2-N1）+0*t‘;
t=612:T:660??%快轉(zhuǎn)彎
x3=x2（N3-N2）+（vx*（t‘-610）-0.3*（（t‘-610）.^2）/2）;
y3=y2（N3-N2）-0.3*（（t‘-610）.^2）/2;
t=662:T:900??%勻速直線
vy=-0.3*（660-610）;
x4=x3（N4-N3）+0*t‘;
y4=y3（N4-N3）+vy*（t‘-660）;

x=[x0;x1;x2;x3;x4];?%真實(shí)軌跡數(shù)據(jù)
y=[y0;y1;y2;y3;y4];?%真實(shí)軌跡數(shù)據(jù)

re_x1=zeros（MN）;??%記錄每次仿真濾波估計(jì)數(shù)據(jù)
re_y1=zeros（MN）;
re_x2=zeros（MN）;??
re_y2=zeros（MN）;
re_x3=zeros（MN）;??%記錄每次仿真濾波估計(jì)數(shù)據(jù)
re_y3=zeros（MN）;
re_x4=zeros（MN）;??
re_y4=zeros（MN）;

for?m=1:M
????
%產(chǎn)生觀測數(shù)據(jù)
nx1=randn（N1）*Delta1;?%傳感器1觀測噪聲
ny1=randn（N1）*Delta1