From dd43ae2856e144ec13eff462556d8e681aa57789 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 30 Jun 2022 14:49:17 -0700 Subject: [PATCH] Full localizer: Use standard naming conventions (#25007) Use standard naming conventions --- selfdrive/locationd/models/loc_kf.py | 32 ++++++++++++++-------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index 6b08828695a4eb..b1ed8599f1e08b 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -25,15 +25,15 @@ class States(): ODO_SCALE_UNUSED = slice(18, 19) # odometer scale ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2 FOCAL_SCALE_UNUSED = slice(22, 23) # focal length scale - IMU_OFFSET = slice(23, 26) # imu offset angles in radians + IMU_FROM_DEVICE_EULER = 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_UNUSED = slice(29, 30) # scale of mems accelerometer ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer # TODO the offset is likely a translation of the sensor, not a rotation of the camera - WIDE_CAM_OFFSET = slice(33, 36) # wide camera offset angles in radians (tici only) - # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET). + WIDE_FROM_DEVICE_EULER = slice(33, 36) # wide camera offset angles in radians (tici only) + # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_FROM_DEVICE_EULER). # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE # Error-state has different slices because it is an ESKF @@ -47,13 +47,13 @@ class States(): ODO_SCALE_ERR_UNUSED = slice(17, 18) ACCELERATION_ERR = slice(18, 21) FOCAL_SCALE_ERR_UNUSED = slice(21, 22) - IMU_OFFSET_ERR = slice(22, 25) + IMU_FROM_DEVICE_EULER_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_UNUSED = slice(28, 29) ACCELEROMETER_BIAS_ERR = slice(29, 32) - WIDE_CAM_OFFSET_ERR = slice(32, 35) + WIDE_FROM_DEVICE_EULER_ERR = slice(32, 35) class LocKalman(): @@ -140,15 +140,15 @@ def generate_code(generated_dir, N=4): cd = state[States.CLOCK_DRIFT, :] roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] acceleration = state[States.ACCELERATION, :] - imu_angles = state[States.IMU_OFFSET, :] - imu_angles[0, 0] = 0 # not observable enough - imu_angles[2, 0] = 0 # not observable enough + imu_from_device_euler = state[States.IMU_FROM_DEVICE_EULER, :] + imu_from_device_euler[0, 0] = 0 # not observable enough + imu_from_device_euler[2, 0] = 0 # not observable enough glonass_bias = state[States.GLONASS_BIAS, :] glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :] ca = state[States.CLOCK_ACCELERATION, :] accel_bias = state[States.ACCELEROMETER_BIAS, :] - wide_cam_angles = state[States.WIDE_CAM_OFFSET, :] - wide_cam_angles[0, 0] = 0 # not observable enough + wide_from_device_euler = state[States.WIDE_FROM_DEVICE_EULER, :] + wide_from_device_euler[0, 0] = 0 # not observable enough dt = sp.Symbol('dt') @@ -273,15 +273,15 @@ def generate_code(generated_dir, N=4): los_vector[2] * (sat_vz - vz) + cd[0]]) - imu_rot = euler_rotate(*imu_angles) - h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias, + imu_from_device = euler_rotate(*imu_from_device_euler) + h_gyro_sym = imu_from_device * sp.Matrix([vroll + roll_bias, vpitch + pitch_bias, vyaw + yaw_bias]) pos = sp.Matrix([x, y, z]) # add 1 for stability, prevent division by 0 gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos) - h_acc_sym = imu_rot * (gravity + acceleration + accel_bias) + h_acc_sym = imu_from_device * (gravity + acceleration + accel_bias) h_acc_stationary_sym = acceleration h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) h_relative_motion = sp.Matrix(quat_rot.T * v) @@ -297,7 +297,7 @@ def generate_code(generated_dir, N=4): [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] - wide_cam_rot = euler_rotate(*wide_cam_angles) + wide_from_device = euler_rotate(*wide_from_device_euler) # MSCKF configuration if N > 0: # experimentally found this is correct value for imx298 with 910 focal length @@ -312,7 +312,7 @@ def generate_code(generated_dir, N=4): track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) track_pos_rot_sym = quat_rot.T * track_pos_sym - track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym + track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) h_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), @@ -329,7 +329,7 @@ def generate_code(generated_dir, N=4): quat_rot = quat_rotate(*q) track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z]) track_pos_rot_sym = quat_rot.T * track_pos_sym - track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym + track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_sym h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]), focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])]) h_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]),