diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/kalman_helpers.py index b4d22dce7f..c4dd4fdd86 100644 --- a/selfdrive/locationd/kalman/kalman_helpers.py +++ b/selfdrive/locationd/kalman/kalman_helpers.py @@ -4,6 +4,10 @@ from bisect import bisect from tqdm import tqdm +class KalmanError(Exception): + pass + + class ObservationKind(): UNKNOWN = 0 NO_OBSERVATION = 1 diff --git a/selfdrive/locationd/kalman/live_kf.py b/selfdrive/locationd/kalman/live_kf.py index 73476fa5ae..8ea2f16cbc 100755 --- a/selfdrive/locationd/kalman/live_kf.py +++ b/selfdrive/locationd/kalman/live_kf.py @@ -1,9 +1,10 @@ #!/usr/bin/env python3 import numpy as np -from .live_model import gen_model, States -from .kalman_helpers import ObservationKind -from .ekf_sym import EKF_sym +from selfdrive.swaglog import cloudlog +from selfdrive.locationd.kalman.live_model import gen_model +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError +from selfdrive.locationd.kalman.ekf_sym import EKF_sym initial_x = np.array([-2.7e6, 4.2e6, 3.8e6, @@ -29,8 +30,6 @@ initial_P_diag = np.array([10000**2, 10000**2, 10000**2, class LiveKalman(): def __init__(self, N=0, max_tracks=3000): - - # process noise Q = np.diag([0.03**2, 0.03**2, 0.03**2, 0.0**2, 0.0**2, 0.0**2, @@ -42,6 +41,9 @@ class LiveKalman(): 0.001**2, (0.05/60)**2, (0.05/60)**2, (0.05/60)**2]) + self.dim_state = initial_x.shape[0] + self.dim_state_err = initial_P_diag.shape[0] + self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]), @@ -50,9 +52,8 @@ class LiveKalman(): ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])} - - name = 'live' % N - gen_model(name, self.dim_state, self.dim_state_err) + name = f'live_{N}' + gen_model(name, self.dim_state, self.dim_state_err, []) # init filter self.filter = EKF_sym(name, Q, initial_x, np.diag(initial_P_diag), self.dim_state, self.dim_state_err) @@ -95,12 +96,17 @@ class LiveKalman(): r = self.predict_and_update_odo_speed(data, t, kind) else: r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + # Normalize quats - quat_norm = np.linalg.norm(self.filter.x[3:7,0]) + quat_norm = np.linalg.norm(self.filter.x[3:7, 0]) + # Should not continue if the quats behave this weirdly - if not 0.1 < quat_norm < 10: - raise RuntimeError("Sir! The filter's gone all wobbly!") - self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm + if not (0.1 < quat_norm < 10): + cloudlog.error("Kalman filter quaternions unstable") + raise KalmanError + + self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm + return r def get_R(self, kind, n): @@ -108,28 +114,28 @@ class LiveKalman(): dim = obs_noise.shape[0] R = np.zeros((n, dim, dim)) for i in range(n): - R[i,:,:] = obs_noise + R[i, :, :] = obs_noise return R def predict_and_update_odo_speed(self, speed, t, kind): z = np.array(speed) R = np.zeros((len(speed), 1, 1)) for i, _ in enumerate(z): - R[i,:,:] = np.diag([0.2**2]) + R[i, :, :] = np.diag([0.2**2]) return self.filter.predict_and_update_batch(t, kind, z, R) def predict_and_update_odo_trans(self, trans, t, kind): - z = trans[:,:3] + z = trans[:, :3] R = np.zeros((len(trans), 3, 3)) for i, _ in enumerate(z): - R[i,:,:] = np.diag(trans[i,3:]**2) + R[i, :, :] = np.diag(trans[i, 3:]**2) return self.filter.predict_and_update_batch(t, kind, z, R) def predict_and_update_odo_rot(self, rot, t, kind): - z = rot[:,:3] + z = rot[:, :3] R = np.zeros((len(rot), 3, 3)) for i, _ in enumerate(z): - R[i,:,:] = np.diag(rot[i,3:]**2) + R[i, :, :] = np.diag(rot[i, 3:]**2) return self.filter.predict_and_update_batch(t, kind, z, R) diff --git a/selfdrive/locationd/kalman/live_model.py b/selfdrive/locationd/kalman/live_model.py index db4340cd56..0e3a7290f3 100644 --- a/selfdrive/locationd/kalman/live_model.py +++ b/selfdrive/locationd/kalman/live_model.py @@ -3,9 +3,9 @@ import sympy as sp import os from laika.constants import EARTH_GM -from .kalman_helpers import ObservationKind -from .ekf_sym import gen_code from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r +from selfdrive.locationd.kalman.kalman_helpers import ObservationKind +from selfdrive.locationd.kalman.ekf_sym import gen_code class States(): @@ -28,11 +28,7 @@ class States(): IMU_OFFSET_ERR = slice(19, 22) -def gen_model(name, - dim_state, dim_state_err, - maha_test_kinds): - - +def gen_model(name, dim_state, dim_state_err, maha_test_kinds): # check if rebuild is needed try: dir_path = os.path.dirname(__file__) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 239ed29435..77334fdebd 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -1,16 +1,23 @@ #!/usr/bin/env python3 -import os -import zmq -import numpy as np +import math from bisect import bisect_right + +import numpy as np + import cereal.messaging as messaging -from selfdrive.swaglog import cloudlog -from common.transformations.orientation import rotations_from_quats, ecef_euler_from_ned, euler2quat, ned_euler_from_ecef, quat2euler 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 -from selfdrive.locationd.kalman.live_kf import LiveKalman, States, initial_x, initial_P_diag -from selfdrive.locationd.kalman.kalman_helpers import ObservationKind -os.environ["OMP_NUM_THREADS"] = "1" +SENSOR_DECIMATION = 1 # No decimation class Localizer(): @@ -19,11 +26,12 @@ class Localizer(): self.reset_kalman() self.max_age = .2 # seconds self.disabled_logs = disabled_logs - self.week = None 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]) @@ -33,36 +41,42 @@ class Localizer(): 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 = float(orientation_ned_euler[0]*180/np.pi) - fix.pitch = float(orientation_ned_euler[1]*180/np.pi) - fix.heading = float(orientation_ned_euler[2]*180/np.pi) + 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 = float((np.arctan2(local_vel[2], local_vel[0]))*180/np.pi) - fix.yawCalibration = float((np.arctan2(local_vel[1], local_vel[0]))*180/np.pi) + fix.pitchCalibration = math.degrees(math.arctan2(local_vel[2], local_vel[0])) + fix.yawCalibration = math.degrees(math.arctan2(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 self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: if self.filter_ready: - self.kf.predict_and_observe(*self.observation_buffer.pop(0)) + 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, log, current_time): - self.converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude]) - fix_ecef = self.converter.ned2ecef([0,0,0]) + converter = coord.LocalCoord.from_geodetic([log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.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.gpsLocationExternal.speed > 5: self.filter_ready = True initial_ecef = fix_ecef - gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180) + gps_bearing = math.radians(log.gpsLocationExternal.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.gpsLocationExternal.speed @@ -80,49 +94,62 @@ class Localizer(): 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) - print("Filter initialized") + 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.info("Locationd vs ubloxLocation difference too large, kalman reset") + cloudlog.error("Locationd vs ubloxLocation difference too large, kalman reset") self.reset_kalman() def handle_car_state(self, log, current_time): self.speed_counter += 1 - if self.speed_counter % 5==0: + + if self.speed_counter % SENSOR_DECIMATION == 0: self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, log.carState.vEgo) if log.carState.vEgo == 0: self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) def handle_cam_odo(self, log, current_time): - self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot, - log.cameraOdometry.rotStd])) - self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans, - log.cameraOdometry.transStd])) + self.update_kalman(current_time, + ObservationKind.CAMERA_ODO_ROTATION, + np.concatenate([log.cameraOdometry.rot, log.cameraOdometry.rotStd])) + self.update_kalman(current_time, + ObservationKind.CAMERA_ODO_TRANSLATION, + np.concatenate([log.cameraOdometry.trans, log.cameraOdometry.transStd])) def handle_sensors(self, log, current_time): + # TODO does not yet account for double sensor readings in the log for sensor_reading in log.sensorEvents: - # TODO does not yet account for double sensor readings in the log - if sensor_reading.type == 4: + # Gyro Uncalibrated + if sensor_reading.sensor == 5 and sensor_reading.type == 16: self.gyro_counter += 1 - if True or self.gyro_counter % 5==0: + if self.gyro_counter % SENSOR_DECIMATION == 0: if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07: - print('imu frame angles exceeded, correcting') + cloudlog.info('imu frame angles exceeded, correcting') self.update_kalman(current_time, ObservationKind.IMU_FRAME, [0, 0, 0]) - self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]]) - if sensor_reading.type == 1: + + 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 True or self.acc_counter % 5==0: - self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-sensor_reading.acceleration.v[2], -sensor_reading.acceleration.v[1], -sensor_reading.acceleration.v[0]]) + 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_log(self, log): - current_time = 1e-9*log.logMonoTime + current_time = 1e-9 * log.logMonoTime + typ = log.which + if typ in self.disabled_logs: return + if typ == "sensorEvents": self.handle_sensors(log, current_time) elif typ == "gpsLocationExternal": @@ -136,46 +163,43 @@ class Localizer(): self.filter_time = None self.filter_ready = False self.observation_buffer = [] - self.converter = None + self.gyro_counter = 0 self.acc_counter = 0 self.speed_counter = 0 -def locationd_thread(gctx, addr, disabled_logs=[]): - poller = zmq.Poller() +def locationd_thread(sm, pm, disabled_logs=[]): + if sm is None: + sm = messaging.SubMaster(['carState', 'gpsLocationExternal', 'sensorEvents', 'cameraOdometry']) + if pm is None: + pm = messaging.PubMaster(['liveLocation']) - #carstate = messaging.sub_sock('carState', poller, addr=addr, conflate=True) - gpsLocationExternal = messaging.sub_sock('gpsLocationExternal', poller, addr=addr, conflate=True) - sensorEvents = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True) - cameraOdometry = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True) + localizer = Localizer(disabled_logs=disabled_logs) - liveLocation = messaging.pub_sock('liveLocation') + while True: + sm.update() - localizer = Localizer(disabled_logs=disabled_logs) - print("init done") - - # buffer with all the messages that still need to be input into the kalman - while 1: - polld = poller.poll(timeout=1000) - for sock, mode in polld: - if mode != zmq.POLLIN: - continue - logs = messaging.drain_sock(sock) - for log in logs: - localizer.handle_log(log) - - if localizer.filter_ready and log.which == 'ubloxGnss': + for sock, updated in sm.updated.items(): + if updated: + localizer.handle_log(sm[sock]) + + if localizer.filter_ready and sm.updated['gpsLocationExternal']: + t = sm.logMonoTime['gpsLocationExternal'] msg = messaging.new_message() - msg.logMonoTime = log.logMonoTime + msg.logMonoTime = t + msg.init('liveLocation') - msg.liveLocation = localizer.liveLocationMsg(log.logMonoTime*1e-9) - liveLocation.send(msg.to_bytes()) + msg.liveLocation = localizer.liveLocationMsg(t * 1e-9) + + pm.send('liveLocation', msg) -def main(gctx=None, addr="127.0.0.1"): - locationd_thread(gctx, addr) +def main(sm=None, pm=None): + locationd_thread(sm, pm) if __name__ == "__main__": + import os + os.environ["OMP_NUM_THREADS"] = "1" main()