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)