replaceWithObservations

 REPLACEWITHOBSERVATIONS - replace some of the particles with observations
 REPLACEWITHOBSERVATIONS( PARTICLES, WEIGHTS, TARGET, OBSERVATIONS, RATIO )

 param: particles - a MxN matrix of M particles for a tracked target
 param: weights - a vector of likelihoods weights (M elements)
 param: target - the target being followed
 param: observations - observations of the target in question (which the particles and weights refer to). NOTE! This function will check if the observations refer to the target of the particle filter, hence only supply relevant observations
 param: ratio - the share of the particles to replace (between 0 and 1). Both 0 and 1 and values very close to them are poor choices.
 return: the MxN set of particles, where some have been replaced. 

 AUTHOR  Ronnie Johansson
 CREATED 2004-12-21
 ALTERED 

 NOTE! This function has not been thoroughly tested

0001 function [new_particles, new_weights] = replaceWithObservations( particles, weights, target, observations, ratio )
0002 % REPLACEWITHOBSERVATIONS - replace some of the particles with observations
0003 % REPLACEWITHOBSERVATIONS( PARTICLES, WEIGHTS, TARGET, OBSERVATIONS, RATIO )
0004 %
0005 % param: particles - a MxN matrix of M particles for a tracked target
0006 % param: weights - a vector of likelihoods weights (M elements)
0007 % param: target - the target being followed
0008 % param: observations - observations of the target in question (which the particles and weights refer to). NOTE! This function will check if the observations refer to the target of the particle filter, hence only supply relevant observations
0009 % param: ratio - the share of the particles to replace (between 0 and 1). Both 0 and 1 and values very close to them are poor choices.
0010 % return: the MxN set of particles, where some have been replaced.
0011 %
0012 % AUTHOR  Ronnie Johansson
0013 % CREATED 2004-12-21
0014 % ALTERED
0015 %
0016 % NOTE! This function has not been thoroughly tested
0017 
0018 disp('Entering replaceWithObservations');
0019 
0020 
0021 new_particles = [];
0022 
0023 [M N] = size(particles);
0024 % check that indata sizes agree
0025 if (M ~= length(weights))
0026      disp('replaceWithObservartions: number of particles and weights do not agree!');
0027   return;
0028 end
0029 
0030 no_of_particles_to_replace = ceil( M*ratio )
0031 
0032 no_of_observations = length( observations );
0033 
0034 % permute observations
0035 
0036 observations = observations( randperm( no_of_observations ) );
0037 particles_per_observation = ceil(no_of_particles_to_replace/no_of_observations);
0038 
0039 replaced = 0; % the number of particles replaced
0040 
0041 % remove the worst particles;
0042 [weights indices] = sort(weights); % the worst alternatives are first
0043 
0044 new_particles = particles( indices(no_of_particles_to_replace+1:end), : )
0045 new_weights = weights(indices(no_of_particles_to_replace+1:end));
0046 
0047 obs = 1;
0048 while (replaced < no_of_particles_to_replace)
0049   replace = min( particles_per_observation, no_of_particles_to_replace - replaced );
0050   for i=1:replace
0051     pos = observations( obs ).data;
0052     if length(pos) < 2
0053       disp('replaceWithObservations: pos malformed');
0054       pos
0055     end
0056     speed = target.max_speed*rand;
0057     angle = 2*pi*rand;
0058     pos
0059     speed 
0060     angle
0061     new_particles
0062     new_particles = [[pos speed angle]; new_particles]
0063     new_weights = [1.1; new_weights]
0064   end
0065   replaced = replaced + replace; 
0066   obs = obs+1; % next observation
0067 end
0068 
0069 [M2 N2] = size(new_particles);
0070 
0071 if (M2 ~= M)
0072   disp('replaceWithObservations: the new set of particles does not have the right size.')
0073 end
0074 
0075 disp('Leaving replaceWithObservations');