diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 6b9106db..7075fdf3 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -147,20 +147,21 @@ class Localizer(): #fix.gpsTimeOfWeek = self.time.tow fix.unixTimestampMillis = self.unix_timestamp_millis - if np.limalg.norm(fix.positionECEF.std) < 50 and self.calibrated: + if self.filter_ready and self.calibrated: fix.status = 'valid' - elif np.limalg.norm(fix.positionECEF.std) < 50: + elif self.filter_ready: fix.status = 'uncalibrated' else: fix.status = 'uninitialized' return fix def update_kalman(self, time, kind, meas): - try: - self.kf.predict_and_observe(time, kind, meas) - except KalmanError: - cloudlog.error("Error in predict and observe, kalman reset") - self.reset_kalman() + 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: @@ -170,19 +171,40 @@ class Localizer(): 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]) - vel_ecef = self.converter.ned2ecef_matrix.dot(np.array(log.vNED)) #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) self.unix_timestamp_millis = log.timestamp - 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(current_time) - self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef) - self.update_kalman(current_time, ObservationKind.ECEF_VEL, vel_ecef) + # 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_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 @@ -234,9 +256,9 @@ class Localizer(): self.calib_from_device = self.device_from_calib.T self.calibrated = log.calStatus == 1 - def reset_kalman(self, current_time=None): - self.filter_time = current_time - self.kf.init_state(self.kf.x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time) + def reset_kalman(self): + self.filter_time = None + self.filter_ready = False self.observation_buffer = [] self.gyro_counter = 0 @@ -270,7 +292,7 @@ def locationd_thread(sm, pm, disabled_logs=[]): elif sock == "liveCalibration": localizer.handle_live_calib(t, sm[sock]) - if sm.updated['gpsLocationExternal']: + if localizer.filter_ready and sm.updated['gpsLocationExternal']: t = sm.logMonoTime['gpsLocationExternal'] msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py index ab2a2252..8b99fce6 100644 --- a/selfdrive/locationd/models/constants.py +++ b/selfdrive/locationd/models/constants.py @@ -27,7 +27,6 @@ class ObservationKind: PSEUDORANGE_RATE_GLONASS = 21 PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 - ECEF_VEL = 31 ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] ROAD_FRAME_YAW_RATE = 25 # [rad/s] @@ -37,7 +36,6 @@ class ObservationKind: STEER_RATIO = 29 # [-] ROAD_FRAME_X_SPEED = 30 # (x) [m/s] - names = [ 'Unknown', 'No observation', diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 1b40724c..299d6b25 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -172,7 +172,6 @@ class LiveKalman(): h_speed_sym = sp.Matrix([speed * odo_scale]) h_pos_sym = sp.Matrix([x, y, z]) - h_vel_sym = sp.Matrix([vx, vy, vz]) h_imu_frame_sym = sp.Matrix(imu_angles) h_relative_motion = sp.Matrix(quat_rot.T * v) @@ -182,7 +181,6 @@ class LiveKalman(): [h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_pos_sym, ObservationKind.ECEF_POS, None], - [h_vel_sym, ObservationKind.ECEF_VEL, None], [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] @@ -199,8 +197,7 @@ class LiveKalman(): ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), 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]), - ObservationKind.ECEF_VEL: np.diag([1**2, 1**2, 1**2])} + ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])} # init filter self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)