IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
iqpilot/selfdrive/locationd/models/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
generated/

0
iqpilot/selfdrive/locationd/models/__init__.py Normal file
View File

94
iqpilot/selfdrive/locationd/models/car_kf.py Normal file
View File

@@ -0,0 +1,94 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
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),
}
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name not in {"self"}:
self.filter.set_global(name, value)

88
iqpilot/selfdrive/locationd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
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]

67
iqpilot/selfdrive/locationd/models/pose_kf.py Normal file
View File

@@ -0,0 +1,67 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import numpy as np
from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update
except ModuleNotFoundError:
pose_predict = None
pose_update = None
EARTH_G = 9.81
class States:
NED_ORIENTATION = slice(0, 3)
DEVICE_VELOCITY = slice(3, 6)
ANGULAR_VELOCITY = slice(6, 9)
GYRO_BIAS = slice(9, 12)
ACCELERATION = slice(12, 15)
ACCEL_BIAS = slice(15, 18)
def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray:
result = state.copy()
result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION]
rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY])
result[States.NED_ORIENTATION] = euler_from_rot(rotation)
return result
def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray:
device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T
centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY])
return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS]
class PoseKalman(EstimatorModel):
name = "pose"
initial_x = np.zeros(18)
initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3)
Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3)
obs_noise = {
ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3),
ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3),
}
def __init__(self, max_rewind_age: float):
measurements = {
ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS],
ObservationKind.PHONE_ACCEL: _phone_acceleration,
ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY],
ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY],
}
def native_predict(state, covariance, dt, process_noise, _):
pose_predict(state, covariance, process_noise, dt)
model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if pose_predict is not None else None, native_update=pose_update)
super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age))