我已经实现了粒子过滤器如下:
系统型号:
X=x+t*cos(theta)*V;
y=y+t*sin(theta)*V;
theta= theta+omega*t;
其中 V, omega 分别是速度和角速度。此外,观察结果包括距框左上角距离的噪声版本。
但是,我不确定我的代码是否正确。(粒子之间的距离正在增加),任何人都可以帮助我吗?
第二:我想在matlab中显示我想要跟踪的对象,但是我尝试了不同的方法,仍然不成功。你能帮我解决这部分的问题吗?
%#######################################################
clc;
clear all;
close all;
N=400; % numebr of Particles
T=100; % Time Steps
x0=zeros(1,N);
theta0=zeros(1,N);
y0=zeros(1,N);
v=5;
omega=pi/4;
%%
% x theta, y and Omega and V
particle=zeros(3,N);
w = ones(T,N); % Importance weights.
resamplingScheme=1;
for t=2:T
%% Prediction Steps
for p=1:N
v_noisy=v+rand*.5;
omega_nosiy=omega*.2;
particle(1,p)=x0(p)+t*v_noisy*cosd(theta0(p));
particle(2,p)=y0(p)+t*v_noisy*sind(theta0(p));
particle(3,p)=theta0(p)+omega_nosiy*t;
end
%% IMPORTANCE WEIGHTS:
for p=1:N
distance=sqrt( particle(1,p)^2+ particle(2,p)^2);
if distance< 4 || distance > 25
distance = .7;
else
distance=.3;
end
w(t,p) =distance;
end
w(t,:) = w(t,:)./sum(w(t,:)); % Normalise the weights.
%% SELECTION STEP:
if resamplingScheme == 1
outIndex = residualR(1:N,w(t,:)'); % Residual resampling.
elseif resamplingScheme == 2
outIndex = systematicR(1:N,w(t,:)'); % Systematic resampling.
else
outIndex = multinomialR(1:N,w(t,:)'); % Multinomial resampling.
end;
x0=particle(1,outIndex);
y0=particle(2,outIndex);
theta0=particle(3,outIndex);
clf;
hold on;
plot(x0,y0,'gx');
refresh;
drawnow;
end