%?二維直線運動模型:
%X=FX+V?狀態模型
%Z=[z1;z2]?觀測模型
clc;
clear?all;
%%
N1=300;
time=300;
x_state（1）=400;
vx（1）=40;
y_state（1）=4800;
vy=-30;
%%Process?Noise?Covariance%%%%%%%%
xstate_noise=10;
Vx_noise=10;
%%Measurement?Noise?Covariance%%%%
theta_noise=3/180*pi;
distance1_noise=20;

%%Ture?State%%%%%%%%
for?i=2:time
????%%?State?model%%%%%%%%%%%
????x_state（i）=x_state（i-1）+vx（i-1）+xstate_noise*randn（1）;
????vx（i）=vx（i-1）+Vx_noise*randn（1）;
????y_state（i）=y_state（i-1）+vy+xstate_noise*randn（1）;;
????vy=vy+Vx_noise*randn（1）;
end
%%Measurement?Value%%%%%
for?i=1:time????
????%%Measure?model%%%%%%%%%
????distance1（i）=sqrt（x_state（i）^2+y_state（i）^2）+distance1_noise*randn（1）;
????theta1（i）=atan（y_state（i）/x_state（i））+theta_noise*randn（1）;
end

%%%Particle?Filtering%%%%%%%%%%%%%%
x_pf（1）=400;vx_pf（1）=40;
y_pf（1）=4800;vy_pf（1）=-30;
xp1=zeros（1N1）;xp2=zeros（1N1）;xp3=zeros（1N1）;xp4=zeros（1N1）;
%%%%%Initial?particles%%%%%%%%
for?n=1:N1;
????%M1=[delta1*randn（1）delta2*randn（1）delta3*randn（1）delta4*randn（1）];
????%M1=diag（M1）;
????xp1（n）=x_pf（1）+xstate_noise*randn（1）;
????xp2（n）=vx_pf（1）+Vx_noise*randn（1）;
????xp3（n）=y_pf（1）+xstate_noise*randn（1）;
????xp4（n）=vy_pf（1）+Vx_noise*randn（1）;
end

%**filter?process***?angel?and?distance****************
for?t=2:time
????%%%Prediction?Process%%%%
??