LIKELIHOOD param: target - the target object param: particles - a MxN matrix of (predicted) particles where each row of the matrix represents the values of the N attributes of one of the M particles. param: observations - an array of observation structs return: l - weights for each particle Calculates the probability for every particle given observations. If there is more than one observation the product of all likelihoods will be returned. We are using an observation model that is similar to that of Lichtenauer and Reinders. Original source code by Hedvig Sidenbladh (January 2004). AUTHOR Ronnie Johansson CREATED 2004-12-09 ALTERED 2004-12-12 - minor correction ALTERED 2004-12-17 - added sensors to parameter list ALTERED 2004-12-20 - minor correction ALTERED 2004-12-21 - minor corrections ALTERED 2005-02-02 - minor correction in l(diff_big)
0001 function l = likelihood( target, particles, observations, services, ... 0002 sensors); 0003 % LIKELIHOOD 0004 % param: target - the target object 0005 % param: particles - a MxN matrix of (predicted) particles where each row of the matrix represents the values of the N attributes of one of the M particles. 0006 % param: observations - an array of observation structs 0007 % return: l - weights for each particle 0008 % 0009 % Calculates the probability for every particle given observations. 0010 % If there is more than one observation the product of all likelihoods will be returned. We are using an observation model that is similar to that of Lichtenauer and Reinders. 0011 % 0012 % Original source code by Hedvig Sidenbladh (January 2004). 0013 % AUTHOR Ronnie Johansson 0014 % CREATED 2004-12-09 0015 % ALTERED 2004-12-12 - minor correction 0016 % ALTERED 2004-12-17 - added sensors to parameter list 0017 % ALTERED 2004-12-20 - minor correction 0018 % ALTERED 2004-12-21 - minor corrections 0019 % ALTERED 2005-02-02 - minor correction in l(diff_big) 0020 % 0021 0022 [no_of_particles dummy] = size( particles ); 0023 0024 sigma = 5; % standard deviation for the normal distribution used in the likelihood calculation 0025 0026 l = ones(no_of_particles,1); 0027 0028 % get all relevant observations 0029 obs_idx = structfind( observations, 'target_id', target.id ); 0030 0031 relevant_obs = observations(obs_idx); 0032 noOfObs = length(relevant_obs); 0033 0034 if noOfObs < 1 0035 l = zeros(no_of_particles,1); 0036 end 0037 0038 epsilon = 0.1; % the minimum likelihood level 0039 0040 % for all relevant observations 0041 for o=1:noOfObs 0042 0043 sensor_id = relevant_obs(o).sensor_id; 0044 %sensor_id 0045 service_idx = find_sensor_services( sensors(structfind(sensors, ... 0046 'id', sensor_id)), ... 0047 services ); 0048 % note! In the future service_idx could be more than one 0049 0050 service = services( service_idx ); 0051 accuracy = service.quality.accuracy; 0052 0053 if ~strcmp( relevant_obs(o).data_format, 'position' ) 0054 disp('Error in data format: position expected!'); 0055 end 0056 0057 obs_pos = relevant_obs(o).data; 0058 particle_pos = particles(:,1:2); % get the positions of all particles 0059 % diff_pos = particle_pos - ones( no_of_particles, 1 ) * obs_pos; 0060 diff_dist = euclidean_distance_MX( particle_pos, ones( no_of_particles, 1) * obs_pos ); 0061 0062 % differentiate between those particles that are close to the observation 0063 % (diff_small) and those that are further away (diff_big). 0064 %diff_dist 0065 diff_small = find(diff_dist <= accuracy); 0066 diff_big = find(diff_dist > accuracy); 0067 0068 if ~isempty( diff_small ) 0069 l(diff_small) = l(diff_small) .* (1 + epsilon); 0070 end 0071 0072 if ~isempty( diff_big ) 0073 l(diff_big) = l(diff_big) .* (epsilon + exp( -(diff_dist( diff_big ) - accuracy).^2/(2*sigma^2) )); 0074 end 0075 0076 end 0077 0078 0079 0080