def _update_trans_matrix(self): """Updates transition matrix for time delta since last prediction Side Effects: self.state_trans -- updates velocity coefficients in position equations self.last_time_changed -- updates last time changed to reflect that state has changed self.delta_t -- updates delta between current time and last time changed (used for predict) """ self.delta_t = (time_in_millis() - self.last_time_changed) / 1000. # update delta_t in state transition matrix self.state_trans[0, 2] = self.delta_t self.state_trans[1, 3] = self.delta_t self.last_time_changed = time_in_millis()
def test_generate_obj_gate(self): """Tests generate obj gate method""" # generate test object parameters num_objects = 3 rng_list, bearing_list, obj_type_list = [5, 12, 40], [45, 0, -20], [ ObjectType.BOAT, ObjectType.NONE, ObjectType.BUOY ] obj_list = [0] * num_objects # initialize truthed gates truth_rng_gates = [0] * num_objects truth_bearing_gates = [0] * num_objects truth_type_gates = [0] * num_objects # create objects and set up truth gates for ii in range(num_objects): obj_list[ii] = Object(bearing_list[ii], rng_list[ii], time_in_millis(), objectType=obj_type_list[ii]) obj_list[ii].kalman.covar = np.eye(4) truth_rng_gates[ii] = (rng_list[ii] - 1, rng_list[ii] + 1) truth_bearing_gates[ii] = (bearing_list[ii] - 1, bearing_list[ii] + 1) truth_type_gates[ii] = (ObjectType.NONE, obj_type_list[ii]) # compare truth gates to generated gates truth_gates = [ *zip(truth_rng_gates, truth_bearing_gates, truth_type_gates) ] for jj, obj in enumerate(obj_list): gate = self.map._generate_obj_gate(obj) self.assertEqual(truth_gates[jj], gate)
def test_clear_objects(self): """Tests clear objects method of map""" # set up objects to add to list (arbitrary values) rng_list = [12.512, 44] bearing_list = [-22, 81.5] type_list = [ObjectType.BUOY, ObjectType.BOAT] # add objects to object list 0.05s apart start_time = dt.now() for ii, (rng, bearing, obj_type) in enumerate(zip(rng_list, bearing_list, type_list)): while (abs((dt.now() - start_time).total_seconds()) < .05): # while less than 0.05s since last object pass obj = Object(bearing, rng, time_in_millis(), objectType=obj_type) self.map.object_list.append(obj) start_time = dt.now() self.assertTrue(len( self.map.object_list) == 2) # assert that length of list is two while (abs(dt.now() - start_time).total_seconds() < .05): # while less than 0.05s since last object pass self.map.clear_objects( timeSinceLastSeen=75) # should only clear 2nd object self.assertTrue(len(self.map.object_list) == 1) # assert that length of list is only one self.map.clear_objects( timeSinceLastSeen=0) # should only clear all objects self.assertTrue(len( self.map.object_list) == 0) # assert that length of list is zero
def test_get_buoys(self): """Tests get buoys method""" # create objects to add to map rng_list = [12.512, 44, 50] bearing_list = [-22, 81.5, 2] type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY] # set up correct object list correct_object_list = [0] * 2 num_correct_objects = 0 # loop through objects and add to map for rng, bearing, obj_type in zip(rng_list, bearing_list, type_list): obj = Object(bearing, rng, time_in_millis(), objectType=obj_type) self.map.object_list.append(obj) # add object to correct object list if is buoy if obj_type == ObjectType.BUOY: correct_object_list[num_correct_objects] = [ rng, bearing, obj_type ] num_correct_objects += 1 # get list of objects from get_buoys returned_objects = self.map.get_buoys() # check if objects map correct object list for jj, obj in enumerate(correct_object_list): self.assertAlmostEqual(obj[0], returned_objects[jj][0]) self.assertAlmostEqual(obj[1], returned_objects[jj][1]) self.assertEqual(obj[2], returned_objects[jj][2])
def clear_objects(self, timeSinceLastSeen=0): """ Clears object from objects with greater than <timeSinceLastSeen> time since last seen Inputs: timeSinceLastSeen -- time since to exclude objects last seen time (in milliseconds) """ cur_time = time_in_millis() del_list = [] mutex.acquire() for ii, obj in enumerate(self.object_list): if (time_in_millis() - obj.lastSeen) > timeSinceLastSeen: del_list.append(ii) for index in sorted(del_list, reverse=True): del self.object_list[index] mutex.release()
def __init__(self, bearing, rng, lastSeen=time_in_millis(), rngRate=0, bearingRate=0, objectType=ObjectType.NONE): """ Initalizes object that tracks a detection in map bearing -- Relative angle of object (in deg) rng -- Range of object from boat (in m) lastSeen -- Time object was last seen (in ms) objectType -- Classification of object (None for unclassified object) rangeRate -- Velocity of object in radial direction (in m/s, + for object moving outwards) bearingRate -- Velocity of object in angular direction (in deg/s, + for object moving CCW) """ self.bearing = bearing self.rng = rng self.lastSeen = lastSeen self.objectType = objectType self.rngRate = rngRate self.bearingRate = bearingRate self.histLength = 10 self.updateHist = [ None ] * self.histLength # list to store if track updates for past <histLength> update cycles self.histScore = 0 # history score of object self.confidence = 0 # confidence score of obj self.prevRng = 0 self.prevBearing = 0 self.kalman = KalmanFilter(np.array([self.rng, self.bearing]), np.array([self.rngRate, self.bearingRate]))
def update(self, rng, bearing, rngRate=None, bearingRate=None): """Updates object position and model based on new reading Inputs: rng -- range measured by sensors bearing -- bearing measured by sensors rngRate -- rate of change of range bearingRate -- rate of change of bearing """ # rotate update history self.updateHist[1:self. histLength] = self.updateHist[0:self.histLength - 1] if (rng is None) and (bearing is None): self.updateHist[0] = 0 # not updated return # exit function self.updateHist[0] = 1 # updated if (rngRate is None) and (bearingRate is None): self.kalman.update([rng, bearing], [self.rngRate, self.bearingRate]) else: self.kalman.update([rng, bearing], [rngRate, bearingRate]) self._set_object_state() # update range and bearing rate for object self._find_object_rngRate() self._find_object_bearingRate() self.lastSeen = time_in_millis() self.prevRng = self.rng self.prevBearing = self.bearing # set hist score self._calc_hist_score()
def test_prune_objects(self): """Tests prune objects method""" # create objects to add to map rng_list = [12.512, 44, 50] bearing_list = [-22, 81.5, 2] type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY] update_hist_list = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1], \ [1, 0, 1, 0, 1, 0, 1, 0, 1, 1], \ [1, 0, 0, 1, 0, 0, 0, 0, 1, 0]] # loop through objects and add to map for ii, (rng, bearing, obj_type) in enumerate(zip(rng_list, bearing_list, type_list)): obj = Object(bearing, rng, time_in_millis(), objectType=obj_type) obj.updateHist = update_hist_list[ii] self.map.object_list.append(obj) # create local copy of original object list orig_object_list = self.map.object_list # call prune objects self.map.prune_objects() # create truth object list truth_obj_list = orig_object_list[0:2] # ensure that only the first two objects remain in the object list self.assertEqual(truth_obj_list, self.map.object_list)
def _find_object_bearingRate(self): """ Finds and sets object bearing rate Side Effects: self.bearingRate -- Updates bearing rate using new measurement """ self.bearingRate = 1000 * (self.bearing - self.prevBearing) / ( time_in_millis() - self.lastSeen)
def _find_object_rngRate(self): """ Finds and sets object range rate Side Effects: self.rngRate -- Updates range rate using new measurement """ self.rngRate = 1000 * (self.rng - self.prevRng) / (time_in_millis() - self.lastSeen)
def test_return_objects(self): """Tests return objects method of map""" # set up objects to add to map timeRange = 5000 # time used to create rngRange rngRange = (0, self.boat_speed * (timeRange / 1000) ) # range of ranges to return bearingRange = (-30, 30) # range of bearings to return type_list = [ ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY, ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY, ObjectType.NONE ] # object types # set up objects num_objects = 7 correct_object_list = [0] * num_objects # create empty object list num_correct_objects = 0 # counter for correct objects # loop over objects to create for n in range(num_objects): rng = (n * 6) + 3 # get range in range from 3 to 39 bearing = (n * 15) - 45 # get bearing in range from -45 to 45 # add object to correct object list if with rngRange and bearingRange if (rngRange[0] <= rng <= rngRange[1]) and ( bearingRange[0] <= bearing <= bearingRange[1]): correct_object_list[num_correct_objects] = [ rng, bearing, type_list[n] ] num_correct_objects += 1 # add object to map obj = Object(bearing, rng, time_in_millis(), objectType=type_list[n]) self.map.object_list.append(obj) # get list of objects meeting conditions returned_objects = self.map.return_objects(bearingRange, rngRange=rngRange) # check that objects match for jj, obj in enumerate(correct_object_list[0:num_correct_objects]): self.assertAlmostEqual(obj[0], returned_objects[jj][0]) self.assertAlmostEqual(obj[1], returned_objects[jj][1]) self.assertEqual(obj[2], returned_objects[jj][2])
def test_smooth_frame(self): """Tests smooth frame method""" # create objects to add to map rng_list = [12.512, 44, 50] bearing_list = [-22, 81.5, 2] type_list = [ObjectType.BUOY, ObjectType.BOAT, ObjectType.BUOY] # loop through objects and add to map for rng, bearing, obj_type in zip(rng_list, bearing_list, type_list): obj = Object(bearing, rng, time_in_millis(), objectType=obj_type) self.map.object_list.append(obj) # create epoch frame num_detects = 3 epoch_frame = [(12, -21, ObjectType.BUOY), (44, 80, ObjectType.BOAT), (100, 100, ObjectType.BUOY)] # create joint_pdaf return vals truth_update_vals = [(12, -21), (44, 80), None] truth_dets_used = [1, 1, 0] with patch('src.tracking.map.joint_pdaf') as mock_pdaf, \ patch('src.tracking.map.Object.update') as mock_update, \ patch('src.tracking.map.Map._generate_obj_gate', return_value = 0), \ patch('src.tracking.map.Object.__init__', return_value = None) as mock_obj_init, \ patch('src.tracking.map.Map._return_full_objects', return_value = self.map.object_list), \ patch('src.tracking.map.time_in_millis', return_value = 1): # set mock joint pdaf return value mock_pdaf.return_value = truth_update_vals, truth_dets_used # call smooth_frame self.map.smooth_frame(epoch_frame, [0, 0]) # check that first two objects are correctly updated for update_vals in filter(None, truth_update_vals): mock_update.assert_any_call(*update_vals) # check that new object is created for final detection new_obj_idx = 2 self.assertEqual( (epoch_frame[new_obj_idx][1], epoch_frame[new_obj_idx][0], 1), mock_obj_init.call_args[0]) self.assertEqual({'objectType': epoch_frame[new_obj_idx][2]}, mock_obj_init.call_args[1])
def test_update_map(self, mock_predict): """Tests update map method""" # add objects to list num_objects = 2 rng_list = [12.512, 44] bearing_list = [-22, 81.5] type_list = [ObjectType.BUOY, ObjectType.BOAT] for ii, (rng, bearing, obj_type) in enumerate(zip(rng_list, bearing_list, type_list)): obj = Object(bearing, rng, time_in_millis(), objectType=obj_type) self.map.object_list.append(obj) # call update_map self.map.update_map() # check if predict was called for all objects in object_list self.assertEqual(mock_predict.call_count, num_objects)
def smooth_frame(self, epoch_frame, frame_bounds): """ Updates map using observations from object list input Inputs: epoch_frame -- list containing rng, bearing, and object type for each detection in epoch frame_bounds -- tuple of lists containing range and bearing bounds Side Effects: object_list -- Updates object list using data from frame (updates or creates new objects) """ # trim object list to only include tracks with frame bounds trimmed_object_list = self._return_full_objects(bearingRange = frame_bounds[1], rngRange = frame_bounds[0]) if len(trimmed_object_list) != 0: # generate gate list gate_list = [self._generate_obj_gate(obj) for obj in trimmed_object_list] # get update list from joint pdaf update_list, detections_used = joint_pdaf(trimmed_object_list, gate_list, epoch_frame) for obj, update in zip(trimmed_object_list, update_list): # update objects if update is not None: mutex.acquire() obj.update(update[0], update[1]) mutex.release() else: mutex.acquire() obj.update(None, None) mutex.release() else: detections_used = [0] * len(epoch_frame) # use all detections NOT used to update objects to create new objects for ii, det in enumerate(epoch_frame): if detections_used[ii] == 0: new_obj = Object(det[1], det[0], time_in_millis(), objectType = det[2]) # create object using detection mutex.acquire() self.object_list.append(new_obj) # add to object_list mutex.release()
def __init__(self, pos, vel, pos_sigma=None, vel_sigma=None): """Initialize kalman filter Inputs: pos -- position of object (polar) vel -- velocity of object (polar) pos_sigma -- uncertainty of position of object (polar) vel_sigma -- uncertainty of veloicty of object (polar) """ kalman_config = read_kalman_config() self.state = np.append(pos, vel).astype( np.float32 ) # create state vector (elements are r, bear, v_r, v_bear) if pos_sigma is None: pos_sigma = np.array( [kalman_config['r_sigma'], kalman_config['theta_sigma']]).astype(np.float32) if vel_sigma is None: vel_sigma = np.array([ kalman_config['r_hat_sigma'], kalman_config['theta_hat_sigma'] ]).astype(np.float32) self.covar = np.diag(np.append(pos_sigma, vel_sigma)).astype( np.float32 ) # create covariance matrix (matrix of certainties of measurements) self.measurement_covar = np.eye(self.covar.shape[0]).astype(np.float32) self.process_noise = np.eye(self.state.shape[0]).astype( np.float32) # initalize process noise self.last_time_changed = time_in_millis() self.delta_t = 0 # create state transition matrix self.state_trans = np.array([[1., 0, self.delta_t, 0], [0, 1., 0, self.delta_t], [0, 0, 1., 0], [0, 0, 0, 1.]]) self.measurement_trans = np.eye( self.state.size) # create measurement transition matrix
def update_detections(self, send_counter): """Updates detections by random dr and dtheta""" # loop through detections for ii in range(len(self.epoch_frame)): # get dt dt = (time_in_millis() - self.prev_time[ii]) / 1000. # get rng and bearing rate rng_rate, bearing_rate = self.rng_rate_list[ ii], self.bearing_rate_list[ii] # adjust bearing rate based on distance from origin bearing_rate /= 0.5 * self.epoch_frame[ii][0] # generate deltas rand_dr = uniform(-0.01, 0.01) rand_dtheta = uniform(-0.001, 0.001) dr = (rand_dr + rng_rate) * dt dtheta = (rand_dtheta + bearing_rate) * dt # fix wraparound theta = self.epoch_frame[ii][1] + dtheta rng = self.epoch_frame[ii][0] + dr if theta > 180: theta = -180 + (theta % 180) if rng < 0: rng *= -1 theta %= -180 # update detection with deltas self.epoch_frame[ii] = (rng, theta, self.epoch_frame[ii][2]) # set detections for plotting self.det_rng_data = [det[0] for det in self.epoch_frame] self.det_bearing_data = [det[1] for det in self.epoch_frame] self.det_type_data = [det[2] for det in self.epoch_frame] idx_list = [1] * len(self.epoch_frame) if self.look_frame == 'aperture': # trim epoch frame to objects in view look aperture for ii, obj in enumerate(self.epoch_frame): if not self.look_rng[0] <= obj[0] <= self.look_rng[1] or \ not self.look_bearing[0] <= obj[1] <= self.look_bearing[1]: idx_list[ii] = 0 if self.detect_mode == 'random': # trim epoch frame using randint (to determine whether or not to send data) for ii in range(len(idx_list)): if randint(0, 9) >= (self.detect_probability * 10): idx_list[ii] = 0 elif self.detect_mode == 'regular': if send_counter != 0: idx_list = [0] * len(idx_list) elif self.detect_mode == 'constant': pass epoch_frame = [ obj for (ii, obj) in zip(idx_list, self.epoch_frame) if ii == 1 ] # update map with detections pub.sendMessage('object(s) detected', epoch_frame=epoch_frame, frame_bounds=self.frame_bounds)
def __init__(self): super().__init__() """Initializes tracker test""" self.update_interval = 0.25 # initialize map self.map = Map(None, True) # create polar plot self.fig = plt.figure() self.polar = self.fig.add_subplot(111, projection='polar') self.polar.set_ylim(0, 200) self.polar.grid(True) self.polar.set_title('Tracker Map') # set up rng, bearing, and type data lists for tracks self.covar_ellipses = [] # set up rng, bearing, and type data lists for tracks self.det_rng_data = [0] self.det_bearing_data = [0] self.det_type_data = [0] self.dets = self.polar.scatter(self.det_rng_data, self.det_bearing_data, c='#ff4444', label='Detections', s=3) # set up legend box = self.polar.get_position() self.polar.set_position( [box.x0, box.y0 + box.height * 0.1, box.width, box.height * 0.9]) # Put a legend below current axis self.polar.legend(loc='upper center', bbox_to_anchor=(0.5, -0.05), fancybox=True, shadow=True, ncol=5) # color plot grey self.polar.set_facecolor('#F5F5F5') self.polar.set_alpha(0.2) # show plot (and set up blitting) plt.show(block=False) plt.pause(0.01) self.fig.canvas.draw() input('Make plot full screen then press enter') self.background = self.fig.canvas.copy_from_bbox(self.polar.bbox) # create initial detections num_initial_detections = 18 # patterns: static, circle_CCW, circle_CW, circle_CCW_radial_out, circle_CW_radial_out, radial_out self.rng_rate_list = [0, 0, 0, 0.05, 0.05, 0.075] * 3 self.bearing_rate_list = [0, 0.0375, -0.0375, 0.025, -0.025, 0] * 3 self.frame_bounds = [(10, 175), (-180, 180)] self.spawn_detections(num_initial_detections) # set look frame parameters self.look_frame = 'aperture' #'full' self.look_rng = (0, 150 ) # initial (and perm) range range of look aperture self.look_bearing = (-70, 70) # initial bearing range of look aperture self.look_sweep = ( -30, 30 ) # sweeps so that the center of the aperture goes between these two points self.pan_direction = 1 # direction of pan (+1 = CW, -1 = CCW) self.look_apertures = [] self.num_apertures = 1 # detection parameters self.detect_mode = 'regular' #'constant' #'random' self.detect_probability = 0.6 # start tracker self.map.start() # initialize old update time (of detection) self.prev_time = [time_in_millis()] * num_initial_detections