def test_estimate(self):
"""Tests that the estimating of the locations via both is sane."""
start_coordinates_m = (100.0, 150.0)
heading_d = 40.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
speed_m_s = 5.0
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
tick_s = 0.5
step_m = speed_m_s * tick_s
step_x_m, step_y_m = Telemetry.rotate_radians_clockwise(
(0.0, step_m), math.radians(heading_d))
actual_x_m, actual_y_m = start_coordinates_m
def check_estimates():
self.assertLess(
abs(location_filter.estimated_location()[0] - actual_x_m), 0.5)
self.assertLess(
abs(location_filter.estimated_location()[1] - actual_y_m), 0.5)
for update in range(1, 21):
actual_x_m += step_x_m
actual_y_m += step_y_m
# First update by compass
measurements = numpy.matrix([0.0, 0.0, heading_d, 0.0]).transpose()
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
tick_s)
check_estimates()
# Add some approximated GPS readings
# We'll use very tight standard deviations to try to avoid random
# test failures
measurements = numpy.matrix([
random.normalvariate(actual_x_m, 0.01),
random.normalvariate(actual_y_m, 0.01),
random.normalvariate(heading_d, 0.5),
random.normalvariate(speed_m_s, speed_m_s * 0.1)
]).transpose()
location_filter._update(
measurements,
location_filter.GPS_OBSERVER_MATRIX,
location_filter.GPS_MEASUREMENT_NOISE,
0.0 # Tick isn't used for GPS
)
check_estimates()
def benchmark_location_filter_update_compass():
"""Benchmark the location filter compass update."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 100
start = time.time()
for _ in range(iterations):
location_filter.update_compass(20.0)
end = time.time()
print('{} iterations of LocationFilter.update_compass, each took {:.5}'.
format(iterations, (end - start) / float(iterations)))
def benchmark_location_filter_update_dead_reckoning():
"""Benchmark the location filter with dead reckoning and no other input."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 1000
start = time.time()
for _ in range(iterations):
location_filter.update_dead_reckoning()
end = time.time()
print(
'{} iterations of LocationFilter.update_dead_reckoning, each took {:.5}'
.format(iterations, (end - start) / float(iterations)))
def __init__(self, kml_file_name=None):
self._data = {}
self._logger = AsyncLogger()
self._speed_history = collections.deque()
self._z_acceleration_g = collections.deque()
self._lock = threading.Lock()
# TODO: For the competition, just hard code the compass. For now, the
# Kalman filter should start reading in values and correct quickly.
self._location_filter = LocationFilter(0.0, 0.0, 0.0)
self._estimated_steering = 0.0
self._estimated_throttle = 0.0
self._target_steering = 0.0
self._target_throttle = 0.0
self._ignored_points = collections.defaultdict(lambda: 0)
self._ignored_points_thresholds = collections.defaultdict(lambda: 10)
consume = lambda: consume_messages(config.TELEMETRY_EXCHANGE, self.
_handle_message)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__, config.TELEMETRY_EXCHANGE)
thread.start()
self._course_m = None
try:
if kml_file_name is not None:
self.load_kml_from_file_name(kml_file_name)
self._logger.info(
'Loaded {} course points and {} inner objects'.format(
len(self._course_m['course']),
len(self._course_m['inner'])))
else:
self._course_m = None
if self._course_m is not None:
if len(self._course_m['course']) == 0:
self._logger.warn(
'No course defined for {}'.format(kml_file_name))
if len(self._course_m['inner']) == 0:
self._logger.warn(
'No inner obstacles defined for {}'.format(
kml_file_name))
except Exception as e:
self._logger.error('Unable to load course file: {}'.format(e))
def test_estimate_constant_speed(self):
"""Tests that the estimating of the locations via dead reckoning at a
constant speed is sane.
"""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
start_coordinates_m = (100.0, 200.0)
heading_d = 32.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
speed_m_s = 1.8
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
measurements = numpy.matrix( # pylint: disable=no-member
[0.0, 0.0, heading_d, 0.0]).transpose() # z
seconds = 5
for _ in range(seconds):
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
time_diff_s=1.0)
offset = Telemetry.rotate_radians_clockwise((0.0, speed_m_s * seconds),
math.radians(heading_d))
new_coordinates = [s + o for s, o in zip(start_coordinates_m, offset)]
for estimated, expected in zip(location_filter.estimated_location(),
new_coordinates):
self.assertAlmostEqual(estimated, expected, 2)
def test_estimate_gps(self):
"""Tests that the estimating of the locations via GPS is sane."""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
heading_d = 0.0
for coordinates in ((100, 200), (200, 100), (300, 300), (50, 50)):
location_filter = LocationFilter(coordinates[0], coordinates[1],
heading_d)
self.assertEqual(location_filter.estimated_location(), coordinates)
new_coordinates = (150, 150)
for _ in range(6):
location_filter.update_gps(new_coordinates[0],
new_coordinates[1],
x_accuracy_m=0.1,
y_accuracy_m=0.1,
heading_d=heading_d,
speed_m_s=0)
for estimated, expected in zip(
location_filter.estimated_location(), new_coordinates):
self.assertAlmostEqual(estimated, expected, 2)
