spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

locationd: cleanup (#23088) 

* remove dead code, avoid indexing by value in locationd

* remove dead states in live_kf
pull/23092/head
Vivek Aithal 2021-12-01 00:09:00 -08:00 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
selfdrive/locationd/locationd.cc
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
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();

37
selfdrive/locationd/models/live_kf.py
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,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)

58
selfdrive/locationd/models/loc_kf.py
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
@ -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))