Частичный фильтр -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

Answer 1

Независимо от того, что вы должны векторизовать этот код.Вы получите огромный удар по производительности, используя эти вложенные циклы для .В общем, в интерпретируемом языке, таком как Matlab, вы никогда не должны использовать команду для , если вам это абсолютно не нужно.Попробуйте что-то вроде этого:

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;