Release 260111

2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
--- a/selfdrive/locationd/models/init.py
+++ b/selfdrive/locationd/models/init.py
--- a/selfdrive/locationd/models/car_kf.py
+++ b/selfdrive/locationd/models/car_kf.py
@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+import math
+import sys
+from typing import Any
+
+import numpy as np
+
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.selfdrive.locationd.models.constants import ObservationKind
+from openpilot.common.swaglog import cloudlog
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+
+i = 0
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States:
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
+
+
+class CarKalman(KalmanFilter):
+  name = 'car'
+
+  initial_x = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+    0.0
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05 / 100)**2,
+    .01**2,
+    math.radians(0.02)**2,
+    math.radians(0.25)**2,
+
+    .1**2, .01**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+    math.radians(1)**2,
+  ])
+  P_initial = Q.copy()
+
+  obs_noise: dict[int, Any] = {
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
+    ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
+  }
+
+  global_vars = [
+    'mass',
+    'rotational_inertia',
+    'center_to_front',
+    'center_to_rear',
+    'stiffness_front',
+    'stiffness_rear',
+  ]
+
+  @staticmethod
+  def generate_code(generated_dir):
+    dim_state = CarKalman.initial_x.shape[0]
+    name = CarKalman.name
+
+    # Linearized single-track lateral dynamics, equations 7.211-7.213
+    # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
+    # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
+
+    # globals
+    global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    sf = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = sf * cF_orig, sf * cR_orig
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
+      [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
+
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CarKalman.generate_code(generated_dir)
--- a/selfdrive/locationd/models/constants.py
+++ b/selfdrive/locationd/models/constants.py
@@ -0,0 +1,92 @@
+import os
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+  ECEF_VEL = 35
+  ECEF_ORIENTATION_FROM_GPS = 32
+  NO_ACCEL = 33
+  ORB_FEATURES_WIDE = 34
+
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+  ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
+
+  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
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+           ObservationKind.PSEUDORANGE_RATE_GPS,
+           ObservationKind.PSEUDORANGE_GLONASS,
+           ObservationKind.PSEUDORANGE_RATE_GLONASS]
--- a/selfdrive/locationd/models/generated/car.h
+++ b/selfdrive/locationd/models/generated/car.h
@@ -0,0 +1,40 @@
+#pragma once
+#include "rednose/helpers/ekf.h"
+extern "C" {
+void car_update_25(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_24(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_30(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_26(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void car_err_fun(double *nom_x, double *delta_x, double *out_2229253592220784796);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_8758496889644692282);
+void car_H_mod_fun(double *state, double *out_8711803353026157689);
+void car_f_fun(double *state, double dt, double *out_4199840031629318215);
+void car_F_fun(double *state, double dt, double *out_1980461025500558356);
+void car_h_25(double *state, double *unused, double *out_3619972370828301387);
+void car_H_25(double *state, double *unused, double *out_5188754714380792541);
+void car_h_24(double *state, double *unused, double *out_3935146291291123111);
+void car_H_24(double *state, double *unused, double *out_8746680313183115677);
+void car_h_30(double *state, double *unused, double *out_4876688310435438605);
+void car_H_30(double *state, double *unused, double *out_7707087672888041168);
+void car_h_26(double *state, double *unused, double *out_2897076540518924503);
+void car_H_26(double *state, double *unused, double *out_8493280684141593142);
+void car_h_27(double *state, double *unused, double *out_6030462238615253089);
+void car_H_27(double *state, double *unused, double *out_8516062329637567231);
+void car_h_29(double *state, double *unused, double *out_7171014245860263865);
+void car_H_29(double *state, double *unused, double *out_8217319017202433352);
+void car_h_28(double *state, double *unused, double *out_4981138400665597714);
+void car_H_28(double *state, double *unused, double *out_3134920000132902778);
+void car_h_31(double *state, double *unused, double *out_4261300279925747798);
+void car_H_31(double *state, double *unused, double *out_6181314108816941822);
+void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
+void car_set_mass(double x);
+void car_set_rotational_inertia(double x);
+void car_set_center_to_front(double x);
+void car_set_center_to_rear(double x);
+void car_set_stiffness_front(double x);
+void car_set_stiffness_rear(double x);
+}
--- a/selfdrive/locationd/models/generated/libcar.so
+++ b/selfdrive/locationd/models/generated/libcar.so
--- a/selfdrive/locationd/models/generated/libpose.so
+++ b/selfdrive/locationd/models/generated/libpose.so
--- a/selfdrive/locationd/models/generated/pose.h
+++ b/selfdrive/locationd/models/generated/pose.h
@@ -0,0 +1,22 @@
+#pragma once
+#include "rednose/helpers/ekf.h"
+extern "C" {
+void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_7073107002944959153);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_786074686772967659);
+void pose_H_mod_fun(double *state, double *out_697652429142743383);
+void pose_f_fun(double *state, double dt, double *out_3774661386476087020);
+void pose_F_fun(double *state, double dt, double *out_1884976403670791797);
+void pose_h_4(double *state, double *unused, double *out_6884445182252047765);
+void pose_H_4(double *state, double *unused, double *out_2639656693110238062);
+void pose_h_10(double *state, double *unused, double *out_1633730535306024295);
+void pose_H_10(double *state, double *unused, double *out_7172126253024250418);
+void pose_h_13(double *state, double *unused, double *out_3626471298505881547);
+void pose_H_13(double *state, double *unused, double *out_4970974515206462867);
+void pose_h_14(double *state, double *unused, double *out_1559882867818069008);
+void pose_H_14(double *state, double *unused, double *out_1323584163229246467);
+void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
+}
--- a/selfdrive/locationd/models/pose_kf.py
+++ b/selfdrive/locationd/models/pose_kf.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+import sys
+import numpy as np
+
+from openpilot.selfdrive.locationd.models.constants import ObservationKind
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__=="__main__":
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+  from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+EARTH_G = 9.81
+
+
+class States:
+  NED_ORIENTATION = slice(0, 3)  # roll, pitch, yaw in rad
+  DEVICE_VELOCITY = slice(3, 6)  # ned velocity in m/s
+  ANGULAR_VELOCITY = slice(6, 9)  # roll, pitch and yaw rates in rad/s
+  GYRO_BIAS = slice(9, 12)  # roll, pitch and yaw gyroscope biases in rad/s
+  ACCELERATION = slice(12, 15)  # acceleration in device frame in m/s**2
+  ACCEL_BIAS = slice(15, 18)  # Acceletometer bias in m/s**2
+
+
+class PoseKalman(KalmanFilter):
+  name = "pose"
+
+  # state
+  initial_x = np.array([0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0,
+                        0.0, 0.0, 0.0])
+  # state covariance
+  initial_P = np.diag([0.01**2, 0.01**2, 0.01**2,
+                       10**2, 10**2, 10**2,
+                       1**2, 1**2, 1**2,
+                       1**2, 1**2, 1**2,
+                       100**2, 100**2, 100**2,
+                       0.01**2, 0.01**2, 0.01**2])
+
+  # process noise
+  Q = np.diag([0.001**2, 0.001**2, 0.001**2,
+               0.01**2, 0.01**2, 0.01**2,
+               0.1**2, 0.1**2, 0.1**2,
+               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+               3**2, 3**2, 3**2,
+               0.005**2, 0.005**2, 0.005**2])
+
+  obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
+               ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
+               ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]),
+               ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = PoseKalman.name
+    dim_state = PoseKalman.initial_x.shape[0]
+    dim_state_err = PoseKalman.initial_P.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+    roll, pitch, yaw = state[States.NED_ORIENTATION, :]
+    velocity = state[States.DEVICE_VELOCITY, :]
+    angular_velocity = state[States.ANGULAR_VELOCITY, :]
+    vroll, vpitch, vyaw = angular_velocity
+    gyro_bias = state[States.GYRO_BIAS, :]
+    acceleration = state[States.ACCELERATION, :]
+    acc_bias = state[States.ACCEL_BIAS, :]
+
+    dt = sp.Symbol('dt')
+
+    ned_from_device = euler_rotate(roll, pitch, yaw)
+    device_from_ned = ned_from_device.T
+
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.DEVICE_VELOCITY, :] = acceleration
+
+    f_sym = state + dt * state_dot
+    device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw)
+    ned_from_device_t1 = ned_from_device * device_from_device_t1
+    f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1)
+
+    centripetal_acceleration = angular_velocity.cross(velocity)
+    gravity = sp.Matrix([0, 0, -EARTH_G])
+    h_gyro_sym = angular_velocity + gyro_bias
+    h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias
+    h_phone_rot_sym = angular_velocity
+    h_relative_motion_sym = velocity
+    obs_eqs = [
+      [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+      [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
+      [h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+      [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
+    ]
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err)
+
+  def __init__(self, generated_dir, max_rewind_age):
+    dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
+                              dim_state, dim_state_err, max_rewind_age=max_rewind_age)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  PoseKalman.generate_code(generated_dir)