mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-23 06:52:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Full localizer: Use standard naming conventions (#25007)

Browse Source 
Use standard naming conventions
This commit is contained in:
HaraldSchafer
2022-06-30 14:49:17 -07:00
committed by GitHub
parent c49f997be5
commit dd43ae2856
1 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/loc_kf.py
+16 -16
View File
@@ -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 @@ class LocKalman():
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 @@ class LocKalman():
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 @@ class LocKalman():
[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 @@ class LocKalman():
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 @@ class LocKalman():
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]),