selfdrive.locationd.models package

Submodules

selfdrive.locationd.models.car_kf module

class selfdrive.locationd.models.car_kf.CarKalman(generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None)[source]

Bases: KalmanFilter

P_initial = array([[2.50000000e-07, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 1.00000000e-04, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 1.21846968e-07, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.90385887e-05,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         1.00000000e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 1.00000000e-04, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 3.04617420e-06, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 3.04617420e-06,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         3.04617420e-04]])
Q = array([[2.50000000e-07, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 1.00000000e-04, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 1.21846968e-07, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.90385887e-05,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         1.00000000e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 1.00000000e-04, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 3.04617420e-06, 0.00000000e+00,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 3.04617420e-06,         0.00000000e+00],        [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,         3.04617420e-04]])
static generate_code(generated_dir)[source]
global_vars = ['mass', 'rotational_inertia', 'center_to_front', 'center_to_rear', 'stiffness_front', 'stiffness_rear']
initial_x = array([ 1., 15.,  0.,  0., 10.,  0.,  0.,  0.,  0.])
name = 'car'
obs_noise: Dict[int, Any] = {26: array([[7.61543549e-07]]), 27: array([[0.03046174]]), 28: array([[0.25]]), 29: array([[25.]]), 30: array([[0.01]]), 31: array([[0.00030462]])}
class selfdrive.locationd.models.car_kf.States[source]

Bases: object

ANGLE_OFFSET = slice(2, 3, None)
ANGLE_OFFSET_FAST = slice(3, 4, None)
ROAD_ROLL = slice(8, 9, None)
STEER_ANGLE = slice(7, 8, None)
STEER_RATIO = slice(1, 2, None)
STIFFNESS = slice(0, 1, None)
VELOCITY = slice(4, 6, None)
YAW_RATE = slice(6, 7, None)

selfdrive.locationd.models.constants module

class selfdrive.locationd.models.constants.ObservationKind[source]

Bases: object

ANGLE_OFFSET_FAST = 27
CAMERA_ODO_ROTATION = 14
CAMERA_ODO_TRANSLATION = 13
ECEF_ORIENTATION_FROM_GPS = 32
ECEF_POS = 12
ECEF_VEL = 35
FEATURE_TRACK_TEST = 17
GPS_NED = 2
GPS_VEL = 5
IMU_FRAME = 19
LANE_PT = 18
MSCKF_TEST = 16
NO_ACCEL = 33
NO_OBSERVATION = 1
NO_ROT = 9
ODOMETRIC_SPEED = 3
ORB_FEATURES = 15
ORB_FEATURES_WIDE = 34
ORB_POINT = 11
PHONE_ACCEL = 10
PHONE_GYRO = 4
PSEUDORANGE = 22
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE = 23
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE_RATE_GPS = 7
ROAD_FRAME_XY_SPEED = 24
ROAD_FRAME_X_SPEED = 30
ROAD_FRAME_YAW_RATE = 25
ROAD_ROLL = 31
SPEED = 8
STEER_ANGLE = 26
STEER_RATIO = 29
STIFFNESS = 28
UNKNOWN = 0
names = ['Unknown', 'No observation', 'GPS NED', 'Odometric speed', 'Phone gyro', 'GPS velocity', 'GPS pseudorange', 'GPS pseudorange rate', 'Speed', 'No rotation', 'Phone acceleration', 'ORB point', 'ECEF pos', 'camera odometric translation', 'camera odometric rotation', 'ORB features', 'MSCKF test', 'Feature track test', 'Lane ecef point', 'imu frame eulers', 'GLONASS pseudorange', 'GLONASS pseudorange rate', 'pseudorange', 'pseudorange rate', 'Road Frame x,y speed', 'Road Frame yaw rate', 'Steer Angle', 'Fast Angle Offset', 'Stiffness', 'Steer Ratio', 'Road Frame x speed', 'Road Roll', 'ECEF orientation from GPS', 'NO accel', 'ORB features wide camera', 'ECEF_VEL']
classmethod to_string(kind)[source]

selfdrive.locationd.models.gnss_helpers module

selfdrive.locationd.models.gnss_helpers.parse_pr(m)[source]
selfdrive.locationd.models.gnss_helpers.parse_prr(m)[source]

selfdrive.locationd.models.gnss_kf module

class selfdrive.locationd.models.gnss_kf.GNSSKalman(generated_dir, cython=False, erratic_clock=False)[source]

Bases: object

