PARTICLE_FILTER - predict and update the filter given the particles of the target and new observations
param: target - a platoon struct
param: observations - an array of observations, possibly empty []
param: services - an array of services
param: resources - an array of own forces
param: no_of_timesteps - the number of timesteps to predict for
param: mode - Either 'prognosis' (only prediction no update) or
'update' (both prediction and update). The 'update' mode is used
for ordinary usage, 'prognosis' when you just want get a
forecast for no_of_timesteps into the future.
return: target with updated particles
This code is based on particle filter source code by Hedvig Sidenbladh. (January 2004)
AUTHOR Ronnie Johansson
CREATED 2004-12-09
ALTERED 2004-12-12 - made a special case for situations when
there are no observations
ALTERED 2004-12-17 - likelihood was called even for targets that
had not been detected
ALTERED 2004-12-21 - added replacement of particles when maximum likelihood
for particles gets too low
ALTERED 2005-01-05 - added mode parameter
ALTERED 2005-01-26 - added resources parameter0001 function target = particle_filter( target, observations, services, ... 0002 sensors, resources, no_of_timesteps, mode ) 0003 % 0004 % PARTICLE_FILTER - predict and update the filter given the particles of the target and new observations 0005 % 0006 % param: target - a platoon struct 0007 % param: observations - an array of observations, possibly empty [] 0008 % param: services - an array of services 0009 % param: resources - an array of own forces 0010 % param: no_of_timesteps - the number of timesteps to predict for 0011 % param: mode - Either 'prognosis' (only prediction no update) or 0012 % 'update' (both prediction and update). The 'update' mode is used 0013 % for ordinary usage, 'prognosis' when you just want get a 0014 % forecast for no_of_timesteps into the future. 0015 % return: target with updated particles 0016 % 0017 % This code is based on particle filter source code by Hedvig Sidenbladh. (January 2004) 0018 % 0019 % AUTHOR Ronnie Johansson 0020 % CREATED 2004-12-09 0021 % ALTERED 2004-12-12 - made a special case for situations when 0022 % there are no observations 0023 % ALTERED 2004-12-17 - likelihood was called even for targets that 0024 % had not been detected 0025 % ALTERED 2004-12-21 - added replacement of particles when maximum likelihood 0026 % for particles gets too low 0027 % ALTERED 2005-01-05 - added mode parameter 0028 % ALTERED 2005-01-26 - added resources parameter 0029 % 0030 0031 %%%%%%%%%%%%%%%%% 0032 % CONSTANTS 0033 %%%%%%%%%%%%%%%%% 0034 0035 0036 % Number of particles defined elsewhere 0037 global noOfParticles; 0038 negative_obs_discount = 0.5; % discount factor for particles which 0039 % should have been observed but which wasn't' 0040 0041 % Prediction: propagate the particles from the former step to the next 0042 % The resulting distribution is the prior for the following timestep 0043 predicted_particles = motion( target, no_of_timesteps ); % connect to Robert's code ' 0044 0045 if strcmp( mode, 'update') == 1 0046 0047 obs_idx = structfind( observations, 'target_id', target.id ); 0048 obs = observations(obs_idx); 0049 noOfObs = length(obs); % no of observations for this target 0050 0051 % Handle the case with negative observations 0052 % weight particles according to negative (i.e., missing observations) 0053 0054 neg_obs_weights = negative_observations( predicted_particles, obs, ... 0055 target, sensors, resources, ... 0056 negative_obs_discount ); 0057 0058 %neg_obs_weights 0059 %length(neg_obs_weights) 0060 0061 if noOfObs > 0 0062 0063 % IF THERE ARE OBSERVATIONS 0064 0065 % update using likelihood weights from observations 0066 weights = likelihood( target, predicted_particles, observations, ... 0067 services, sensors ); 0068 0069 weights = weights.*neg_obs_weights; 0070 0071 replacement_ratio = 0.4; % the share of the particles to replace, e.g., 10% 0072 0073 if max( weights ) < 0.75^noOfObs 0074 % if even the most likely particle is quite unlikely ... 0075 % ... remove some of the particles and exchange with some 0076 % particles which have the same position as the observations 0077 [predicted_particles weights] = replaceWithObservations( predicted_particles, weights, target, obs, replacement_ratio ); 0078 end 0079 0080 % Montecarlo sample (draw new particles with replacement) according to 0081 % likelihood - particles with low weight are less likely drawn 0082 % and particles with high weight are drawn with a higher probability 0083 cum = cumsum( weights / sum(weights) ); 0084 seeds = rand( noOfParticles, 1 ); 0085 sel_ind = montecarlo( cum, seeds ); 0086 updated_particles = predicted_particles( sel_ind, : ); 0087 target.particles = updated_particles; 0088 0089 else 0090 % IF THERE ARE NO OBSERVATIONS 0091 0092 % but negative observations 0093 if sum(neg_obs_weights) < noOfParticles % since each weight is one if 0094 % there are no neg obs 0095 weights = neg_obs_weights; 0096 cum = cumsum( weights / sum(weights) ); 0097 seeds = rand( noOfParticles, 1 ); 0098 sel_ind = montecarlo( cum, seeds ); 0099 updated_particles = predicted_particles( sel_ind, : ); 0100 target.particles = updated_particles; 0101 else 0102 % no negative observations 0103 % keep the predicted particles 0104 target.particles = predicted_particles; 0105 end 0106 0107 end 0108 % keep the predicted particles 0109 % disp(['Particles for: ' target.id]); 0110 % target.particles 0111 0112 elseif strcmp( mode, 'prognosis' ) 0113 target.particles = predicted_particles; 0114 else 0115 disp(['particle_filter: NOTE! Wrong mode: ' mode ' supplied']); 0116 end 0117 0118 0119 0120