粒子滤波 - Matlab

Question

我已按照以下方式实现了粒子滤波器：

系统模型：

X=x+t*cos(theta)*V; 
y=y+t*sin(theta)*V; 
theta= theta+omega*t;

其中V和ω分别表示速度和角速度。此外，观测结果包括距离盒子左上角的噪声版本。

然而，我不确定我的代码是否正确（粒子之间的距离正在增加），有没有人能帮我解决这个问题？

第二个问题：我想在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

- superMind

---

你确定residualR()函数没问题吗？ - macduff

好的，我全身心投入到这个问题中。我真的很喜欢这种东西，但是我需要一些帮助来理解算法，因为我更熟悉卡尔曼滤波器，如果你愿意@superMind。看起来重采样索引从不改变，因为归一化权重不会改变。 - macduff

我想我找到了问题所在，大多数问题是由于错误的观察模型造成的。 - superMind

好的，这是我的解决方案：

1. 对于预测模型，需要定义每个粒子的Vx和Vy而不仅仅是V。每当代理撞到墙壁时，只需反转速度即可。
2. 对于观察模型：应该像这样：d = sqrt((obs_x-x0)^2+(obs_y-y0)^2)) 和 ds = sqrt((xi-x0)^2+(yi-y0)^2)。其中xi和yi是粒子坐标，obs_x/y是观察值。x0和y0是我们想要找到代理和粒子之间距离的点。然后将其发送到N(d-ds,sigam)以计算权重。N是高斯分布。

- superMind

如果您解决了问题，请将解决方案作为单独的答案添加并接受它。这样，其他用户就会知道问题已经解决了。 - bla

Answer 1

无论如何，您都应该将此代码向量化。使用这些嵌套的for循环会导致巨大的性能损失。总的来说，在像Matlab这样的解释型语言中，除非您绝对必须使用for命令，否则不要使用它。尝试使用以下内容：

distance = sqrt(particle(1,:).^2 + particle(2,:).^2);
outOfBounds = distance < 4 | distance > 25; % note use of vectorized | operator instead of scalar || operator
w(t,outOfBounds) = 0.7;
w(t,~outOfBounds) = 0.3;

- David Graff