property P
P_initial = array([[1.e+16, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 1.e+16, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 1.e+16, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 1.e+02, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 1.e+02, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 1.e+02, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 1.e+14, 0.e+00,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 1.e+04,         0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         4.e-02, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 1.e+02, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00, 0.e+00,         0.e+00, 0.e+00, 1.e+00]])
static generate_code(generated_dir)[source]
init_state(state, covs_diag=None, covs=None, filter_time=None)[source]
maha_test_kinds: List[int] = []
name = 'gnss'
predict(t)[source]
predict_and_observe(t, kind, data)[source]
predict_and_update_pseudorange(meas, t, kind)[source]
predict_and_update_pseudorange_rate(meas, t, kind)[source]
rts_smooth(estimates)[source]
property x
x_initial = array([-2712700.6008, -4281600.6679,  3859300.183 ,        0.    ,               0.    ,        0.    ,        0.    ,        0.    ,               0.    ,        0.    ,        0.    ])
class selfdrive.locationd.models.gnss_kf.States[source]

Bases: object

CLOCK_ACCELERATION = slice(8, 9, None)
CLOCK_BIAS = slice(6, 7, None)
CLOCK_DRIFT = slice(7, 8, None)
ECEF_POS = slice(0, 3, None)
ECEF_VELOCITY = slice(3, 6, None)
GLONASS_BIAS = slice(9, 10, None)
GLONASS_FREQ_SLOPE = slice(10, 11, None)

selfdrive.locationd.models.live_kf module

class selfdrive.locationd.models.live_kf.LiveKalman[source]

Bases: object

Q_diag = array([9.0e-04, 9.0e-04, 9.0e-04, 1.0e-06, 1.0e-06, 1.0e-06, 1.0e-04,        1.0e-04, 1.0e-04, 1.0e-02, 1.0e-02, 1.0e-02, 2.5e-09, 2.5e-09,        2.5e-09, 9.0e+00, 9.0e+00, 9.0e+00, 2.5e-05, 2.5e-05, 2.5e-05])
fake_gps_pos_cov_diag = array([1000000, 1000000, 1000000])
fake_gps_vel_cov_diag = array([100, 100, 100])
static generate_code(generated_dir)[source]
initial_P_diag = array([1.e+02, 1.e+02, 1.e+02, 1.e-04, 1.e-04, 1.e-04, 1.e+02, 1.e+02,        1.e+02, 1.e+00, 1.e+00, 1.e+00, 1.e+00, 1.e+00, 1.e+00, 1.e+04,        1.e+04, 1.e+04, 1.e-04, 1.e-04, 1.e-04])
initial_x = array([ 3.8800000e+06, -3.3700000e+06,  3.7600000e+06,  4.2254641e-01,        -3.1238054e-01, -8.3602975e-01, -1.5788347e-01,  0.0000000e+00,         0.0000000e+00,  0.0000000e+00,  0.0000000e+00,  0.0000000e+00,         0.0000000e+00,  0.0000000e+00,  0.0000000e+00,  0.0000000e+00,         0.0000000e+00,  0.0000000e+00,  0.0000000e+00,  0.0000000e+00,         0.0000000e+00,  0.0000000e+00])
name = 'live'
obs_noise_diag = {4: array([0.000625, 0.000625, 0.000625]), 9: array([2.5e-05, 2.5e-05, 2.5e-05]), 10: array([0.25, 0.25, 0.25]), 12: array([25, 25, 25]), 14: array([0.0025, 0.0025, 0.0025]), 32: array([0.04, 0.04, 0.04, 0.04]), 33: array([0.0025, 0.0025, 0.0025]), 35: array([0.25, 0.25, 0.25])}
reset_orientation_diag = array([1, 1, 1])
class selfdrive.locationd.models.live_kf.States[source]

Bases: object

ACCELERATION = slice(16, 19, None)
ACCELERATION_ERR = slice(15, 18, None)
ACC_BIAS = slice(19, 22, None)
ACC_BIAS_ERR = slice(18, 21, None)
ANGULAR_VELOCITY = slice(10, 13, None)
ANGULAR_VELOCITY_ERR = slice(9, 12, None)
ECEF_ORIENTATION = slice(3, 7, None)
ECEF_ORIENTATION_ERR = slice(3, 6, None)
ECEF_POS = slice(0, 3, None)
ECEF_POS_ERR = slice(0, 3, None)
ECEF_VELOCITY = slice(7, 10, None)
ECEF_VELOCITY_ERR = slice(6, 9, None)
GYRO_BIAS = slice(13, 16, None)
GYRO_BIAS_ERR = slice(12, 15, None)
selfdrive.locationd.models.live_kf.numpy2eigenstring(arr)[source]

