Newer
Older
abgabensammlungSS15 / mr / ub9 / loc_framework / pf_prediction.m
@MaxXximus92 MaxXximus92 on 26 Jun 2015 1 KB mr ue 9 A2 fertig
function [ xnew ] = pf_prediction( x, u, R, motion)
%PF_PREDICTION Particle Filter prediction step

% Number of particles:
m = size(x, 3);

% YOUR CODE STARTS HERE:


% Pay attention to the representation of particles in pf_x: 
% A trajectory is stored as a 2D matrix [x_0, x_1, x_2, x_3]
% Columns: different time steps
% Trajectories of all particles are concatenated along a third dimension,
% the result is a 3D tensor.
% The pose (3x1 vector) of particle m at timestep i can be accessed using:
%    pf_x(:,i,m);

%xl=x(:,1,1)
%r=mvnrnd(zeros(size(R,1)),R)
%r2= r(:,1)
%x(:,1,1)= xl+motion(xl,u)+r2

%todo neue matrix mit einem zeitschritt mehr

% x = horzcat(x, zeros(size(x,1),1,size(x,3))); steht halt echt nirgends
% dass ihr erst danach concatenated
% size(x)    
%  j =size(x,2)-1;
%    for k=1: size(x,3)
%        xl=x(:,j,k);
%        r=mvnrnd(zeros(size(R,1)),R);
%        r2= r(:,1);
%        x(:,j+1,k)= xl+motion(xl,u)+r2;
%    end
% 
% xnew=x;

   for k=1: size(x,3)
       xl=x(:,1,k);
       r= mvnrnd(zeros(1,size(R,2)), R)';
       x(:,1,k)=motion(xl,u)+r;
   end
 
xnew=x;


% YOUR CODE ENDS HERE:
    
end