You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
196 lines
7.6 KiB
196 lines
7.6 KiB
#!/usr/bin/env python3
|
|
import math
|
|
from bisect import bisect_right
|
|
|
|
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,
|
|
ned_euler_from_ecef,
|
|
quat2euler,
|
|
rotations_from_quats)
|
|
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
|
|
from selfdrive.locationd.kalman.live_kf import (LiveKalman, initial_P_diag,
|
|
initial_x)
|
|
from selfdrive.locationd.kalman.live_model import States
|
|
from selfdrive.swaglog import cloudlog
|
|
|
|
SENSOR_DECIMATION = 1 # No decimation
|
|
|
|
|
|
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])]
|
|
|
|
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.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])]
|
|
return fix
|
|
|
|
def update_kalman(self, time, kind, meas):
|
|
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:
|
|
if self.filter_ready:
|
|
try:
|
|
self.kf.predict_and_observe(*self.observation_buffer.pop(0))
|
|
except KalmanError:
|
|
cloudlog.error("Error in predict and observe, kalman reset")
|
|
self.reset_kalman()
|
|
else:
|
|
self.observation_buffer.pop(0)
|
|
|
|
def handle_gps(self, current_time, log):
|
|
converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
|
|
fix_ecef = 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 = initial_x
|
|
initial_covs_diag = 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.carState.vEgo == 0:
|
|
self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
|
|
|
|
def handle_cam_odo(self, current_time, log):
|
|
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
|
|
|
|
|
|
def locationd_thread(sm, pm, disabled_logs=[]):
|
|
if sm is None:
|
|
sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry'])
|
|
if pm is None:
|
|
pm = messaging.PubMaster(['liveLocation'])
|
|
|
|
localizer = Localizer(disabled_logs=disabled_logs)
|
|
|
|
while True:
|
|
sm.update()
|
|
|
|
for sock, updated in sm.updated.items():
|
|
if updated:
|
|
t = sm.logMonoTime[sock] * 1e-9
|
|
if sock == "sensorEvents":
|
|
localizer.handle_sensors(t, sm[sock])
|
|
elif sock == "gpsLocationExternal":
|
|
localizer.handle_gps(t, sm[sock])
|
|
elif sock == "carState":
|
|
localizer.handle_car_state(t, sm[sock])
|
|
elif sock == "cameraOdometry":
|
|
localizer.handle_cam_odo(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)
|
|
|
|
|
|
def main(sm=None, pm=None):
|
|
locationd_thread(sm, pm)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
import os
|
|
os.environ["OMP_NUM_THREADS"] = "1"
|
|
main()
|
|
|