예제 #1
0
 def __init__(self, disabled_logs=[], dog=None):
     self.kf = LiveKalman()
     self.reset_kalman()
     self.max_age = .2  # seconds
     self.disabled_logs = disabled_logs
     self.calib = np.zeros(3)
     self.device_from_calib = np.eye(3)
     self.calib_from_device = np.eye(3)
     self.calibrated = 0
예제 #2
0
 def __init__(self, disabled_logs=[], dog=None):
     self.kf = LiveKalman()
     self.reset_kalman()
     self.max_age = .2  # seconds
     self.disabled_logs = disabled_logs
예제 #3
0
class Localizer():
    def __init__(self, disabled_logs=[], dog=None):
        self.kf = LiveKalman()
        self.reset_kalman()
        self.max_age = .2  # seconds
        self.disabled_logs = disabled_logs

    def liveLocationMsg(self, time):
        fix = messaging.log.LiveLocationData.new_message()

        predicted_state = self.kf.x

        fix_ecef = predicted_state[States.ECEF_POS]
        fix_pos_geo = coord.ecef2geodetic(fix_ecef)
        fix.lat = float(fix_pos_geo[0])
        fix.lon = float(fix_pos_geo[1])
        fix.alt = float(fix_pos_geo[2])

        fix.speed = float(np.linalg.norm(
            predicted_state[States.ECEF_VELOCITY]))

        orientation_ned_euler = ned_euler_from_ecef(
            fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
        fix.roll = math.degrees(orientation_ned_euler[0])
        fix.pitch = math.degrees(orientation_ned_euler[1])
        fix.heading = math.degrees(orientation_ned_euler[2])

        fix.gyro = [
            float(predicted_state[10]),
            float(predicted_state[11]),
            float(predicted_state[12])
        ]
        fix.accel = [
            float(predicted_state[19]),
            float(predicted_state[20]),
            float(predicted_state[21])
        ]

        ned_vel = self.converter.ecef2ned(
            predicted_state[States.ECEF_POS] +
            predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(
                predicted_state[States.ECEF_POS])
        fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])]
        fix.source = 'kalman'

        #local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY])
        #fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0]))
        #fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0]))

        imu_frame = predicted_state[States.IMU_OFFSET]
        fix.imuFrame = [
            math.degrees(imu_frame[0]),
            math.degrees(imu_frame[1]),
            math.degrees(imu_frame[2])
        ]
        return fix

    def update_kalman(self, time, kind, meas):
        if self.filter_ready:
            try:
                self.kf.predict_and_observe(time, kind, meas)
            except KalmanError:
                cloudlog.error("Error in predict and observe, kalman reset")
                self.reset_kalman()
        #idx = bisect_right([x[0] for x in self.observation_buffer], time)
        #self.observation_buffer.insert(idx, (time, kind, meas))
        #while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
        #  else:
        #    self.observation_buffer.pop(0)

    def handle_gps(self, current_time, log):
        self.converter = coord.LocalCoord.from_geodetic(
            [log.latitude, log.longitude, log.altitude])
        fix_ecef = self.converter.ned2ecef([0, 0, 0])

        # TODO initing with bad bearing not allowed, maybe not bad?
        if not self.filter_ready and log.speed > 5:
            self.filter_ready = True
            initial_ecef = fix_ecef
            gps_bearing = math.radians(log.bearing)
            initial_pose_ecef = ecef_euler_from_ned(initial_ecef,
                                                    [0, 0, gps_bearing])
            initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
            gps_speed = log.speed
            quat_uncertainty = 0.2**2
            initial_pose_ecef_quat = euler2quat(initial_pose_ecef)

            initial_state = LiveKalman.initial_x
            initial_covs_diag = LiveKalman.initial_P_diag

            initial_state[States.ECEF_POS] = initial_ecef
            initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
            initial_state[States.ECEF_VELOCITY] = rotations_from_quats(
                initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))

            initial_covs_diag[States.ECEF_POS_ERR] = 10**2
            initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
            initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
            self.kf.init_state(initial_state,
                               covs=np.diag(initial_covs_diag),
                               filter_time=current_time)
            cloudlog.info("Filter initialized")
        elif self.filter_ready:
            self.update_kalman(current_time, ObservationKind.ECEF_POS,
                               fix_ecef)
            gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
                                    (self.kf.x[1] - fix_ecef[1])**2 +
                                    (self.kf.x[2] - fix_ecef[2])**2)
            if gps_est_error > 50:
                cloudlog.error(
                    "Locationd vs ubloxLocation difference too large, kalman reset"
                )
                self.reset_kalman()

    def handle_car_state(self, current_time, log):
        self.speed_counter += 1

        if self.speed_counter % SENSOR_DECIMATION == 0:
            self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED,
                               [log.vEgo])
            if log.vEgo == 0:
                self.update_kalman(current_time, ObservationKind.NO_ROT,
                                   [0, 0, 0])

    def handle_cam_odo(self, current_time, log):
        self.cam_counter += 1

        if self.cam_counter % VISION_DECIMATION == 0:
            self.update_kalman(current_time,
                               ObservationKind.CAMERA_ODO_ROTATION,
                               np.concatenate([log.rot, log.rotStd]))
            self.update_kalman(current_time,
                               ObservationKind.CAMERA_ODO_TRANSLATION,
                               np.concatenate([log.trans, log.transStd]))

    def handle_sensors(self, current_time, log):
        # TODO does not yet account for double sensor readings in the log
        for sensor_reading in log:
            # Gyro Uncalibrated
            if sensor_reading.sensor == 5 and sensor_reading.type == 16:
                self.gyro_counter += 1
                if self.gyro_counter % SENSOR_DECIMATION == 0:
                    if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
                        cloudlog.info('imu frame angles exceeded, correcting')
                        self.update_kalman(current_time,
                                           ObservationKind.IMU_FRAME,
                                           [0, 0, 0])

                    v = sensor_reading.gyroUncalibrated.v
                    self.update_kalman(current_time,
                                       ObservationKind.PHONE_GYRO,
                                       [-v[2], -v[1], -v[0]])

            # Accelerometer
            if sensor_reading.sensor == 1 and sensor_reading.type == 1:
                self.acc_counter += 1
                if self.acc_counter % SENSOR_DECIMATION == 0:
                    v = sensor_reading.acceleration.v
                    self.update_kalman(current_time,
                                       ObservationKind.PHONE_ACCEL,
                                       [-v[2], -v[1], -v[0]])

    def reset_kalman(self):
        self.filter_time = None
        self.filter_ready = False
        self.observation_buffer = []

        self.gyro_counter = 0
        self.acc_counter = 0
        self.speed_counter = 0
        self.cam_counter = 0
