sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

locationd: cleanup (#23088)

Browse Source 
* remove dead code, avoid indexing by value in locationd

* remove dead states in live_kf
pull/23092/head
Vivek Aithal 3 years ago committed by GitHub
parent 077ec6725a
commit 64eb5b0da4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 29 additions and 93 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 23
      selfdrive/locationd/locationd.cc
  2. 4
      selfdrive/locationd/models/live_kf.cc
  3. 35
      selfdrive/locationd/models/live_kf.py
  4. 56
      selfdrive/locationd/models/loc_kf.py

23
selfdrive/locationd/locationd.cc
Unescape View File

@ -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_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(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_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(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_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(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_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(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_LEN, STATE_ECEF_POS_ERR_LEN>(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;

4
selfdrive/locationd/models/live_kf.cc
Unescape View File

@ -28,8 +28,8 @@ std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& 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();

35
selfdrive/locationd/models/live_kf.py
Unescape View File

@ -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,7 +199,6 @@ 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

56
selfdrive/locationd/models/loc_kf.py
Unescape View File

@ -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
@ -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))

Write Preview
Loading…
Cancel
Save