def _prediction_step(self, time_diff_s):
"""Runs the prediction step and returns the transition matrix."""
# x = A * x + B
heading_r = math.radians(self.estimated_heading())
from control.telemetry import Telemetry
x_delta, y_delta = Telemetry.rotate_radians_clockwise(
(0.0, time_diff_s),
heading_r
)
speed_m_s = self.estimated_speed()
transition = numpy.matrix([ # A
[1.0, 0.0, 0.0, x_delta],
[0.0, 1.0, 0.0, y_delta],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]
])
# Update heading estimate based on steering
new_heading = Telemetry.wrap_degrees(
self.estimated_heading()
+ self._estimated_turn_rate_d_s * time_diff_s
)
self._estimates.itemset(2, new_heading)
# TODO: Add acceleration values
self._estimates = transition * self._estimates
return transition
def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
from control.telemetry import Telemetry
# Normally, you'd have a loop that periodically checks for new readings
# or that blocks until readings are received
while self._run:
try:
time.sleep(self._sleep_time_s)
except Exception: # pylint: disable=broad-except
pass
speed_m_s = 0.0
if self._driver is not None:
speed_m_s = self._driver.get_throttle() * self.MAX_SPEED_M_S
point_m = (0.0, speed_m_s * self._sleep_time_s)
offset_m = Telemetry.rotate_radians_clockwise(
point_m,
math.radians(self._heading_d)
)
self._x_m += offset_m[0]
self._y_m += offset_m[1]
self._heading_d += \
self._driver.get_turn() * self.TURN_D_S * self._sleep_time_s
self._heading_d = Telemetry.wrap_degrees(self._heading_d)
self._iterations += 1
if self._iterations % 5 == 0:
gps_d = Telemetry.wrap_degrees(
self._random.normalvariate(
self._heading_d,
self.SIGMA_GPS_D
)
)
self._telemetry.handle_message({
'x_m': self._random.normalvariate(self._x_m, self.SIGMA_M),
'y_m': self._random.normalvariate(self._y_m, self.SIGMA_M),
'x_accuracy_m': self.SIGMA_M,
'y_accuracy_m': self.SIGMA_M,
'speed_m_s': self._random.normalvariate(
speed_m_s,
self.SIGMA_M_S
),
'gps_d': gps_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
else:
compass_d = Telemetry.wrap_degrees(
self._random.normalvariate(
self._heading_d,
self.SIGMA_COMPASS_D
)
)
self._telemetry.handle_message({
'compass_d': compass_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
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 run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
from control.telemetry import Telemetry
# Normally, you'd have a loop that periodically checks for new readings
# or that blocks until readings are received
while self._run:
try:
time.sleep(self._sleep_time_s)
except Exception: # pylint: disable=broad-except
pass
speed_m_s = 0.0
if self._driver is not None:
speed_m_s = self._driver.get_throttle() * self.MAX_SPEED_M_S
point_m = (0.0, speed_m_s * self._sleep_time_s)
offset_m = Telemetry.rotate_radians_clockwise(
point_m, math.radians(self._heading_d))
self._x_m += offset_m[0]
self._y_m += offset_m[1]
self._heading_d += \
self._driver.get_turn() * self.TURN_D_S * self._sleep_time_s
self._heading_d = Telemetry.wrap_degrees(self._heading_d)
self._iterations += 1
if self._iterations % 5 == 0:
gps_d = Telemetry.wrap_degrees(
self._random.normalvariate(self._heading_d,
self.SIGMA_GPS_D))
self._telemetry.handle_message({
'x_m':
self._random.normalvariate(self._x_m, self.SIGMA_M),
'y_m':
self._random.normalvariate(self._y_m, self.SIGMA_M),
'x_accuracy_m':
self.SIGMA_M,
'y_accuracy_m':
self.SIGMA_M,
'speed_m_s':
self._random.normalvariate(speed_m_s, self.SIGMA_M_S),
'gps_d':
gps_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
else:
compass_d = Telemetry.wrap_degrees(
self._random.normalvariate(self._heading_d,
self.SIGMA_COMPASS_D))
self._telemetry.handle_message({
'compass_d':
compass_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
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_rotate_radians_clockwise(self):
"""Tests rotating a vector radians clockwise."""
base = (1.0, 0.0)
not_rotated = Telemetry.rotate_radians_clockwise(base, 0.0)
self.assertAlmostEqual(not_rotated[0], base[0])
self.assertAlmostEqual(not_rotated[1], base[1])
ninety = Telemetry.rotate_radians_clockwise(base, math.radians(90.0))
self.assertAlmostEqual(ninety[0], 0.0)
self.assertAlmostEqual(ninety[1], -1.0)
one_eighty = Telemetry.rotate_radians_clockwise(
base, math.radians(180.0))
self.assertAlmostEqual(one_eighty[0], -base[0])
self.assertAlmostEqual(one_eighty[1], base[1])
two_seventy = Telemetry.rotate_radians_clockwise(
base, math.radians(270.0))
self.assertAlmostEqual(two_seventy[0], 0.0)
self.assertAlmostEqual(two_seventy[1], 1.0)
three_sixty = Telemetry.rotate_radians_clockwise(
base, math.radians(360.0))
self.assertAlmostEqual(three_sixty[0], base[0])
self.assertAlmostEqual(three_sixty[1], base[1])
negative_ninety = Telemetry.rotate_radians_clockwise(
base, math.radians(-90.0))
self.assertAlmostEqual(negative_ninety[0], two_seventy[0])
self.assertAlmostEqual(negative_ninety[1], two_seventy[1])
def _update_position(self):
"""Updates the position using dead reckoning."""
self.update_count -= 1
diff_time_s = 1.0
if self._throttle > 0.0:
self._heading += self._turn * 30.0
self._heading = Telemetry.wrap_degrees(self._heading)
step_m = diff_time_s * self._throttle * self.MAX_SPEED_M_S
point = (0, step_m)
radians = math.radians(self._heading)
point = Telemetry.rotate_radians_clockwise(point, radians)
self._x_m += point[0]
self._y_m += point[1]
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 _prediction_step(self, time_diff_s):
"""Runs the prediction step and returns the transition matrix."""
# x = A * x + B
heading_r = math.radians(self.estimated_heading())
from control.telemetry import Telemetry
x_delta, y_delta = Telemetry.rotate_radians_clockwise(
(0.0, time_diff_s), heading_r)
speed_m_s = self.estimated_speed()
transition = numpy.matrix([ # A
[1.0, 0.0, 0.0, x_delta], [0.0, 1.0, 0.0, y_delta],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]
])
# Update heading estimate based on steering
new_heading = Telemetry.wrap_degrees(self.estimated_heading() +
self._estimated_turn_rate_d_s *
time_diff_s)
self._estimates.itemset(2, new_heading)
# TODO: Add acceleration values
self._estimates = transition * self._estimates
return transition
def test_rotate_radians_clockwise(self):
"""Tests rotating a vector radians clockwise."""
base = (1.0, 0.0)
not_rotated = Telemetry.rotate_radians_clockwise(base, 0.0)
self.assertAlmostEqual(not_rotated[0], base[0])
self.assertAlmostEqual(not_rotated[1], base[1])
ninety = Telemetry.rotate_radians_clockwise(base, math.radians(90.0))
self.assertAlmostEqual(ninety[0], 0.0)
self.assertAlmostEqual(ninety[1], -1.0)
one_eighty = Telemetry.rotate_radians_clockwise(
base,
math.radians(180.0)
)
self.assertAlmostEqual(one_eighty[0], -base[0])
self.assertAlmostEqual(one_eighty[1], base[1])
two_seventy = Telemetry.rotate_radians_clockwise(
base,
math.radians(270.0)
)
self.assertAlmostEqual(two_seventy[0], 0.0)
self.assertAlmostEqual(two_seventy[1], 1.0)
three_sixty = Telemetry.rotate_radians_clockwise(
base,
math.radians(360.0)
)
self.assertAlmostEqual(three_sixty[0], base[0])
self.assertAlmostEqual(three_sixty[1], base[1])
negative_ninety = Telemetry.rotate_radians_clockwise(
base,
math.radians(-90.0)
)
self.assertAlmostEqual(negative_ninety[0], two_seventy[0])
self.assertAlmostEqual(negative_ninety[1], two_seventy[1])
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()
