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 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_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(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()