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]) )