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