Locationd packet (#1204)
* initial * add desire * initial * some progress * fill in, untests * add timestamping * fixes * unix timestamp * no debug print * cleanre * normal syntax * no laikapull/1202/head^2
HaraldSchafer
GitHub
parent
6db044c86c
commit
24eeec4e0b
|
|
@ -6,56 +6,113 @@ import numpy as np
|
|||
import cereal.messaging as messaging
|
||||
import common.transformations.coordinates as coord
|
||||
from common.transformations.orientation import (ecef_euler_from_ned,
|
||||
euler2quat,
|
||||
euler_from_quat,
|
||||
ned_euler_from_ecef,
|
||||
quat2euler,
|
||||
rotations_from_quats)
|
||||
quat_from_euler,
|
||||
rot_from_quat, rot_from_euler)
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError
|
||||
from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States
|
||||
from selfdrive.swaglog import cloudlog
|
||||
#from datetime import datetime
|
||||
#from laika.gps_time import GPSTime
|
||||
|
||||
|
||||
VISION_DECIMATION = 2
|
||||
SENSOR_DECIMATION = 10
|
||||
|
||||
|
||||
def to_float(arr):
|
||||
return [float(arr[0]), float(arr[1]), float(arr[2])]
|
||||
|
||||
|
||||
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
|
||||
|
||||
def liveLocationMsg(self, time):
|
||||
fix = messaging.log.LiveLocationData.new_message()
|
||||
|
||||
predicted_state = self.kf.x
|
||||
predicted_std = 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.lat = float(fix_pos_geo[0])
|
||||
fix.lon = float(fix_pos_geo[1])
|
||||
fix.alt = float(fix_pos_geo[2])
|
||||
fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo
|
||||
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)
|
||||
device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
|
||||
vel_device = device_from_ecef.dot(vel_ecef)
|
||||
vel_device_std = device_from_ecef.dot(vel_ecef_std)
|
||||
orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
|
||||
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
|
||||
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
|
||||
vel_calib = self.calib_from_device.dot(vel_device)
|
||||
vel_calib_std = self.calib_from_device.dot(vel_device_std)
|
||||
acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION])
|
||||
acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR])
|
||||
ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY])
|
||||
ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR])
|
||||
|
||||
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 = 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.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])]
|
||||
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
|
||||
|
||||
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'
|
||||
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
|
||||
|
||||
#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]))
|
||||
#fix.gpsWeek = self.time.week
|
||||
#fix.gpsTimeOfWeek = self.time.tow
|
||||
fix.unixTimestampMillis = self.unix_timestamp_millis
|
||||
|
||||
imu_frame = predicted_state[States.IMU_OFFSET]
|
||||
fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])]
|
||||
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):
|
||||
|
|
@ -75,23 +132,26 @@ class Localizer():
|
|||
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 = euler2quat(initial_pose_ecef)
|
||||
initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef)
|
||||
gps_speed = log.speed
|
||||
quat_uncertainty = 0.2**2
|
||||
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
|
||||
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] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
|
||||
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
|
||||
|
|
@ -119,12 +179,16 @@ class Localizer():
|
|||
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([log.rot, log.rotStd]))
|
||||
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([log.trans, log.transStd]))
|
||||
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
|
||||
|
|
@ -147,6 +211,12 @@ class Localizer():
|
|||
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
|
||||
|
|
@ -160,9 +230,9 @@ class Localizer():
|
|||
|
||||
def locationd_thread(sm, pm, disabled_logs=[]):
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveLocation'])
|
||||
pm = messaging.PubMaster(['liveLocationKalman'])
|
||||
|
||||
localizer = Localizer(disabled_logs=disabled_logs)
|
||||
|
||||
|
|
@ -180,16 +250,17 @@ def locationd_thread(sm, pm, disabled_logs=[]):
|
|||
localizer.handle_car_state(t, sm[sock])
|
||||
elif sock == "cameraOdometry":
|
||||
localizer.handle_cam_odo(t, sm[sock])
|
||||
elif sock == "liveCalibration":
|
||||
localizer.handle_live_calib(t, sm[sock])
|
||||
|
||||
if localizer.filter_ready and sm.updated['gpsLocationExternal']:
|
||||
t = sm.logMonoTime['gpsLocationExternal']
|
||||
msg = messaging.new_message()
|
||||
msg.logMonoTime = t
|
||||
|
||||
msg.init('liveLocation')
|
||||
msg.liveLocation = localizer.liveLocationMsg(t * 1e-9)
|
||||
|
||||
pm.send('liveLocation', msg)
|
||||
msg.init('liveLocationKalman')
|
||||
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
|
||||
pm.send('liveLocationKalman', msg)
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
|
|
|
|||
Loading…
Reference in New Issue