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

locationd : Acceleration Bias in locationd (#22879)

Browse Source 
* add accel bias to filter for offline calculation

* bugfix acc bias in live_kf

* add no_accel obsertvation

* increase initial certainty of acc-bias and reduce PN

* increase initial certainty of acc-bias and reduce PN

* increase accel bias PN

* increase obs noise for no_accel observation

* style fixes

* update refs
old-commit-hash: 534bf697ee
commatwo_master
Vivek Aithal 4 years ago committed by GitHub
parent c142416931
commit 27a6dc0c08
5 changed files with 20 additions and 8 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. 1
      selfdrive/locationd/locationd.cc
  2. 1
      selfdrive/locationd/models/constants.py
  3. 4
      selfdrive/locationd/models/live_kf.cc
  4. 20
      selfdrive/locationd/models/live_kf.py
  5. 2
      selfdrive/test/process_replay/ref_commit

1
selfdrive/locationd/locationd.cc
Unescape View File

@ -305,6 +305,7 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re
this->car_speed = std::abs(log.getVEgo());
if (log.getStandstill()) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}

1
selfdrive/locationd/models/constants.py
Unescape View File

@ -29,6 +29,7 @@ class ObservationKind:
PSEUDORANGE_RATE = 23
ECEF_VEL = 31
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]

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 = 23;
this->dim_state_err = 22;
this->dim_state = 26;
this->dim_state_err = 25;
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();

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

@ -29,6 +29,7 @@ class States():
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)
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@ -39,6 +40,7 @@ class States():
ODO_SCALE_ERR = slice(15, 16)
ACCELERATION_ERR = slice(16, 19)
IMU_OFFSET_ERR = slice(19, 22)
ACC_BIAS_ERR = slice(22, 25)
class LiveKalman():
@ -51,6 +53,7 @@ class LiveKalman():
0, 0, 0,
1,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
@ -60,8 +63,9 @@ class LiveKalman():
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
0.02**2,
1**2, 1**2, 1**2,
(0.01)**2, (0.01)**2, (0.01)**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])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
@ -71,7 +75,8 @@ class LiveKalman():
(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.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]),
@ -79,6 +84,7 @@ class LiveKalman():
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]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@ -100,6 +106,7 @@ class LiveKalman():
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')
@ -133,6 +140,7 @@ 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)
@ -183,7 +191,8 @@ class LiveKalman():
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration)
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)
@ -205,7 +214,8 @@ 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_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

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
5703f591ce18bb3aebf67c0594220485f8cb3f79
b3c61dc5f6777497fdd82fb7421a469a43efcbf1
Write Preview
Loading…
Cancel
Save