diff --git a/selfdrive/locationd/kalman/live_kf.py b/selfdrive/locationd/kalman/live_kf.py index a070ebac7d..d18a4b0896 100755 --- a/selfdrive/locationd/kalman/live_kf.py +++ b/selfdrive/locationd/kalman/live_kf.py @@ -6,20 +6,18 @@ from .kalman_helpers import ObservationKind from .ekf_sym import EKF_sym - -class LiveKalman(): - def __init__(self, N=0, max_tracks=3000): - x_initial = np.array([-2.7e6, 4.2e6, 3.8e6, - 1, 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 1, - 0, 0, 0, - 0, 0, 0]) - - # state covariance - P_initial = np.diag([10000**2, 10000**2, 10000**2, +initial_x = np.array([-2.7e6, 4.2e6, 3.8e6, + 1, 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, + 0, 0, 0, + 0, 0, 0]) + + +# state covariance +initial_P_diag = np.array([10000**2, 10000**2, 10000**2, 10**2, 10**2, 10**2, 10**2, 10**2, 10**2, 1**2, 1**2, 1**2, @@ -28,6 +26,11 @@ class LiveKalman(): 1**2, 1**2, 1**2, (0.01)**2, (0.01)**2, (0.01)**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, @@ -52,7 +55,7 @@ class LiveKalman(): gen_model(name, self.dim_state, self.dim_state_err) # init filter - self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_state, self.dim_state_err) + self.filter = EKF_sym(name, Q, initial_x, np.diag(initial_P_diag), self.dim_state, self.dim_state_err) @property def x(self): diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 567ff0149f..a5620e4dfc 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -5,26 +5,18 @@ import numpy as np from bisect import bisect_right import cereal.messaging as messaging from selfdrive.swaglog import cloudlog -from cereal.services import service_list from common.transformations.orientation import rotations_from_quats, ecef_euler_from_ned, euler2quat, ned_euler_from_ecef, quat2euler import common.transformations.coordinates as coord -import laika.raw_gnss as gnss -from laika.astro_dog import AstroDog - -from selfdrive.locationd.kalman.loc_kf import LocKalman +from selfdrive.locationd.kalman.live_kf import LiveKalman, States, inital_x, initial_P_diag from selfdrive.locationd.kalman.kalman_helpers import ObservationKind os.environ["OMP_NUM_THREADS"] = "1" class Localizer(): def __init__(self, disabled_logs=[], dog=None): - self.kf = LocKalman(0) + self.kf = LiveKalman() self.reset_kalman() - if dog: - self.dog = dog - else: - self.dog = AstroDog(auto_update=True) self.max_age = .2 # seconds self.disabled_logs = disabled_logs self.week = None @@ -32,32 +24,31 @@ class Localizer(): def liveLocationMsg(self, time): fix = messaging.log.LiveLocationData.new_message() predicted_state = self.kf.x - fix_ecef = predicted_state[0:3] + 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[7:10])) + fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) - orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[3:7])) + 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.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[3:7]).T.dot(predicted_state[7:10]) + 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.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])] + #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)) - #print len(self.observation_buffer), idx, self.kf.filter.filter_time, time 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)) @@ -67,38 +58,31 @@ class Localizer(): 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]) - # initing with bad bearing allowed, maybe bad? - if not self.filter_ready and len(list(self.dog.orbits.keys())) >6: # and log.gpsLocationExternal.speed > 5: + # 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 - initial_state = np.zeros(29) gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180) 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 quat_uncertainty = 0.2**2 initial_pose_ecef_quat = euler2quat(initial_pose_ecef) - initial_state[:3] = initial_ecef - initial_state[3:7] = initial_pose_ecef_quat - initial_state[7:10] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) - initial_state[18] = 1 - initial_state[22] = 1 - covs_diag = np.array([10**2,10**2,10**2, - quat_uncertainty, quat_uncertainty, quat_uncertainty, - 2**2, 2**2, 2**2, - 1, 1, 1, - 20000000**2, 100**2, - 0.01**2, 0.01**2, 0.01**2, - 0.02**2, - 2**2, 2**2, 2**2, - .01**2, - 0.01**2, 0.01**2, 0.01**2, - 10**2, 1**2, - 0.2**2]) - self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time) + + initial_state = inital_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) print("Filter initialized") elif self.filter_ready: - #self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef) + 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) @@ -113,34 +97,11 @@ class Localizer(): if log.carState.vEgo == 0: self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0]) - def handle_ublox_gnss(self, log, current_time): - if hasattr(log.ubloxGnss, 'measurementReport'): - self.raw_gnss_counter += 1 - if True or self.raw_gnss_counter % 3==0: - processed_raw = gnss.process_measurements(gnss.read_raw_ublox(log.ubloxGnss.measurementReport), dog=self.dog) - corrected_raw = gnss.correct_measurements(processed_raw, self.kf.x[:3], dog=self.dog) - corrected_raw = np.array([c.as_array() for c in corrected_raw]).reshape((-1,14)) - self.update_kalman(current_time, ObservationKind.PSEUDORANGE_GPS, corrected_raw) - self.update_kalman(current_time, ObservationKind.PSEUDORANGE_RATE_GPS, corrected_raw) - #elif hasattr(log.ubloxGnss, 'ephemeris'): - # self.dog.add_ublox_ephems([log]) - # if len(self.dog.orbits.keys()) < 6: - # print 'Added ublox ephem now has ', len(self.dog.orbits.keys()) - - def handle_qcom_gnss(self, log, current_time): - if hasattr(log.qcomGnss, 'drSvPoly') and self.week is not None: - self.dog.add_qcom_ephems([log], self.week) - if len(list(self.dog.orbits.keys())) < 6: - print('Added qcom ephem now has ', len(list(self.dog.orbits.keys()))) - if hasattr(log.qcomGnss, 'drMeasurementReport') and log.qcomGnss.drMeasurementReport.source == "gps": - self.week = log.qcomGnss.drMeasurementReport.gpsWeek - 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])) - pass def handle_sensors(self, log, current_time): for sensor_reading in log.sensorEvents: @@ -148,7 +109,7 @@ class Localizer(): if sensor_reading.type == 4: self.gyro_counter += 1 if True or self.gyro_counter % 5==0: - if max(abs(self.kf.x[23:26])) > 0.07: + if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07: print('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]]) @@ -168,10 +129,6 @@ class Localizer(): self.handle_gps(log, current_time) elif typ == "carState": self.handle_car_state(log, current_time) - elif typ == "ubloxGnss": - self.handle_ublox_gnss(log, current_time) - elif typ == "qcomGnss": - self.handle_qcom_gnss(log, current_time) elif typ == "cameraOdometry": self.handle_cam_odo(log, current_time) @@ -182,18 +139,16 @@ class Localizer(): self.converter = None self.gyro_counter = 0 self.acc_counter = 0 - self.raw_gnss_counter = 0 self.speed_counter = 0 -def locationd_thread(gctx, addr, disabled_logs): +def locationd_thread(gctx, addr, disabled_logs=[]): poller = zmq.Poller() #carstate = messaging.sub_sock('carState', poller, addr=addr, conflate=True) gpsLocationExternal = messaging.sub_sock('gpsLocationExternal', poller, addr=addr, conflate=True) - ubloxGnss = messaging.sub_sock('ubloxGnss', poller, addr=addr, conflate=True) - qcomGnss = messaging.sub_sock('qcomGnss', 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) liveLocation = messaging.pub_sock('liveLocation') @@ -219,13 +174,7 @@ def locationd_thread(gctx, addr, disabled_logs): def main(gctx=None, addr="127.0.0.1"): - IN_CAR = os.getenv("IN_CAR", False) - disabled_logs = os.getenv("DISABLED_LOGS", "").split(",") - # No speed for now - disabled_logs.append('carState') - if IN_CAR: - addr = "192.168.5.11" - locationd_thread(gctx, addr, disabled_logs) + locationd_thread(gctx, addr) if __name__ == "__main__":