Newer
Older
abgabensammlungSS15 / mr / ub9 / loc_framework / pf_prediction.m
@MaxXximus92 MaxXximus92 on 24 Jun 2015 916 bytes mr
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)));
%size(x,2)    
 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;

% YOUR CODE ENDS HERE:
    
end