Ejemplo n.º 1
0
    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),
                })
Ejemplo n.º 3
0
    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
Ejemplo n.º 4
0
    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))
Ejemplo n.º 6
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))
Ejemplo n.º 7
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]
Ejemplo n.º 9
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')
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
0
    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])
            )