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

Full localizer: Use standard naming conventions (#25007)

Browse Source 
Use standard naming conventions
pull/83/head
HaraldSchafer 3 years ago committed by GitHub
parent c49f997be5
commit dd43ae2856
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 16 additions and 16 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. 32
      selfdrive/locationd/models/loc_kf.py

32
selfdrive/locationd/models/loc_kf.py
Unescape 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]),

Write Preview
Loading…
Cancel
Save