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 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 _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 _update(self, measurements, observer_matrix, measurement_noise,
time_diff_s):
"""Runs the Kalman update using the provided measurements."""
# Prediction step
transition = self._prediction_step(time_diff_s)
# Update uncertainty
# P = A * P * A' + Q
self._covariance_matrix = \
transition * self._covariance_matrix * transition.transpose() \
+ self._process_noise
# Compute the Kalman gain
# K = P * H' * inv(H * P * H' + R)
hphtr = (observer_matrix * self._covariance_matrix *
observer_matrix.transpose() + measurement_noise)
try:
hphtri = hphtr.getI()
except numpy.linalg.linalg.LinAlgError:
for diagonal_index in range(len(hphtr)):
diagonal_value = hphtr[diagonal_index].item(diagonal_index)
if diagonal_value == 0.0:
hphtr[diagonal_index].itemset(diagonal_index, 0.00001)
hphtri = hphtr.getI()
kalman_gain = \
self._covariance_matrix * observer_matrix.transpose() * hphtri
# Determine innovation or residual and update our estimate
# x = x + K * (z - H * x)
zhx = measurements - observer_matrix * self._estimates
heading_d = zhx[2].item(0)
while heading_d > 180.0:
heading_d -= 360.0
while heading_d <= -180.0:
heading_d += 360.0
zhx[2].itemset(0, heading_d)
self._estimates = self._estimates + kalman_gain * zhx
from control.telemetry import Telemetry
heading_d = Telemetry.wrap_degrees(self._estimates[2].item(0))
self._estimates[2].itemset(0, heading_d)
# Update the covariance
# P = (I - K * H) * P
self._covariance_matrix = (
numpy.identity(len(kalman_gain)) -
kalman_gain * observer_matrix) * self._covariance_matrix
assert len(self._estimates) == 4 and len(self._estimates[0]) == 1, \
'Estimates should be size 4x1 but is {}x{} after update'.format(
len(self._estimates),
len(self._estimates[0])
)
def test_wrap_degrees(self):
"""Tests wrap degrees."""
for d in range(0, 360):
self.assertAlmostEqual(d, Telemetry.wrap_degrees(d))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(0.0))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(360.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-1.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(721.0))
def test_wrap_degrees(self):
"""Tests wrap degrees."""
for d in range(0, 360):
self.assertAlmostEqual(d, Telemetry.wrap_degrees(d))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(0.0))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(360.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-1.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(721.0))
def _handle_binary(self, message):
"""Handles properietary SUP800F binary messages."""
if message is None:
return
flux_x = float(message.magnetic_flux_ut_x - self._compass_offsets[0])
flux_y = float(message.magnetic_flux_ut_y - self._compass_offsets[1])
if flux_x == 0.0:
# TODO: Figure out what to do here
return
self._last_compass_heading_d = Telemetry.wrap_degrees(
270.0 - math.degrees(
math.atan2(flux_y, flux_x)
) + 8.666 # Boulder declination
)
magnitude = flux_x ** 2 + flux_y ** 2
std_devs_away = abs(
self._magnitude_mean - magnitude
) / self._magnitude_std_dev
# In a normal distribution, 95% of readings should be within 2 std devs
if std_devs_away > 2.0:
self._dropped_compass_messages += 1
if self._dropped_compass_messages > self._dropped_threshold:
self._logger.warn(
'Dropped {} compass messages in a row, std dev = {}'.format(
self._dropped_compass_messages,
round(std_devs_away, 3)
)
)
self._dropped_compass_messages = 0
self._dropped_threshold += 10
return
self._dropped_compass_messages = 0
self._dropped_threshold = 10
if std_devs_away > 1.0:
confidence = 2.0 - std_devs_away
else:
confidence = 1.0
self._telemetry.compass_reading(
self._last_compass_heading_d,
confidence,
'sup800f'
)
self._telemetry.accelerometer_reading(
message.acceleration_g_x,
message.acceleration_g_y,
message.acceleration_g_z,
'sup800f'
)
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 _handle_binary(self, message):
"""Handles properietary SUP800F binary messages."""
if message is None:
return
flux_x = float(message.magnetic_flux_ut_x - self._compass_offsets[0])
flux_y = float(message.magnetic_flux_ut_y - self._compass_offsets[1])
if flux_x == 0.0:
# TODO: Figure out what to do here
return
self._last_compass_heading_d = Telemetry.wrap_degrees(
270.0 - math.degrees(math.atan2(flux_y, flux_x)) +
8.666 # Boulder declination
)
magnitude = flux_x**2 + flux_y**2
std_devs_away = abs(self._magnitude_mean -
magnitude) / self._magnitude_std_dev
# In a normal distribution, 95% of readings should be within 2 std devs
if std_devs_away > 2.0:
self._dropped_compass_messages += 1
if self._dropped_compass_messages > self._dropped_threshold:
self._logger.warn(
'Dropped {} compass messages in a row, std dev = {}'.
format(self._dropped_compass_messages,
round(std_devs_away, 3)))
self._dropped_compass_messages = 0
self._dropped_threshold += 10
return
self._dropped_compass_messages = 0
self._dropped_threshold = 10
if std_devs_away > 1.0:
confidence = 2.0 - std_devs_away
else:
confidence = 1.0
self._telemetry.compass_reading(self._last_compass_heading_d,
confidence, 'sup800f')
self._telemetry.accelerometer_reading(message.acceleration_g_x,
message.acceleration_g_y,
message.acceleration_g_z,
'sup800f')
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 _update(
self,
measurements,
observer_matrix,
measurement_noise,
time_diff_s
):
"""Runs the Kalman update using the provided measurements."""
# Prediction step
transition = self._prediction_step(time_diff_s)
# Update uncertainty
# P = A * P * A' + Q
self._covariance_matrix = \
transition * self._covariance_matrix * transition.transpose() \
+ self._process_noise
# Compute the Kalman gain
# K = P * H' * inv(H * P * H' + R)
hphtr = (
observer_matrix
* self._covariance_matrix
* observer_matrix.transpose()
+ measurement_noise
)
try:
hphtri = hphtr.getI()
except numpy.linalg.linalg.LinAlgError:
for diagonal_index in range(len(hphtr)):
diagonal_value = hphtr[diagonal_index].item(diagonal_index)
if diagonal_value == 0.0:
hphtr[diagonal_index].itemset(diagonal_index, 0.00001)
hphtri = hphtr.getI()
kalman_gain = \
self._covariance_matrix * observer_matrix.transpose() * hphtri
# Determine innovation or residual and update our estimate
# x = x + K * (z - H * x)
zhx = measurements - observer_matrix * self._estimates
heading_d = zhx[2].item(0)
while heading_d > 180.0:
heading_d -= 360.0
while heading_d <= -180.0:
heading_d += 360.0
zhx[2].itemset(0, heading_d)
self._estimates = self._estimates + kalman_gain * zhx
from control.telemetry import Telemetry
heading_d = Telemetry.wrap_degrees(self._estimates[2].item(0))
self._estimates[2].itemset(0, heading_d)
# Update the covariance
# P = (I - K * H) * P
self._covariance_matrix = (
numpy.identity(len(kalman_gain)) - kalman_gain * observer_matrix
) * self._covariance_matrix
assert len(self._estimates) == 4 and len(self._estimates[0]) == 1, \
'Estimates should be size 4x1 but is {}x{} after update'.format(
len(self._estimates),
len(self._estimates[0])
)