예제 #4
0
class Localizer():
    def __init__(self, disabled_logs=[], dog=None):
        self.kf = LiveKalman()
        self.reset_kalman()
        self.max_age = .2  # seconds
        self.disabled_logs = disabled_logs
        self.calib = np.zeros(3)
        self.device_from_calib = np.eye(3)
        self.calib_from_device = np.eye(3)
        self.calibrated = 0
        self.H = get_H()

    def liveLocationMsg(self, time):
        predicted_state = self.kf.x
        predicted_cov = self.kf.P
        predicted_std = np.sqrt(np.diagonal(self.kf.P))

        fix_ecef = predicted_state[States.ECEF_POS]
        fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
        vel_ecef = predicted_state[States.ECEF_VELOCITY]
        vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
        fix_pos_geo = coord.ecef2geodetic(fix_ecef)
        #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
        orientation_ecef = euler_from_quat(
            predicted_state[States.ECEF_ORIENTATION])
        orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]

        acc_calib = self.calib_from_device.dot(
            predicted_state[States.ACCELERATION])
        acc_calib_std = np.sqrt(
            np.diagonal(
                self.calib_from_device.dot(
                    predicted_cov[States.ACCELERATION_ERR,
                                  States.ACCELERATION_ERR]).dot(
                                      self.calib_from_device.T)))
        ang_vel_calib = self.calib_from_device.dot(
            predicted_state[States.ANGULAR_VELOCITY])
        ang_vel_calib_std = np.sqrt(
            np.diagonal(
                self.calib_from_device.dot(
                    predicted_cov[States.ANGULAR_VELOCITY_ERR,
                                  States.ANGULAR_VELOCITY_ERR]).dot(
                                      self.calib_from_device.T)))

        device_from_ecef = rot_from_quat(
            predicted_state[States.ECEF_ORIENTATION]).T
        vel_device = device_from_ecef.dot(vel_ecef)
        device_from_ecef_eul = euler_from_quat(
            predicted_state[States.ECEF_ORIENTATION]).T
        idxs = list(
            range(States.ECEF_ORIENTATION_ERR.start,
                  States.ECEF_ORIENTATION_ERR.stop)) + list(
                      range(States.ECEF_VELOCITY_ERR.start,
                            States.ECEF_VELOCITY_ERR.stop))
        condensed_cov = predicted_cov[idxs][:, idxs]
        H = self.H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
        vel_device_cov = H.dot(condensed_cov).dot(H.T)
        vel_device_std = np.sqrt(np.diagonal(vel_device_cov))

        vel_calib = self.calib_from_device.dot(vel_device)
        vel_calib_std = np.sqrt(
            np.diagonal(
                self.calib_from_device.dot(vel_device_cov).dot(
                    self.calib_from_device.T)))

        orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
        #orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
        ned_vel = self.converter.ecef2ned(
            fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef)
        #ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)

        fix = messaging.log.LiveLocationKalman.new_message()
        fix.positionGeodetic.value = to_float(fix_pos_geo)
        #fix.positionGeodetic.std = to_float(fix_pos_geo_std)
        #fix.positionGeodetic.valid = True
        fix.positionECEF.value = to_float(fix_ecef)
        fix.positionECEF.std = to_float(fix_ecef_std)
        fix.positionECEF.valid = True
        fix.velocityECEF.value = to_float(vel_ecef)
        fix.velocityECEF.std = to_float(vel_ecef_std)
        fix.velocityECEF.valid = True
        fix.velocityNED.value = to_float(ned_vel)
        #fix.velocityNED.std = to_float(ned_vel_std)
        #fix.velocityNED.valid = True
        fix.velocityDevice.value = to_float(vel_device)
        fix.velocityDevice.std = to_float(vel_device_std)
        fix.velocityDevice.valid = True
        fix.accelerationDevice.value = to_float(
            predicted_state[States.ACCELERATION])
        fix.accelerationDevice.std = to_float(
            predicted_std[States.ACCELERATION_ERR])
        fix.accelerationDevice.valid = True

        fix.orientationECEF.value = to_float(orientation_ecef)
        fix.orientationECEF.std = to_float(orientation_ecef_std)
        fix.orientationECEF.valid = True
        fix.orientationNED.value = to_float(orientation_ned)
        #fix.orientationNED.std = to_float(orientation_ned_std)
        #fix.orientationNED.valid = True
        fix.angularVelocityDevice.value = to_float(
            predicted_state[States.ANGULAR_VELOCITY])
        fix.angularVelocityDevice.std = to_float(
            predicted_std[States.ANGULAR_VELOCITY_ERR])
        fix.angularVelocityDevice.valid = True

        fix.velocityCalibrated.value = to_float(vel_calib)
        fix.velocityCalibrated.std = to_float(vel_calib_std)
        fix.velocityCalibrated.valid = True
        fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
        fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
        fix.angularVelocityCalibrated.valid = True
        fix.accelerationCalibrated.value = to_float(acc_calib)
        fix.accelerationCalibrated.std = to_float(acc_calib_std)
        fix.accelerationCalibrated.valid = True

        #fix.gpsWeek = self.time.week
        #fix.gpsTimeOfWeek = self.time.tow
        fix.unixTimestampMillis = self.unix_timestamp_millis

        if self.filter_ready and self.calibrated:
            fix.status = 'valid'
        elif self.filter_ready:
            fix.status = 'uncalibrated'
        else:
            fix.status = 'uninitialized'
        return fix

    def update_kalman(self, time, kind, meas):
        if self.filter_ready:
            try:
                self.kf.predict_and_observe(time, kind, meas)
            except KalmanError:
                cloudlog.error("Error in predict and observe, kalman reset")
                self.reset_kalman()
        #idx = bisect_right([x[0] for x in self.observation_buffer], time)
        #self.observation_buffer.insert(idx, (time, kind, meas))
        #while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
        #  else:
        #    self.observation_buffer.pop(0)

    def handle_gps(self, current_time, log):
        self.converter = coord.LocalCoord.from_geodetic(
            [log.latitude, log.longitude, log.altitude])
        fix_ecef = self.converter.ned2ecef([0, 0, 0])

        #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
        self.unix_timestamp_millis = log.timestamp

        # TODO initing with bad bearing not allowed, maybe not bad?
        if not self.filter_ready and log.speed > 5:
            self.filter_ready = True
            initial_ecef = fix_ecef
            gps_bearing = math.radians(log.bearing)
            initial_pose_ecef = ecef_euler_from_ned(initial_ecef,
                                                    [0, 0, gps_bearing])
            initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
            gps_speed = log.speed
            quat_uncertainty = 0.2**2
            initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)

            initial_state = LiveKalman.initial_x
            initial_covs_diag = LiveKalman.initial_P_diag

            initial_state[States.ECEF_POS] = initial_ecef
            initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
            initial_state[States.ECEF_VELOCITY] = rot_from_quat(
                initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))

            initial_covs_diag[States.ECEF_POS_ERR] = 10**2
            initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
            initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
            self.kf.init_state(initial_state,
                               covs=np.diag(initial_covs_diag),
                               filter_time=current_time)
            cloudlog.info("Filter initialized")
        elif self.filter_ready:
            self.update_kalman(current_time, ObservationKind.ECEF_POS,
                               fix_ecef)
            gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
                                    (self.kf.x[1] - fix_ecef[1])**2 +
                                    (self.kf.x[2] - fix_ecef[2])**2)
            if gps_est_error > 50:
                cloudlog.error(
                    "Locationd vs ubloxLocation difference too large, kalman reset"
                )
                self.reset_kalman()

    def handle_car_state(self, current_time, log):
        self.speed_counter += 1

        if self.speed_counter % SENSOR_DECIMATION == 0:
            self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED,
                               [log.vEgo])
            if log.vEgo == 0:
                self.update_kalman(current_time, ObservationKind.NO_ROT,
                                   [0, 0, 0])

    def handle_cam_odo(self, current_time, log):
        self.cam_counter += 1

        if self.cam_counter % VISION_DECIMATION == 0:
            rot_device = self.device_from_calib.dot(log.rot)
            rot_device_std = self.device_from_calib.dot(log.rotStd)
            self.update_kalman(current_time,
                               ObservationKind.CAMERA_ODO_ROTATION,
                               np.concatenate([rot_device, rot_device_std]))
            trans_device = self.device_from_calib.dot(log.trans)
            trans_device_std = self.device_from_calib.dot(log.transStd)
            self.update_kalman(
                current_time, ObservationKind.CAMERA_ODO_TRANSLATION,
                np.concatenate([trans_device, trans_device_std]))

    def handle_sensors(self, current_time, log):
        # TODO does not yet account for double sensor readings in the log
        for sensor_reading in log:
            # Gyro Uncalibrated
            if sensor_reading.sensor == 5 and sensor_reading.type == 16:
                self.gyro_counter += 1
                if self.gyro_counter % SENSOR_DECIMATION == 0:
                    if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
                        cloudlog.info('imu frame angles exceeded, correcting')
                        self.update_kalman(current_time,
                                           ObservationKind.IMU_FRAME,
                                           [0, 0, 0])

                    v = sensor_reading.gyroUncalibrated.v
                    self.update_kalman(current_time,
                                       ObservationKind.PHONE_GYRO,
                                       [-v[2], -v[1], -v[0]])

            # Accelerometer
            if sensor_reading.sensor == 1 and sensor_reading.type == 1:
                self.acc_counter += 1
                if self.acc_counter % SENSOR_DECIMATION == 0:
                    v = sensor_reading.acceleration.v
                    self.update_kalman(current_time,
                                       ObservationKind.PHONE_ACCEL,
                                       [-v[2], -v[1], -v[0]])

    def handle_live_calib(self, current_time, log):
        self.calib = log.rpyCalib
        self.device_from_calib = rot_from_euler(self.calib)
        self.calib_from_device = self.device_from_calib.T
        self.calibrated = log.calStatus == 1

    def reset_kalman(self):
        self.filter_time = None
        self.filter_ready = False
        self.observation_buffer = []

        self.gyro_counter = 0
        self.acc_counter = 0
        self.speed_counter = 0
        self.cam_counter = 0