selfdrive.locationd.models.loc_kf module

class selfdrive.locationd.models.loc_kf.LocKalman(generated_dir, N=4, erratic_clock=False)[source]

Bases: object

property P
P_initial = array([[1.e+16, 0.e+00, 0.e+00, ..., 0.e+00, 0.e+00, 0.e+00],        [0.e+00, 1.e+16, 0.e+00, ..., 0.e+00, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 1.e+16, ..., 0.e+00, 0.e+00, 0.e+00],        ...,        [0.e+00, 0.e+00, 0.e+00, ..., 1.e-04, 0.e+00, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, ..., 0.e+00, 1.e-04, 0.e+00],        [0.e+00, 0.e+00, 0.e+00, ..., 0.e+00, 0.e+00, 1.e-04]])
dim_augment = 7
dim_augment_err = 6
static generate_code(generated_dir, N=4)[source]
get_R(kind, n)[source]
init_state(state, covs_diag=None, covs=None, filter_time=None)[source]
maha_test_kinds = [15, 34]
maha_test_pseudorange(x, P, meas, kind, maha_thresh=0.3)[source]
maha_test_pseudorange_rate(x, P, meas, kind, maha_thresh=0.999)[source]
name = 'loc'
pad_augmented(x, P, Q=None)[source]
predict(t)[source]
predict_and_observe(t, kind, data)[source]
predict_and_update_msckf_test(test_data, t, kind)[source]
predict_and_update_odo_rot(rot, t, kind)[source]
predict_and_update_odo_trans(trans, t, kind)[source]
predict_and_update_orb_features(tracks, t, kind)[source]
predict_and_update_pseudorange(meas, t, kind)[source]
predict_and_update_pseudorange_rate(meas, t, kind)[source]
rts_smooth(estimates)[source]
property t
property x
x_initial = array([0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,        0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,        0., 0.])
class selfdrive.locationd.models.loc_kf.States[source]

Bases: object

ACCELERATION = slice(19, 22, None)
ACCELERATION_ERR = slice(18, 21, None)
ACCELEROMETER_BIAS = slice(30, 33, None)
ACCELEROMETER_BIAS_ERR = slice(29, 32, None)
ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29, None)
ACCELEROMETER_SCALE_UNUSED = slice(29, 30, None)
ANGULAR_VELOCITY = slice(10, 13, None)
ANGULAR_VELOCITY_ERR = slice(9, 12, None)
CLOCK_ACCELERATION = slice(28, 29, None)
CLOCK_ACCELERATION_ERR = slice(27, 28, None)
CLOCK_BIAS = slice(13, 14, None)
CLOCK_BIAS_ERR = slice(12, 13, None)
CLOCK_DRIFT = slice(14, 15, None)
CLOCK_DRIFT_ERR = slice(13, 14, None)
ECEF_ORIENTATION = slice(3, 7, None)
ECEF_ORIENTATION_ERR = slice(3, 6, None)
ECEF_POS = slice(0, 3, None)
ECEF_POS_ERR = slice(0, 3, None)
ECEF_VELOCITY = slice(7, 10, None)
ECEF_VELOCITY_ERR = slice(6, 9, None)
FOCAL_SCALE_ERR_UNUSED = slice(21, 22, None)
FOCAL_SCALE_UNUSED = slice(22, 23, None)
GLONASS_BIAS = slice(26, 27, None)
GLONASS_BIAS_ERR = slice(25, 26, None)
GLONASS_FREQ_SLOPE = slice(27, 28, None)
GLONASS_FREQ_SLOPE_ERR = slice(26, 27, None)
GYRO_BIAS = slice(15, 18, None)
GYRO_BIAS_ERR = slice(14, 17, None)
IMU_FROM_DEVICE_EULER = slice(23, 26, None)
IMU_FROM_DEVICE_EULER_ERR = slice(22, 25, None)
ODO_SCALE_ERR_UNUSED = slice(17, 18, None)
ODO_SCALE_UNUSED = slice(18, 19, None)
WIDE_FROM_DEVICE_EULER = slice(33, 36, None)
WIDE_FROM_DEVICE_EULER_ERR = slice(32, 35, None)

Module contents