二维粒子滤波matlab x程序

% 二维直线运动模型:
%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%%%%