From 64eb5b0da4d624ecc03f8a45b73296871ae726ff Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Wed, 1 Dec 2021 00:09:00 -0800 Subject: [PATCH] locationd: cleanup (#23088) * remove dead code, avoid indexing by value in locationd * remove dead states in live_kf --- selfdrive/locationd/locationd.cc | 23 ++++++----- selfdrive/locationd/models/live_kf.cc | 4 +- selfdrive/locationd/models/live_kf.py | 37 ++++------------- selfdrive/locationd/models/loc_kf.py | 58 ++++----------------------- 4 files changed, 29 insertions(+), 93 deletions(-) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 0056687e51..8e8641e7d5 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -251,8 +251,8 @@ void Localizer::input_fake_gps_observations(double current_time) { this->kf->predict(current_time); VectorXd current_x = this->kf->get_x(); - VectorXd ecef_pos = current_x.segment<3>(0); - VectorXd ecef_vel = current_x.segment<3>(7); + VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START); + VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START); MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov(); MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov(); @@ -286,7 +286,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal(); this->unix_timestamp_millis = log.getTimestamp(); - double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm(); + double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm(); VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START))); VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef); @@ -410,15 +410,16 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd MatrixXdr current_P = this->kf->get_P(); MatrixXdr init_P = this->kf->get_initial_P(); MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P(); + int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN); - current_x.segment<4>(3) = init_orient; - current_x.segment<3>(7) = init_vel; - current_x.head(3) = init_pos; + current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient; + current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel; + current_x.segment(STATE_ECEF_POS_START) = init_pos; - init_P.block<3,3>(0,0).diagonal() = init_pos_R.diagonal(); - init_P.block<3,3>(3,3).diagonal() = reset_orientation_P.diagonal(); - init_P.block<3,3>(6,6).diagonal() = init_vel_R.diagonal(); - init_P.block<16,16>(9,9).diagonal() = current_P.block<16,16>(9,9).diagonal(); + init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal(); + init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal(); + init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal(); + init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal(); this->reset_kalman(current_time, current_x, init_P); } @@ -478,7 +479,7 @@ void Localizer::determine_gps_mode(double current_time) { // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is - VectorXd current_pos_std = this->kf->get_P().block<3,3>(0,0).diagonal().array().sqrt(); + VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt(); if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){ if (this->gps_mode){ this->gps_mode = false; diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 914f272fd3..5ff0f26995 100755 --- a/selfdrive/locationd/models/live_kf.cc +++ b/selfdrive/locationd/models/live_kf.cc @@ -28,8 +28,8 @@ std::vector> get_vec_mapmat(std::vector& mat_ve } LiveKalman::LiveKalman() { - this->dim_state = 26; - this->dim_state_err = 25; + this->dim_state = live_initial_x.rows(); + this->dim_state_err = live_initial_P_diag.rows(); this->initial_x = live_initial_x; this->initial_P = live_initial_P_diag.asDiagonal(); diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 93aab2774f..fa52945932 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -26,10 +26,8 @@ class States(): ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases - ODO_SCALE = slice(16, 17) # odometer scale - ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2 - IMU_OFFSET = slice(20, 23) # imu offset angles in radians - ACC_BIAS = slice(23, 26) + ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2 + ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2 # Error-state has different slices because it is an ESKF ECEF_POS_ERR = slice(0, 3) @@ -37,10 +35,8 @@ class States(): ECEF_VELOCITY_ERR = slice(6, 9) ANGULAR_VELOCITY_ERR = slice(9, 12) GYRO_BIAS_ERR = slice(12, 15) - ODO_SCALE_ERR = slice(15, 16) - ACCELERATION_ERR = slice(16, 19) - IMU_OFFSET_ERR = slice(19, 22) - ACC_BIAS_ERR = slice(22, 25) + ACCELERATION_ERR = slice(15, 18) + ACC_BIAS_ERR = slice(18, 21) class LiveKalman(): @@ -51,8 +47,6 @@ class LiveKalman(): 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0]) @@ -62,9 +56,7 @@ class LiveKalman(): 10**2, 10**2, 10**2, 1**2, 1**2, 1**2, 1**2, 1**2, 1**2, - 0.02**2, 100**2, 100**2, 100**2, - 0.01**2, 0.01**2, 0.01**2, 0.01**2, 0.01**2, 0.01**2]) # state covariance when resetting midway in a segment @@ -80,16 +72,12 @@ class LiveKalman(): 0.01**2, 0.01**2, 0.01**2, 0.1**2, 0.1**2, 0.1**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, - (0.02 / 100)**2, 3**2, 3**2, 3**2, - (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2, 0.005**2, 0.005**2, 0.005**2]) - obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]), - ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), + obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]), - ObservationKind.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]), ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]), ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]), ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]), @@ -112,7 +100,6 @@ class LiveKalman(): vroll, vpitch, vyaw = omega roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] acceleration = state[States.ACCELERATION, :] - imu_angles = state[States.IMU_OFFSET, :] acc_bias = state[States.ACC_BIAS, :] dt = sp.Symbol('dt') @@ -147,7 +134,6 @@ class LiveKalman(): omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] acceleration_err = state_err[States.ACCELERATION_ERR, :] - # Time derivative of the state error as a function of state error and state quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) @@ -190,7 +176,6 @@ class LiveKalman(): # # Observation functions # - # imu_rot = euler_rotate(*imu_angles) h_gyro_sym = sp.Matrix([ vroll + roll_bias, vpitch + pitch_bias, @@ -201,19 +186,12 @@ class LiveKalman(): h_acc_sym = (gravity + acceleration + acc_bias) h_acc_stationary_sym = acceleration h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) - - speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6) - h_speed_sym = sp.Matrix([speed]) - h_pos_sym = sp.Matrix([x, y, z]) h_vel_sym = sp.Matrix([vx, vy, vz]) h_orientation_sym = q - h_imu_frame_sym = sp.Matrix(imu_angles) - h_relative_motion = sp.Matrix(quat_rot.T * v) - obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], - [h_gyro_sym, ObservationKind.PHONE_GYRO, None], + obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_pos_sym, ObservationKind.ECEF_POS, None], @@ -221,12 +199,11 @@ class LiveKalman(): [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, 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], [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] # this returns a sympy routine for the jacobian of the observation function of the local vel in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz - h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T*(sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) + h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]])) extra_routines = [('H', h.jacobian(in_vec), [in_vec])] gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines) diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index 48e309d9c6..adb9098d4d 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -41,14 +41,14 @@ class States(): CLOCK_BIAS = slice(13, 14) # clock bias in light-meters, CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s, GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases - ODO_SCALE = slice(18, 19) # odometer scale + ODO_SCALE_UNUSED = slice(18, 19) # odometer scale ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2 - FOCAL_SCALE = slice(22, 23) # focal length scale + FOCAL_SCALE_UNUSED = slice(22, 23) # focal length scale IMU_OFFSET = slice(23, 26) # imu offset angles in radians GLONASS_BIAS = slice(26, 27) # GLONASS bias in m expressed as bias + freq_num*freq_slope GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2, - ACCELEROMETER_SCALE = slice(29, 30) # scale of mems accelerometer + ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET). # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE @@ -61,14 +61,14 @@ class States(): CLOCK_BIAS_ERR = slice(12, 13) CLOCK_DRIFT_ERR = slice(13, 14) GYRO_BIAS_ERR = slice(14, 17) - ODO_SCALE_ERR = slice(17, 18) + ODO_SCALE_ERR_UNUSED = slice(17, 18) ACCELERATION_ERR = slice(18, 21) - FOCAL_SCALE_ERR = slice(21, 22) + FOCAL_SCALE_ERR_UNUSED = slice(21, 22) IMU_OFFSET_ERR = slice(22, 25) GLONASS_BIAS_ERR = slice(25, 26) GLONASS_FREQ_SLOPE_ERR = slice(26, 27) CLOCK_ACCELERATION_ERR = slice(27, 28) - ACCELEROMETER_SCALE_ERR = slice(28, 29) + ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29) ACCELEROMETER_BIAS_ERR = slice(29, 32) @@ -152,9 +152,7 @@ class LocKalman(): cb = state[States.CLOCK_BIAS, :] cd = state[States.CLOCK_DRIFT, :] roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] - odo_scale = state[States.ODO_SCALE, :] acceleration = state[States.ACCELERATION, :] - focal_scale = state[States.FOCAL_SCALE, :] imu_angles = state[States.IMU_OFFSET, :] imu_angles[0, 0] = 0 imu_angles[2, 0] = 0 @@ -258,13 +256,10 @@ class LocKalman(): sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1) sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1) # sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1) - orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1) # expand extra args sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:] - # los_x, los_y, los_z = sat_los_sym - orb_x, orb_y, orb_z = orb_epos_sym h_pseudorange_sym = sp.Matrix([ sp.sqrt( @@ -300,35 +295,17 @@ class LocKalman(): h_acc_sym = imu_rot * (gravity + acceleration + accel_bias) h_acc_stationary_sym = acceleration h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) - - speed = sp.sqrt(vx**2 + vy**2 + vz**2) - h_speed_sym = sp.Matrix([speed * odo_scale]) - - # orb stuff - orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z]) - orb_pos_rot_sym = quat_rot.T * orb_pos_sym - s = orb_pos_rot_sym[0] - h_orb_point_sym = sp.Matrix([(1 / s) * (orb_pos_rot_sym[1]), - (1 / s) * (orb_pos_rot_sym[2])]) - - h_pos_sym = sp.Matrix([x, y, z]) - h_imu_frame_sym = sp.Matrix(imu_angles) - h_relative_motion = sp.Matrix(quat_rot.T * v) - obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], - [h_gyro_sym, ObservationKind.PHONE_GYRO, None], + obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym], [h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym], [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym], [h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym], - [h_pos_sym, ObservationKind.ECEF_POS, 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], - [h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym], [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] # MSCKF configuration @@ -392,7 +369,7 @@ class LocKalman(): self.computer = LstSqComputer(generated_dir, N) self.max_tracks = max_tracks - self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)] + self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)] # init filter self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err, @@ -450,14 +427,10 @@ class LocKalman(): r = self.predict_and_update_pseudorange(data, t, kind) elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS: r = self.predict_and_update_pseudorange_rate(data, t, kind) - elif kind == ObservationKind.ORB_POINT: - r = self.predict_and_update_orb(data, t, kind) elif kind == ObservationKind.ORB_FEATURES: r = self.predict_and_update_orb_features(data, t, kind) elif kind == ObservationKind.MSCKF_TEST: r = self.predict_and_update_msckf_test(data, t, kind) - elif kind == ObservationKind.ODOMETRIC_SPEED: - 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 @@ -497,21 +470,6 @@ class LocKalman(): z[i, :] = z_i return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel) - def predict_and_update_orb(self, orb, t, kind): - true_pos = orb[:, 2:] - z = orb[:, :2] - R = np.zeros((len(orb), 2, 2)) - for i, _ in enumerate(z): - R[i, :, :] = np.diag([10**2, 10**2]) - return self.filter.predict_and_update_batch(t, kind, z, R, true_pos) - - 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]) - return self.filter.predict_and_update_batch(t, kind, z, R) - def predict_and_update_odo_trans(self, trans, t, kind): z = trans[:, :3] R = np.zeros((len(trans), 3, 3))