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)

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)

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

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

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)

selfdrive.locationd.models.live_kf module

class selfdrive.locationd.models.live_kf.LiveKalman

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)

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

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)

Module contents