sensorPathPlanning

 Makes path planinging of the sensor based on current_sensor_position and
 predicted_particles postions
 in this version - sample path nodes randomly
 altered: rj - 050120 - minor change to the assignment of sensor_plan
 ALTERED RJ 050125 - added last_observed argument

0001 
0002 function [sensor_plan] = sensorPathPlanning( current_sensor_position, ...
0003                                              predicted_particles, ...
0004                                              last_observed, noOfPathNodes )
0005 % Makes path planinging of the sensor based on current_sensor_position and
0006 % predicted_particles postions
0007 % in this version - sample path nodes randomly
0008 % altered: rj - 050120 - minor change to the assignment of sensor_plan
0009 % ALTERED RJ 050125 - added last_observed argument
0010 
0011 sensor_plan = [];
0012 %bootstrap  noOfPathNodes
0013 % idx  index of the particles that become path nodes
0014  Idx = round(1 + (length(predicted_particles(:,1)) - 1)*rand(noOfPathNodes,1));
0015  PathNodes = predicted_particles(Idx,:);
0016  distanceSensorParticle = [];
0017  for i = 1:length(PathNodes(:,1))
0018       distanceSensorParticle = [distanceSensorParticle; sqrt((current_sensor_position(1) - PathNodes(i,1))^2 + (current_sensor_position(2) - PathNodes(i,2))^2)];
0019  end;
0020  while not(isempty(distanceSensorParticle)) 
0021      [value, idx] = min(distanceSensorParticle);
0022      %idx
0023      sensor_plan = [sensor_plan; PathNodes(idx, 1:2)];
0024      distanceSensorParticle(idx) = [];  
0025  end;
0026 % finally add the last observed position
0027 sensor_plan = [sensor_plan; last_observed]; 
0028  
0029  
0030  
0031  
0032  
0033 
0034 
0035 
0036 
0037