diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index eb67d7e15d..8bdc30a8d2 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -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) }); } } diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py index ebe83186e4..7450f76be7 100644 --- a/selfdrive/locationd/models/constants.py +++ b/selfdrive/locationd/models/constants.py @@ -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] diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc index 4bc655821f..541c0f7730 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 = 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(); diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 7db4ecc261..c67644ff1a 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ec46d5e43f..d58be73f57 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5703f591ce18bb3aebf67c0594220485f8cb3f79 \ No newline at end of file +b3c61dc5f6777497fdd82fb7421a469a43efcbf1 \ No newline at end of file