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;?%?仿真次數

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


?
%產生真實軌跡
t=2:T:400;??%沿y軸勻速直線
x0=1000+0*t‘;
y0=12000-15*t‘;
t=402:T:600;??%慢轉彎
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??%快轉彎
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];?%真實軌跡數據
y=[y0;y1;y2;y3;y4];?%真實軌跡數據

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

for?m=1:M
????
%產生觀測數據
nx1=randn（N1）*Delta1;?%傳感器1觀測噪聲
ny1=randn（N1）*Delta1