IQ.Pilot Release Commit @ cd83f5a

iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py

@@ -2,14 +2,16 @@
 Lateral Edge Guard uses the model's lateral road-edge geometry to withhold lane
 changes that lack room for a target lane. The model standard deviation remains
 in metres: measurements above the validity limit are rejected, while valid
-measurements use a two-sigma lower confidence bound for conservative clearance.
+measurements use a one-sigma lower confidence bound for conservative clearance.
 Unavailable geometry briefly holds the last output, then fails open because a
-model dropout is not geometric evidence of a nearby edge.
+model dropout is not geometric evidence of a nearby edge. A visible outer lane
+line on the target side is direct evidence that a lane exists and overrides the
+edge-distance inference.
 """
 from __future__ import annotations
 
 import math
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 from enum import IntEnum
 from typing import Any
 
@@ -20,13 +22,22 @@ from iqpilot.common.swaglog import cloudlog
 
 MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS  # Matches the lane-change speed gate and excludes parking manoeuvres.
 MAX_VALID_ROAD_EDGE_STD_M = 1.0  # A 2-sigma bound beyond 2 m cannot distinguish an adjacent 3.5 m lane reliably.
-EDGE_CONFIDENCE_SIGMA = 2.0  # 97.7% one-sided confidence under the model's Gaussian uncertainty assumption.
+# roadEdgeStd describes a single edge point, but it is applied to a 5-40 m minimum that already absorbs the
+# spatial worst case; 1 sigma covers ~1.1x the measured p99 frame-to-frame spread of that minimum, 2 sigma 2.2x.
+EDGE_CONFIDENCE_SIGMA = 1.0
 ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0  # Ignore near-field edge points dominated by vehicle-body perspective.
 ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0  # Covers about 2 s at the 20 m/s model-training reference speed.
 LANE_CENTER_OFFSET_M = 3.5  # Typical freeway lane width and the target-centre lateral displacement.
 # CarParams exposes neither width nor track; 0.95 m is half of an assumed conservative 1.90 m body width.
 VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
 EDGE_CLEARANCE_MARGIN_M = 0.25  # Additional lateral separation between the vehicle body and detected road edge.
+ADJACENT_LANE_LINE_PROB = 0.5
+EGO_LANE_LINE_PROB_MIN = 0.5
+MIN_MEASURED_LANE_WIDTH_M = 2.5
+MAX_MEASURED_LANE_WIDTH_M = 4.5
+# modelV2 lane lines are ordered outer-left, ego-left, ego-right, outer-right.
+OUTER_LANE_LINE_INDEX = (0, 3)
+EGO_LANE_LINE_INDEX = (1, 2)
 REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
 BLOCK_DEBOUNCE_S = 0.30  # Six model frames reject a transient close-edge prediction before blocking.
 CLEAR_DEBOUNCE_S = 0.50  # Ten model frames make release slower than assertion for conservative hysteresis.
@@ -60,7 +71,8 @@ class _SideState:
   fallback_reported: bool = False
 
 
-def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasurement:
+def evaluate_road_edge(edge: Any, std_m: Any, direction: int,
+                       lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement:
   if edge is None or std_m is None:
     return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
 
@@ -103,11 +115,12 @@ def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasure
     return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
 
   conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
+  required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
   return RoadEdgeMeasurement(
     RoadEdgeDataState.VALID,
     lateral_distance_m,
     conservative_distance_m,
-    conservative_distance_m < REQUIRED_ROAD_EDGE_DISTANCE_M,
+    conservative_distance_m < required_distance_m,
   )
 
 
@@ -160,12 +173,60 @@ class LateralEdgeGuard:
     except (AttributeError, TypeError):
       return None, None
 
+  @staticmethod
+  def _lane_line_prob(modeldata: Any, index: int) -> float | None:
+    if modeldata is None:
+      return None
+    try:
+      probs = modeldata.laneLineProbs
+      if len(probs) <= index:
+        return None
+      value = float(probs[index])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return None
+    return value if math.isfinite(value) else None
+
+  @classmethod
+  def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool:
+    prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index])
+    return prob is not None and prob > ADJACENT_LANE_LINE_PROB
+
+  @classmethod
+  def _measured_lane_width(cls, modeldata: Any) -> float:
+    left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0])
+    right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1])
+    if left_prob is None or right_prob is None:
+      return LANE_CENTER_OFFSET_M
+    if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN:
+      return LANE_CENTER_OFFSET_M
+    try:
+      lines = modeldata.laneLines
+      left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0])
+      right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return LANE_CENTER_OFFSET_M
+    width = abs(right_y - left_y)
+    if not math.isfinite(width):
+      return LANE_CENTER_OFFSET_M
+    return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M)
+
+  @staticmethod
+  def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement:
+    if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block:
+      return replace(measurement, should_block=False)
+    return measurement
+
   def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
     dt = max(float(dt_s), 0.0)
     left_edge, left_std = self._model_side(modeldata, 0)
     right_edge, right_std = self._model_side(modeldata, 1)
-    self.left_measurement = evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left)
-    self.right_measurement = evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right)
+    lane_width_m = self._measured_lane_width(modeldata)
+    self.left_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 0))
+    self.right_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 1))
     speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
     self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
     self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)

iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py

@@ -9,12 +9,16 @@ from iqpilot.common.realtime import DT_MDL
 from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
 from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
+  ADJACENT_LANE_LINE_PROB,
   BLOCK_DEBOUNCE_S,
   CLEAR_DEBOUNCE_S,
   MAX_VALID_ROAD_EDGE_STD_M,
   MIN_ACTIVE_SPEED_MPS,
   REQUIRED_ROAD_EDGE_DISTANCE_M,
   UNAVAILABLE_HOLD_S,
+  LANE_CENTER_OFFSET_M,
+  MAX_MEASURED_LANE_WIDTH_M,
+  MIN_MEASURED_LANE_WIDTH_M,
   LateralEdgeGuard,
   RoadEdgeDataState,
   evaluate_road_edge,
@@ -35,6 +39,25 @@ class ModelData:
   roadEdgeStds: list[float]
 
 
+@dataclass
+class LaneModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+  laneLines: list[Edge]
+  laneLineProbs: list[float]
+
+
+def lane_model(left_distance_m: float = 4.0, outer_prob: float = 0.0,
+               ego_width_m: float = 3.5, ego_prob: float = 0.9) -> LaneModelData:
+  xs = [5.0, 20.0, 40.0]
+  base = edge_model(left_distance_m, left_distance_m)
+  half = ego_width_m / 2.0
+  lines = [Edge(xs, [-(half + 3.0)] * 3), Edge(xs, [-half] * 3),
+           Edge(xs, [half] * 3), Edge(xs, [half + 3.0] * 3)]
+  return LaneModelData(base.roadEdges, base.roadEdgeStds, lines,
+                       [outer_prob, ego_prob, ego_prob, outer_prob])
+
+
 class CarState:
   def __init__(self, left_blindspot: bool = False) -> None:
     self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
@@ -86,11 +109,15 @@ def test_unavailable_and_invalid_are_distinct() -> None:
   assert invalid.should_block is None
 
 
-def test_two_sigma_bound_uses_std_in_metres() -> None:
+def test_one_sigma_bound_uses_std_in_metres() -> None:
   measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
   assert measurement.lateral_distance_m == 5.0
-  assert measurement.conservative_distance_m == 4.6
-  assert measurement.should_block is True
+  assert measurement.conservative_distance_m == 4.8
+  assert measurement.should_block is False
+
+  blocking = evaluate_road_edge(edge_model(4.5).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocking.conservative_distance_m == 4.3
+  assert blocking.should_block is True
 
 
 def test_distance_threshold_on_either_side() -> None:
@@ -193,3 +220,36 @@ def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
   alert = EVENTS_IQ[event_name][ET.WARNING]
   assert alert.alert_text_1 == "Lane Change Blocked"
   assert alert.alert_text_2 == "Road edge detected"
+
+
+def test_visible_outer_lane_line_overrides_edge_block() -> None:
+  blocking = lane_model(4.0, outer_prob=0.0)
+  guard = LateralEdgeGuard()
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
+
+  guard = LateralEdgeGuard()
+  update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB + 0.2), BLOCK_DEBOUNCE_S * 4)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_outer_lane_line_below_threshold_still_blocks() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB - 0.1), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
+
+
+def test_narrow_measured_lane_relaxes_required_distance() -> None:
+  narrow = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, 3.0)
+  wide = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, LANE_CENTER_OFFSET_M)
+  assert narrow.should_block is False
+  assert wide.should_block is True
+
+
+def test_measured_lane_width_is_clamped_and_falls_back() -> None:
+  assert LateralEdgeGuard._measured_lane_width(None) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(edge_model(4.0)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_prob=0.1)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=9.0)) == MAX_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=0.5)) == MIN_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=3.2)) == 3.2

iqpilot/selfdrive/iqlocd/SConscript

@@ -1,37 +1,8 @@
-Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
+Import('env', 'arch', 'common', 'messaging', 'transformations')
 
 loc_libs = [messaging, common, 'pthread', 'dl']
 
-# build ekf models
-rednose_gen_dir = 'models/generated'
-rednose_gen_deps = [
-  "models/constants.py",
-]
-orbit_filter = env.RednoseCompileFilter(
-  target='orbit',
-  filter_gen_script='models/orbit_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=['orbit_state_constants.h'],
-  gen_script_deps=rednose_gen_deps,
-)
-car_ekf = env.RednoseCompileFilter(
-  target='car',
-  filter_gen_script='models/car_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=[],
-  gen_script_deps=rednose_gen_deps,
-)
-
-# iqlocd build
 iqlocd_sources = ["atlas_loc_core.cc", "models/orbit_kf.cc"]
 
 lenv = env.Clone()
-# ekf filter libraries need to be linked, even if no symbols are used
-if arch != "Darwin":
-  lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
-
-lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
-lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
-iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=["orbit", rednose] + loc_libs + transformations)
-lenv.Depends(iqlocd, rednose)
-lenv.Depends(iqlocd, orbit_filter)
+iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=loc_libs + transformations)

iqpilot/selfdrive/iqlocd/atlas_loc_core.cc

@@ -7,7 +7,6 @@
 #include <cmath>
 #include <vector>
 
-using namespace EKFS;
 using namespace Eigen;
 
 ExitHandler do_exit;

iqpilot/selfdrive/iqlocd/models/car_kf.py

@@ -1,75 +1,63 @@
-#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
 import math
-import sys
 from typing import Any
 
 import numpy as np
 
 from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
-from iqpilot.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
+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:
-  # 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]
+  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)
 
 
-class CarKalman(KalmanFilter):
-  name = 'car'
+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
 
-  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,
-  ])
+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),
@@ -79,102 +67,28 @@ class CarKalman(KalmanFilter):
     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',
-  ]
+  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)
 
-  @staticmethod
-  def generate_code(generated_dir):
-    dim_state = CarKalman.initial_x.shape[0]
-    name = CarKalman.name
+    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))
 
-    # 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)
+  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 != "self":
+        self.filter.set_global(name, value)

iqpilot/selfdrive/iqlocd/models/constants.py

@@ -1,7 +1,3 @@
-import os
-
-GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
-
 class ObservationKind:
   UNKNOWN = 0
   NO_OBSERVATION = 1

iqpilot/selfdrive/iqlocd/models/orbit_kf.cc

@@ -1,122 +1,225 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
 #include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
 
-using namespace EKFS;
-using namespace Eigen;
+#include <cmath>
 
-Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
-  return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
+using Eigen::Matrix3d;
+using Eigen::Quaterniond;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using iqpilot::state_estimation::ModelDefinition;
+using iqpilot::state_estimation::StateEstimator;
+
+namespace {
+
+constexpr double EARTH_GM = 3.986005e14;
+
+Matrix3d rotation(const VectorXd &state) {
+  return Quaterniond(state(3), state(4), state(5), state(6)).normalized().toRotationMatrix();
 }
 
-Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
-  return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
+Matrix3d skew(const Vector3d &value) {
+  Matrix3d result;
+  result << 0.0, -value.z(), value.y(), value.z(), 0.0, -value.x(), -value.y(), value.x(), 0.0;
+  return result;
 }
 
-std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
-  std::vector<Eigen::Map<Eigen::VectorXd>> res;
-  for (const Eigen::VectorXd &vec : vec_vec) {
-    res.push_back(get_mapvec(vec));
-  }
-  return res;
+VectorXd transition(const VectorXd &state, double dt) {
+  VectorXd result = state;
+  const Quaterniond orientation(state(3), state(4), state(5), state(6));
+  const Vector3d omega = state.segment<3>(10);
+  const Quaterniond derivative(0.0, omega.x(), omega.y(), omega.z());
+  const Quaterniond rate = orientation * derivative;
+  result.segment<3>(0) += dt * state.segment<3>(7);
+  result.segment<4>(3) += 0.5 * dt * (VectorXd(4) << rate.w(), rate.x(), rate.y(), rate.z()).finished();
+  result.segment<3>(7) += dt * rotation(state) * state.segment<3>(16);
+  return result;
+}
+
+VectorXd normalize(const VectorXd &state) {
+  VectorXd result = state;
+  result.segment<4>(3) /= result.segment<4>(3).norm();
+  return result;
+}
+
+VectorXd inject(const VectorXd &state, const VectorXd &delta) {
+  VectorXd result = state;
+  result.segment<3>(0) += delta.segment<3>(0);
+  const Quaterniond orientation(state(3), state(4), state(5), state(6));
+  Quaterniond error(1.0, 0.5 * delta(3), 0.5 * delta(4), 0.5 * delta(5));
+  const Quaterniond updated = error * orientation;
+  result.segment<4>(3) << updated.w(), updated.x(), updated.y(), updated.z();
+  result.segment(7, 15) += delta.segment(6, 15);
+  return normalize(result);
+}
+
+MatrixXdr error_projection(const VectorXd &state) {
+  MatrixXdr projection = MatrixXdr::Zero(22, 21);
+  projection.block<3, 3>(0, 0).setIdentity();
+  const double w = state(3);
+  const double x = state(4);
+  const double y = state(5);
+  const double z = state(6);
+  projection.block<4, 3>(3, 3) << -0.5 * x, -0.5 * y, -0.5 * z,
+                                      0.5 * w,  0.5 * z, -0.5 * y,
+                                     -0.5 * z,  0.5 * w,  0.5 * x,
+                                      0.5 * y, -0.5 * x,  0.5 * w;
+  projection.block(7, 6, 15, 15).setIdentity();
+  return projection;
+}
+
+MatrixXdr orbit_error_transition(const VectorXd &state, double dt) {
+  MatrixXdr result = MatrixXdr::Identity(21, 21);
+  const Matrix3d transform = rotation(state);
+  result.block<3, 3>(0, 6) = Matrix3d::Identity() * dt;
+  result.block<3, 3>(3, 3) += -dt * skew(transform * state.segment<3>(10));
+  result.block<3, 3>(3, 9) = dt * transform;
+  result.block<3, 3>(6, 3) = -dt * skew(transform * state.segment<3>(16));
+  result.block<3, 3>(6, 15) = dt * transform;
+  return result;
+}
+
+MatrixXdr selected_jacobian(int start) {
+  MatrixXdr result = MatrixXdr::Zero(3, 21);
+  result.block<3, 3>(0, start).setIdentity();
+  return result;
+}
+
+VectorXd phone_acceleration(const VectorXd &state) {
+  const Vector3d position = state.segment<3>(0);
+  const Vector3d gravity = rotation(state).transpose() * (EARTH_GM * position / std::pow(position.squaredNorm(), 1.5));
+  return gravity + state.segment<3>(16) + state.segment<3>(19);
+}
+
+MatrixXdr diagonal(std::initializer_list<double> values) {
+  VectorXd vector(values.size());
+  int index = 0;
+  for (double value : values) vector(index++) = value;
+  return vector.asDiagonal();
 }
 
-std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
-  std::vector<Eigen::Map<MatrixXdr>> res;
-  for (const MatrixXdr &mat : mat_vec) {
-    res.push_back(get_mapmat(mat));
-  }
-  return res;
 }
 
 OrbitKalman::OrbitKalman() {
-  this->dim_state = orbit_initial_x.rows();
-  this->dim_state_err = orbit_initial_P_diag.rows();
-
-  this->initial_x = orbit_initial_x;
-  this->initial_P = orbit_initial_P_diag.asDiagonal();
-  this->fake_gps_pos_cov = orbit_fake_gps_pos_cov_diag.asDiagonal();
-  this->fake_gps_vel_cov = orbit_fake_gps_vel_cov_diag.asDiagonal();
-  this->reset_orientation_P = orbit_reset_orientation_diag.asDiagonal();
-  this->Q = orbit_Q_diag.asDiagonal();
-  for (auto& pair : orbit_obs_noise_diag) {
-    this->obs_noise[pair.first] = pair.second.asDiagonal();
-  }
-
-  // init filter
-  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
-    get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
-    std::vector<int>{3}, std::vector<std::string>(), 0.8);
+  initial_x.resize(22);
+  initial_x << 3.88e6, -3.37e6, 3.76e6, 0.42254641, -0.31238054, -0.83602975, -0.15788347,
+               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;
+  initial_P = diagonal({100.0, 100.0, 100.0, 0.0001, 0.0001, 0.0001, 100.0, 100.0, 100.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10000.0, 10000.0, 10000.0, 0.0001, 0.0001, 0.0001});
+  fake_gps_pos_cov = diagonal({1e6, 1e6, 1e6});
+  fake_gps_vel_cov = diagonal({100.0, 100.0, 100.0});
+  reset_orientation_P = diagonal({1.0, 1.0, 1.0});
+  obs_noise = {
+    {OBSERVATION_PHONE_GYRO, diagonal({0.000625, 0.000625, 0.000625})},
+    {OBSERVATION_PHONE_ACCEL, diagonal({0.25, 0.25, 0.25})},
+    {OBSERVATION_CAMERA_ODO_ROTATION, diagonal({0.0025, 0.0025, 0.0025})},
+    {OBSERVATION_CAMERA_ODO_TRANSLATION, diagonal({0.25, 0.25, 0.25})},
+    {OBSERVATION_NO_ROT, diagonal({0.000025, 0.000025, 0.000025})},
+    {OBSERVATION_NO_ACCEL, diagonal({0.0025, 0.0025, 0.0025})},
+    {OBSERVATION_ECEF_POS, diagonal({25.0, 25.0, 25.0})},
+    {OBSERVATION_ECEF_VEL, diagonal({0.25, 0.25, 0.25})},
+    {OBSERVATION_ECEF_ORIENTATION_FROM_GPS, diagonal({0.04, 0.04, 0.04, 0.04})},
+  };
+  const MatrixXdr process_noise = diagonal({0.0009, 0.0009, 0.0009, 0.000001, 0.000001, 0.000001,
+                                             0.0001, 0.0001, 0.0001, 0.01, 0.01, 0.01,
+                                             2.5e-9, 2.5e-9, 2.5e-9, 9.0, 9.0, 9.0, 0.000025, 0.000025, 0.000025});
+  std::unordered_map<int, std::function<VectorXd(const VectorXd &)>> measurements = {
+    {OBSERVATION_PHONE_GYRO, [](const VectorXd &state) { return state.segment<3>(10) + state.segment<3>(13); }},
+    {OBSERVATION_NO_ROT, [](const VectorXd &state) { return state.segment<3>(10); }},
+    {OBSERVATION_PHONE_ACCEL, phone_acceleration},
+    {OBSERVATION_ECEF_POS, [](const VectorXd &state) { return state.segment<3>(0); }},
+    {OBSERVATION_ECEF_VEL, [](const VectorXd &state) { return state.segment<3>(7); }},
+    {OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return state.segment<4>(3); }},
+    {OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) { return rotation(state).transpose() * state.segment<3>(7); }},
+    {OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &state) { return state.segment<3>(10); }},
+    {OBSERVATION_NO_ACCEL, [](const VectorXd &state) { return state.segment<3>(16); }},
+  };
+  std::unordered_map<int, std::function<MatrixXdr(const VectorXd &)>> observation_jacobians = {
+    {OBSERVATION_PHONE_GYRO, [](const VectorXd &) {
+      MatrixXdr result = selected_jacobian(9);
+      result.block<3, 3>(0, 12).setIdentity();
+      return result;
+    }},
+    {OBSERVATION_NO_ROT, [](const VectorXd &) { return selected_jacobian(9); }},
+    {OBSERVATION_PHONE_ACCEL, [](const VectorXd &state) {
+      MatrixXdr result = MatrixXdr::Zero(3, 21);
+      const Vector3d position = state.segment<3>(0);
+      const double radius_squared = position.squaredNorm();
+      const double radius = std::sqrt(radius_squared);
+      const Vector3d gravity = EARTH_GM * position / (radius_squared * radius);
+      result.block<3, 3>(0, 0) = rotation(state).transpose() * EARTH_GM *
+                                 (Matrix3d::Identity() / (radius_squared * radius) -
+                                  3.0 * position * position.transpose() / (radius_squared * radius_squared * radius));
+      result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(gravity);
+      result.block<3, 3>(0, 15).setIdentity();
+      result.block<3, 3>(0, 18).setIdentity();
+      return result;
+    }},
+    {OBSERVATION_ECEF_POS, [](const VectorXd &) { return selected_jacobian(0); }},
+    {OBSERVATION_ECEF_VEL, [](const VectorXd &) { return selected_jacobian(6); }},
+    {OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return error_projection(state).block(3, 0, 4, 21); }},
+    {OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) {
+      MatrixXdr result = MatrixXdr::Zero(3, 21);
+      result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(state.segment<3>(7));
+      result.block<3, 3>(0, 6) = rotation(state).transpose();
+      return result;
+    }},
+    {OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &) { return selected_jacobian(9); }},
+    {OBSERVATION_NO_ACCEL, [](const VectorXd &) { return selected_jacobian(15); }},
+  };
+  ModelDefinition model{22, 21, transition, measurements, process_noise, obs_noise, inject, error_projection, normalize,
+                        orbit_error_transition, observation_jacobians};
+  filter = std::make_shared<StateEstimator>(std::move(model), initial_x, initial_P);
 }
 
 void OrbitKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
-  MatrixXdr covs = covs_diag.asDiagonal();
-  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+  filter->init_state(state, covs_diag.asDiagonal(), filter_time);
 }
 
 void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
-  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+  filter->init_state(state, covs, filter_time);
 }
 
 void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
-  MatrixXdr covs = this->filter->covs();
-  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+  filter->init_state(state, filter->covariance(), filter_time);
 }
 
-VectorXd OrbitKalman::get_x() {
-  return this->filter->state();
-}
-
-MatrixXdr OrbitKalman::get_P() {
-  return this->filter->covs();
-}
-
-double OrbitKalman::get_filter_time() {
-  return this->filter->get_filter_time();
-}
+VectorXd OrbitKalman::get_x() { return filter->state(); }
+MatrixXdr OrbitKalman::get_P() { return filter->covariance(); }
+double OrbitKalman::get_filter_time() { return filter->time(); }
 
 std::vector<MatrixXdr> OrbitKalman::get_R(int kind, int n) {
-  std::vector<MatrixXdr> R;
-  for (int i = 0; i < n; i++) {
-    R.push_back(this->obs_noise[kind]);
-  }
-  return R;
+  return std::vector<MatrixXdr>(n, obs_noise.at(kind));
 }
 
 std::optional<Estimate> OrbitKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
-  std::optional<Estimate> r;
-  if (R.size() == 0) {
-    R = this->get_R(kind, meas.size());
-  }
-  r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
-  return r;
+  return filter->predict_and_observe(t, kind, meas, R);
 }
 
-void OrbitKalman::predict(double t) {
-  this->filter->predict(t);
-}
-
-const Eigen::VectorXd &OrbitKalman::get_initial_x() {
-  return this->initial_x;
-}
-
-const MatrixXdr &OrbitKalman::get_initial_P() {
-  return this->initial_P;
-}
-
-const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() {
-  return this->fake_gps_pos_cov;
-}
-
-const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() {
-  return this->fake_gps_vel_cov;
-}
-
-const MatrixXdr &OrbitKalman::get_reset_orientation_P() {
-  return this->reset_orientation_P;
-}
+void OrbitKalman::predict(double t) { filter->predict(t); }
+const VectorXd &OrbitKalman::get_initial_x() { return initial_x; }
+const MatrixXdr &OrbitKalman::get_initial_P() { return initial_P; }
+const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() { return fake_gps_pos_cov; }
+const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() { return fake_gps_vel_cov; }
+const MatrixXdr &OrbitKalman::get_reset_orientation_P() { return reset_orientation_P; }
 
 MatrixXdr OrbitKalman::H(const VectorXd &in) {
-  assert(in.size() == 6);
-  Matrix<double, 3, 6, Eigen::RowMajor> res;
-  this->filter->get_extra_routine("H")((double*)in.data(), res.data());
-  return res;
+  if (in.size() != 6) throw std::invalid_argument("local velocity input dimension mismatch");
+  auto function = [](const VectorXd &value) {
+    const Matrix3d transform = (Eigen::AngleAxisd(value(2), Vector3d::UnitZ()) * Eigen::AngleAxisd(value(1), Vector3d::UnitY()) *
+                                Eigen::AngleAxisd(value(0), Vector3d::UnitX())).toRotationMatrix();
+    return transform.transpose() * value.segment<3>(3);
+  };
+  MatrixXdr result(3, 6);
+  for (int index = 0; index < 6; ++index) {
+    const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(in(index)));
+    VectorXd upper = in;
+    VectorXd lower = in;
+    upper(index) += step;
+    lower(index) -= step;
+    result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
+  }
+  return result;
 }

iqpilot/selfdrive/iqlocd/models/orbit_kf.h

@@ -1,66 +1,46 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
 #pragma once
 
-#include <string>
-#include <cmath>
 #include <memory>
+#include <optional>
 #include <unordered_map>
 #include <vector>
 
-#include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
 
-#include "generated/orbit_state_constants.h"
-#include "rednose/helpers/ekf_sym.h"
+#include "iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h"
+#include "iqpilot/selfdrive/state_estimation/estimator.h"
 
-#define EARTH_GM 3.986005e14  // m^3/s^2 (gravitational constant * mass of earth)
-
-using namespace EKFS;
-
-Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
-Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
-std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
-std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
+using MatrixXdr = iqpilot::state_estimation::Matrix;
+using Estimate = iqpilot::state_estimation::Estimate;
 
 class OrbitKalman {
 public:
   OrbitKalman();
-
   void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
   void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
   void init_state(const Eigen::VectorXd &state, double filter_time);
-
   Eigen::VectorXd get_x();
   MatrixXdr get_P();
   double get_filter_time();
   std::vector<MatrixXdr> get_R(int kind, int n);
-
   std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
-  std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
-  std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
-  std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
   void predict(double t);
-
   const Eigen::VectorXd &get_initial_x();
   const MatrixXdr &get_initial_P();
   const MatrixXdr &get_fake_gps_pos_cov();
   const MatrixXdr &get_fake_gps_vel_cov();
   const MatrixXdr &get_reset_orientation_P();
-
   MatrixXdr H(const Eigen::VectorXd &in);
 
 private:
-  std::string name = "orbit";
-
-  std::shared_ptr<EKFSym> filter;
-
-  int dim_state;
-  int dim_state_err;
-
+  std::shared_ptr<iqpilot::state_estimation::StateEstimator> filter;
   Eigen::VectorXd initial_x;
   MatrixXdr initial_P;
   MatrixXdr fake_gps_pos_cov;
   MatrixXdr fake_gps_vel_cov;
   MatrixXdr reset_orientation_P;
-  MatrixXdr Q;  // process noise
   std::unordered_map<int, MatrixXdr> obs_noise;
 };

iqpilot/selfdrive/iqlocd/models/orbit_kf.py

@@ -1,242 +0,0 @@
-#!/usr/bin/env python3
-
-import sys
-import os
-import numpy as np
-
-from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
-
-import sympy as sp
-import inspect
-from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
-from rednose.helpers.ekf_sym import gen_code
-
-EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
-
-
-def numpy2eigenstring(arr):
-  assert(len(arr.shape) == 1)
-  arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
-  return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
-
-
-class States:
-  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
-  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
-  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
-  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
-  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
-  ACCELERATION = slice(16, 19)  # Acceleration in device frame in m/s**2
-  ACC_BIAS = slice(19, 22)  # Acceletometer bias in m/s**2
-
-  # Error-state has different slices because it is an ESKF
-  ECEF_POS_ERR = slice(0, 3)
-  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
-  ECEF_VELOCITY_ERR = slice(6, 9)
-  ANGULAR_VELOCITY_ERR = slice(9, 12)
-  GYRO_BIAS_ERR = slice(12, 15)
-  ACCELERATION_ERR = slice(15, 18)
-  ACC_BIAS_ERR = slice(18, 21)
-
-
-class OrbitScopeModel:
-  name = 'orbit'
-
-  initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
-                        0.42254641, -0.31238054, -0.83602975, -0.15788347,  # NED [0,0,0] -> ECEF Quat
-                        0, 0, 0,
-                        0, 0, 0,
-                        0, 0, 0,
-                        0, 0, 0,
-                        0, 0, 0])
-
-  # state covariance
-  initial_P_diag = np.array([10**2, 10**2, 10**2,
-                             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])
-
-  # state covariance when resetting midway in a segment
-  reset_orientation_diag = np.array([1**2, 1**2, 1**2])
-
-  # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
-  fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
-  fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
-
-  # process noise
-  Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
-                     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_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
-                    ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
-                    ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
-                    ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
-                    ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
-                    ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
-                    ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
-                    ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
-
-  @staticmethod
-  def generate_code(generated_dir):
-    name = OrbitScopeModel.name
-    dim_state = OrbitScopeModel.initial_x.shape[0]
-    dim_state_err = OrbitScopeModel.initial_P_diag.shape[0]
-
-    state_sym = sp.MatrixSymbol('state', dim_state, 1)
-    state = sp.Matrix(state_sym)
-    x, y, z = state[States.ECEF_POS, :]
-    q = state[States.ECEF_ORIENTATION, :]
-    v = state[States.ECEF_VELOCITY, :]
-    vx, vy, vz = v
-    omega = state[States.ANGULAR_VELOCITY, :]
-    vroll, vpitch, vyaw = omega
-    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
-    acceleration = state[States.ACCELERATION, :]
-    acc_bias = state[States.ACC_BIAS, :]
-
-    dt = sp.Symbol('dt')
-
-    # calibration and attitude rotation matrices
-    quat_rot = quat_rotate(*q)
-
-    # Got the quat predict equations from here
-    # A New Quaternion-Based Kalman Filter for
-    # Real-Time Attitude Estimation Using the Two-Step
-    # Geometrically-Intuitive Correction Algorithm
-    A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
-                         [vroll, 0, vyaw, -vpitch],
-                         [vpitch, -vyaw, 0, vroll],
-                         [vyaw, vpitch, -vroll, 0]])
-    q_dot = A * q
-
-    # Time derivative of the state as a function of state
-    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
-    state_dot[States.ECEF_POS, :] = v
-    state_dot[States.ECEF_ORIENTATION, :] = q_dot
-    state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
-
-    # Basic descretization, 1st order intergrator
-    # Can be pretty bad if dt is big
-    f_sym = state + dt * state_dot
-
-    state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
-    state_err = sp.Matrix(state_err_sym)
-    quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
-    v_err = state_err[States.ECEF_VELOCITY_ERR, :]
-    omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
-    acceleration_err = state_err[States.ACCELERATION_ERR, :]
-
-    # Time derivative of the state error as a function of state error and state
-    quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
-    q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
-    state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
-    state_err_dot[States.ECEF_POS_ERR, :] = v_err
-    state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
-    state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
-    f_err_sym = state_err + dt * state_err_dot
-
-    # Observation matrix modifier
-    H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
-    H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
-    H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
-    H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
-
-    # these error functions are defined so that say there
-    # is a nominal x and true x:
-    # true x = err_function(nominal x, delta x)
-    # delta x = inv_err_function(nominal x, true x)
-    nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
-    true_x = sp.MatrixSymbol('true_x', dim_state, 1)
-    delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
-
-    err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
-    delta_quat = sp.Matrix(np.ones(4))
-    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
-    err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
-    err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
-    err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
-
-    inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
-    inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
-    delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
-    inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
-    inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
-
-    eskf_params = [[err_function_sym, nom_x, delta_x],
-                   [inv_err_function_sym, nom_x, true_x],
-                   H_mod_sym, f_err_sym, state_err_sym]
-    #
-    # Observation functions
-    #
-    h_gyro_sym = sp.Matrix([
-      vroll + roll_bias,
-      vpitch + pitch_bias,
-      vyaw + yaw_bias])
-
-    pos = sp.Matrix([x, y, z])
-    gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
-    h_acc_sym = (gravity + acceleration + acc_bias)
-    h_acc_stationary_sym = acceleration
-    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
-    h_pos_sym = sp.Matrix([x, y, z])
-    h_vel_sym = sp.Matrix([vx, vy, vz])
-    h_orientation_sym = q
-    h_relative_motion = sp.Matrix(quat_rot.T * v)
-
-    obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
-               [h_phone_rot_sym, ObservationKind.NO_ROT, None],
-               [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
-               [h_pos_sym, ObservationKind.ECEF_POS, None],
-               [h_vel_sym, ObservationKind.ECEF_VEL, None],
-               [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
-               [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
-               [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
-               [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
-
-    # this returns a sympy routine for the jacobian of the observation function of the local vel
-    in_vec = sp.MatrixSymbol('in_vec', 6, 1)  # roll, pitch, yaw, vx, vy, vz
-    h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
-    extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
-
-    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
-
-    # write constants to extra header file for use in cpp
-    orbit_header = "#pragma once\n\n"
-    orbit_header += "#include <unordered_map>\n"
-    orbit_header += "#include <eigen3/Eigen/Dense>\n\n"
-    for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
-      assert(slc.step is None)  # unsupported
-      orbit_header += f'#define STATE_{state}_START {slc.start}\n'
-      orbit_header += f'#define STATE_{state}_END {slc.stop}\n'
-      orbit_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
-    orbit_header += "\n"
-
-    for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
-      orbit_header += f'#define OBSERVATION_{kind} {val}\n'
-    orbit_header += "\n"
-
-    orbit_header += f"static const Eigen::VectorXd orbit_initial_x = {numpy2eigenstring(OrbitScopeModel.initial_x)};\n"
-    orbit_header += f"static const Eigen::VectorXd orbit_initial_P_diag = {numpy2eigenstring(OrbitScopeModel.initial_P_diag)};\n"
-    orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_pos_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_pos_cov_diag)};\n"
-    orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_vel_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_vel_cov_diag)};\n"
-    orbit_header += f"static const Eigen::VectorXd orbit_reset_orientation_diag = {numpy2eigenstring(OrbitScopeModel.reset_orientation_diag)};\n"
-    orbit_header += f"static const Eigen::VectorXd orbit_Q_diag = {numpy2eigenstring(OrbitScopeModel.Q_diag)};\n"
-    orbit_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> orbit_obs_noise_diag = {\n"
-    for kind, noise in OrbitScopeModel.obs_noise_diag.items():
-      orbit_header += f"  {{ {kind}, {numpy2eigenstring(noise)} }},\n"
-    orbit_header += "};\n\n"
-
-    open(os.path.join(generated_dir, "orbit_state_constants.h"), 'w').write(orbit_header)
-
-
-if __name__ == "__main__":
-  generated_dir = sys.argv[2]
-  OrbitScopeModel.generate_code(generated_dir)

iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h

@@ -0,0 +1,42 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
+#pragma once
+
+#define STATE_ECEF_POS_START 0
+#define STATE_ECEF_POS_LEN 3
+#define STATE_ECEF_ORIENTATION_START 3
+#define STATE_ECEF_ORIENTATION_LEN 4
+#define STATE_ECEF_VELOCITY_START 7
+#define STATE_ECEF_VELOCITY_LEN 3
+#define STATE_ANGULAR_VELOCITY_START 10
+#define STATE_ANGULAR_VELOCITY_LEN 3
+#define STATE_GYRO_BIAS_START 13
+#define STATE_GYRO_BIAS_LEN 3
+#define STATE_ACCELERATION_START 16
+#define STATE_ACCELERATION_LEN 3
+#define STATE_ACC_BIAS_START 19
+#define STATE_ACC_BIAS_LEN 3
+#define STATE_ECEF_POS_ERR_START 0
+#define STATE_ECEF_POS_ERR_LEN 3
+#define STATE_ECEF_ORIENTATION_ERR_START 3
+#define STATE_ECEF_ORIENTATION_ERR_LEN 3
+#define STATE_ECEF_VELOCITY_ERR_START 6
+#define STATE_ECEF_VELOCITY_ERR_LEN 3
+#define STATE_ANGULAR_VELOCITY_ERR_START 9
+#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
+#define STATE_GYRO_BIAS_ERR_START 12
+#define STATE_GYRO_BIAS_ERR_LEN 3
+#define STATE_ACCELERATION_ERR_START 15
+#define STATE_ACCELERATION_ERR_LEN 3
+#define STATE_ACC_BIAS_ERR_START 18
+#define STATE_ACC_BIAS_ERR_LEN 3
+#define OBSERVATION_PHONE_GYRO 4
+#define OBSERVATION_NO_ROT 9
+#define OBSERVATION_PHONE_ACCEL 10
+#define OBSERVATION_ECEF_POS 12
+#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
+#define OBSERVATION_CAMERA_ODO_ROTATION 14
+#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
+#define OBSERVATION_NO_ACCEL 33
+#define OBSERVATION_ECEF_VEL 35

iqpilot/selfdrive/locationd/SConscript

@@ -1,21 +1,5 @@
-Import('env', 'rednose')
+Import('env', 'envCython')
 
-# build ekf models
-rednose_gen_dir = 'models/generated'
-rednose_gen_deps = [
-  "models/constants.py",
-]
-pose_ekf = env.RednoseCompileFilter(
-  target='pose',
-  filter_gen_script='models/pose_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=[],
-  gen_script_deps=rednose_gen_deps,
-)
-car_ekf = env.RednoseCompileFilter(
-  target='car',
-  filter_gen_script='models/car_kf.py',
-  output_dir=rednose_gen_dir,
-  extra_gen_artifacts=[],
-  gen_script_deps=rednose_gen_deps,
-)
+native_kernel = env.StaticLibrary("../state_estimation/native_kernels", ["../state_estimation/native_kernels.cc"])
+native_binding_env = envCython.Clone(CYTHONFLAGS=["--cplus"])
+native_binding_env.Program("../state_estimation/native_binding_pyx.so", ["../state_estimation/native_binding_pyx.pyx"], LIBS=[native_kernel] + envCython["LIBS"])

iqpilot/selfdrive/locationd/locationd.py

@@ -15,7 +15,7 @@ from iqpilot.common.swaglog import cloudlog
 from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
 from iqpilot.selfdrive.locationd.helpers import rotate_std
 from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
-from iqpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
 
 ACCEL_SANITY_CHECK = 100.0  # m/s^2
 ROTATION_SANITY_CHECK = 10.0  # rad/s
@@ -51,7 +51,7 @@ class HandleLogResult(Enum):
 
 class LocationEstimator:
   def __init__(self, debug: bool):
-    self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)
+    self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
 
     self.debug = debug
 

iqpilot/selfdrive/locationd/models/car_kf.py

@@ -1,75 +1,63 @@
-#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
 import math
-import sys
 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.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
+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:
-  # 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]
+  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)
 
 
-class CarKalman(KalmanFilter):
-  name = 'car'
+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
 
-  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,
-  ])
+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),
@@ -79,102 +67,28 @@ class CarKalman(KalmanFilter):
     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',
-  ]
+  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)
 
-  @staticmethod
-  def generate_code(generated_dir):
-    dim_state = CarKalman.initial_x.shape[0]
-    name = CarKalman.name
+    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))
 
-    # 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)
+  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)

iqpilot/selfdrive/locationd/models/constants.py

@@ -1,7 +1,3 @@
-import os
-
-GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
-
 class ObservationKind:
   UNKNOWN = 0
   NO_OBSERVATION = 1

iqpilot/selfdrive/locationd/models/pose_kf.py

@@ -1,111 +1,67 @@
-#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
 
-import sys
 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
 
-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
+  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)
 
 
-class PoseKalman(KalmanFilter):
+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),
+  }
 
-  # 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])
+  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)
 
-  # 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)
+    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))

iqpilot/selfdrive/locationd/paramsd.py

@@ -8,7 +8,6 @@ from iqpilot.common.issue_debug import log_issue_limited
 from iqpilot.common.params import Params
 from iqpilot.common.realtime import DT_MDL
 from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
-from iqpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from iqpilot.common.swaglog import cloudlog
 
@@ -26,7 +25,7 @@ LOW_ACTIVE_SPEED = 10.0
 
 class VehicleParamsEstimator:
   def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
-    self.kf = CarKalman(GENERATED_DIR)
+    self.kf = CarKalman()
 
     self.x_initial = CarKalman.initial_x.copy()
     self.x_initial[States.STEER_RATIO] = steer_ratio

iqpilot/selfdrive/state_estimation/__init__.py

@@ -0,0 +1,7 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from iqpilot.selfdrive.state_estimation.estimator import EstimatorModel, ModelDefinition, Observation, StateEstimator
+
+__all__ = ["EstimatorModel", "ModelDefinition", "Observation", "StateEstimator"]

iqpilot/selfdrive/state_estimation/benchmark.py

@@ -0,0 +1,38 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import json
+import time
+
+import numpy as np
+
+from iqpilot.selfdrive.locationd.models.car_kf import CarKalman
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman
+
+
+def measure(function, count: int) -> dict[str, float]:
+  samples = np.empty(count)
+  for index in range(count):
+    started = time.perf_counter_ns()
+    function(index)
+    samples[index] = (time.perf_counter_ns() - started) / 1000.0
+  return {"p50_us": float(np.percentile(samples, 50)), "p99_us": float(np.percentile(samples, 99)), "mean_us": float(samples.mean())}
+
+
+def main() -> None:
+  car = CarKalman()
+  car.set_globals(1800.0, 2500.0, 1.2, 1.6, 90000.0, 100000.0)
+  car.init_state(CarKalman.initial_x, CarKalman.P_initial, 0.0)
+  pose = PoseKalman(0.8)
+  pose.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0)
+  result = {
+    "car": measure(lambda index: car.predict_and_observe(index * 0.01, ObservationKind.ROAD_FRAME_X_SPEED, np.array([15.0])), 1000),
+    "pose": measure(lambda index: pose.predict_and_observe(index * 0.01, ObservationKind.PHONE_GYRO, np.zeros(3)), 1000),
+  }
+  print(json.dumps(result, sort_keys=True))
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/state_estimation/estimator.h

@@ -0,0 +1,157 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
+#pragma once
+
+#include <cmath>
+#include <functional>
+#include <optional>
+#include <stdexcept>
+#include <unordered_map>
+#include <vector>
+
+#include <eigen3/Eigen/Dense>
+
+namespace iqpilot::state_estimation {
+
+using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+using Vector = Eigen::VectorXd;
+
+struct Estimate {
+  double time;
+  Vector state;
+  Matrix covariance;
+  std::vector<Vector> innovations;
+};
+
+struct ModelDefinition {
+  int state_size;
+  int error_size;
+  std::function<Vector(const Vector &, double)> transition;
+  std::unordered_map<int, std::function<Vector(const Vector &)>> measurements;
+  Matrix process_noise;
+  std::unordered_map<int, Matrix> observation_noise;
+  std::function<Vector(const Vector &, const Vector &)> inject_error;
+  std::function<Matrix(const Vector &)> error_projection;
+  std::function<Vector(const Vector &)> normalize;
+  std::function<Matrix(const Vector &, double)> error_transition;
+  std::unordered_map<int, std::function<Matrix(const Vector &)>> observation_jacobians;
+};
+
+class StateEstimator {
+public:
+  StateEstimator(ModelDefinition model, Vector state, Matrix covariance) : model_(std::move(model)) {
+    init_state(state, covariance, NAN);
+  }
+
+  void init_state(const Vector &state, const Matrix &covariance, double time) {
+    if (state.size() != model_.state_size || covariance.rows() != model_.error_size || covariance.cols() != model_.error_size) {
+      throw std::invalid_argument("estimator initialization dimension mismatch");
+    }
+    state_ = normalize(state);
+    covariance_ = stabilize(covariance);
+    time_ = time;
+  }
+
+  void predict(double time) {
+    if (std::isnan(time_)) {
+      time_ = time;
+      return;
+    }
+    if (time < time_) {
+      throw std::invalid_argument("prediction time precedes estimator time");
+    }
+    const double dt = time - time_;
+    if (dt == 0.0) return;
+    const Vector previous = state_;
+    const Vector predicted = model_.transition(previous, dt);
+    Matrix error_transition;
+    if (model_.error_transition) {
+      error_transition = model_.error_transition(previous, dt);
+    } else {
+      const Matrix state_jacobian = jacobian([this, dt](const Vector &value) { return model_.transition(value, dt); }, previous);
+      error_transition = error_projection(predicted).completeOrthogonalDecomposition().pseudoInverse() * state_jacobian * error_projection(previous);
+    }
+    state_ = normalize(predicted);
+    covariance_ = error_transition * covariance_ * error_transition.transpose() + dt * model_.process_noise;
+    time_ = time;
+  }
+
+  std::optional<Estimate> predict_and_observe(double time, int kind, const std::vector<Vector> &measurements,
+                                               const std::vector<Matrix> &noise = {}) {
+    if (!std::isnan(time_) && time < time_) return std::nullopt;
+    predict(time);
+    auto measurement_function = model_.measurements.find(kind);
+    if (measurement_function == model_.measurements.end()) throw std::invalid_argument("unknown observation kind");
+    std::vector<Vector> innovations;
+    for (size_t index = 0; index < measurements.size(); ++index) {
+      const Matrix &measurement_noise = noise.empty() ? model_.observation_noise.at(kind) : noise.at(index);
+      const Vector expected = measurement_function->second(state_);
+      if (measurements[index].size() != expected.size() || measurement_noise.rows() != expected.size() || measurement_noise.cols() != expected.size()) {
+        throw std::invalid_argument("observation dimension mismatch");
+      }
+      const Vector innovation = measurements[index] - expected;
+      Matrix observation_jacobian;
+      auto analytic_jacobian = model_.observation_jacobians.find(kind);
+      if (analytic_jacobian != model_.observation_jacobians.end()) {
+        observation_jacobian = analytic_jacobian->second(state_);
+      } else {
+        const Matrix state_jacobian = jacobian(measurement_function->second, state_);
+        observation_jacobian = state_jacobian * error_projection(state_);
+      }
+      const Matrix innovation_covariance = observation_jacobian * covariance_ * observation_jacobian.transpose() + measurement_noise;
+      const Matrix gain = innovation_covariance.ldlt().solve(observation_jacobian * covariance_).transpose();
+      state_ = normalize(inject(state_, gain * innovation));
+      const Matrix identity = Matrix::Identity(model_.error_size, model_.error_size);
+      const Matrix residual = identity - gain * observation_jacobian;
+      covariance_ = residual * covariance_ * residual.transpose() + gain * measurement_noise * gain.transpose();
+      if (!state_.allFinite() || !covariance_.allFinite()) throw std::runtime_error("estimator produced non-finite values");
+      innovations.push_back(innovation);
+    }
+    return Estimate{time_, state_, covariance_, innovations};
+  }
+
+  const Vector &state() const { return state_; }
+  const Matrix &covariance() const { return covariance_; }
+  double time() const { return time_; }
+
+private:
+  Matrix jacobian(const std::function<Vector(const Vector &)> &function, const Vector &value) const {
+    const Vector output = function(value);
+    Matrix result(output.size(), value.size());
+    for (int index = 0; index < value.size(); ++index) {
+      const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(value(index)));
+      Vector upper = value;
+      Vector lower = value;
+      upper(index) += step;
+      lower(index) -= step;
+      result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
+    }
+    return result;
+  }
+
+  Vector inject(const Vector &state, const Vector &delta) const {
+    return model_.inject_error ? model_.inject_error(state, delta) : state + delta;
+  }
+
+  Matrix error_projection(const Vector &state) const {
+    return model_.error_projection ? model_.error_projection(state) : Matrix::Identity(model_.state_size, model_.error_size);
+  }
+
+  Vector normalize(const Vector &state) const {
+    return model_.normalize ? model_.normalize(state) : state;
+  }
+
+  Matrix stabilize(const Matrix &covariance) const {
+    Matrix symmetric = (covariance + covariance.transpose()) * 0.5;
+    if (!symmetric.allFinite()) throw std::runtime_error("invalid covariance");
+    return symmetric;
+  }
+
+  ModelDefinition model_;
+  Vector state_;
+  Matrix covariance_;
+  double time_ = NAN;
+};
+
+}

iqpilot/selfdrive/state_estimation/estimator.py

@@ -0,0 +1,311 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from collections.abc import Callable
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+
+Array = np.ndarray
+Prediction = Callable[[Array, float, dict[str, float]], Array]
+Measurement = Callable[[Array, dict[str, float]], Array]
+Injection = Callable[[Array, Array], Array]
+NativePrediction = Callable[[Array, Array, float, Array, dict[str, float]], None]
+NativeUpdate = Callable[[Array, Array, int, Array, Array], Array]
+
+
+@dataclass(frozen=True)
+class Observation:
+  kind: int
+  values: Array
+  noise: Array
+
+
+@dataclass(frozen=True)
+class ModelDefinition:
+  state_size: int
+  error_size: int
+  transition: Prediction
+  measurements: dict[int, Measurement]
+  process_noise: Array
+  observation_noise: dict[int, Array]
+  inject_error: Injection | None = None
+  error_projection: Callable[[Array], Array] | None = None
+  normalize: Callable[[Array], Array] | None = None
+  native_predict: NativePrediction | None = None
+  native_update: NativeUpdate | None = None
+
+
+@dataclass
+class _Snapshot:
+  time: float
+  state: Array
+  covariance: Array
+
+
+@dataclass
+class _Event:
+  time: float
+  observation: Observation
+  order: int
+
+
+class StateEstimator:
+  def __init__(self, model: ModelDefinition, initial_state: Array, initial_covariance: Array,
+               max_rewind_age: float = 0.0):
+    self.model = model
+    self.parameters: dict[str, float] = {}
+    self.max_rewind_age = max_rewind_age
+    self._order = 0
+    self.init_state(initial_state, initial_covariance, None)
+
+  @property
+  def x(self) -> Array:
+    return self._state.copy()
+
+  @property
+  def P(self) -> Array:
+    return self._covariance.copy()
+
+  @property
+  def t(self) -> float:
+    return self._time
+
+  def set_global(self, name: str, value: float) -> None:
+    self.parameters[name] = float(value)
+
+  def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None:
+    state = np.asarray(state, dtype=np.float64).reshape(-1)
+    covariance = np.asarray(covs, dtype=np.float64)
+    self._validate_state(state, covariance)
+    self._state = self._normalize(state.copy())
+    self._covariance = self._stabilize(covariance.copy())
+    self._time = math.nan if filter_time is None else float(filter_time)
+    self._events: list[_Event] = []
+    self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())]
+
+  def set_filter_time(self, filter_time: float | None) -> None:
+    self._time = math.nan if filter_time is None else float(filter_time)
+
+  def reset_rewind(self) -> None:
+    self._events.clear()
+    self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())]
+
+  def predict(self, time: float) -> None:
+    time = float(time)
+    if math.isnan(self._time):
+      self._time = time
+      return
+    if time < self._time:
+      raise ValueError("prediction time precedes estimator time")
+    dt = time - self._time
+    if dt == 0.0:
+      return
+    if self.model.native_predict is not None:
+      self.model.native_predict(self._state, self._covariance, dt, self.model.process_noise, self.parameters)
+      self._time = time
+      return
+    previous = self._state.copy()
+    transition_jacobian = self._jacobian(lambda value: self.model.transition(value, dt, self.parameters), previous)
+    predicted = self.model.transition(previous, dt, self.parameters)
+    projection = self._error_projection(previous)
+    if self.model.error_size == self.model.state_size:
+      error_transition = transition_jacobian
+    else:
+      error_transition = np.linalg.pinv(self._error_projection(predicted)) @ transition_jacobian @ projection
+    self._state = self._normalize(predicted)
+    self._covariance = self._stabilize(error_transition @ self._covariance @ error_transition.T + dt * self.model.process_noise)
+    self._time = time
+
+  def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None):
+    values = self._measurement_batch(kind, measurements)
+    noises = self._noise_batch(kind, len(values), noise)
+    event = _Event(float(time), Observation(kind, values, noises), self._order)
+    self._order += 1
+    if not math.isnan(self._time) and event.time < self._time:
+      if self.max_rewind_age <= 0.0 or self._time - event.time > self.max_rewind_age:
+        return None
+      return self._rewind(event)
+    result = self._apply_event(event)
+    self._events.append(event)
+    self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy()))
+    self._trim_history()
+    return result
+
+  def _apply_event(self, event: _Event):
+    self.predict(event.time)
+    prior_state = self._state.copy()
+    prior_covariance = self._covariance.copy()
+    innovations = []
+    for measurement, noise in zip(event.observation.values, event.observation.noise, strict=True):
+      innovations.append(self._update(event.observation.kind, measurement, noise))
+    return (event.time, self.x, prior_state, self.P, prior_covariance, event.observation.kind,
+            tuple(innovations), event.observation.values.copy(), event.observation.noise.copy())
+
+  def _update(self, kind: int, measurement: Array, noise: Array) -> Array:
+    measurement_function = self.model.measurements.get(kind)
+    if measurement_function is None:
+      raise KeyError(f"unknown observation kind {kind}")
+    measurement = np.asarray(measurement, dtype=np.float64).reshape(-1)
+    if noise.shape != (measurement.size, measurement.size):
+      raise ValueError("observation noise dimension mismatch")
+    if self.model.native_update is not None:
+      innovation = self.model.native_update(self._state, self._covariance, kind, measurement, noise)
+      return innovation
+    predicted = np.asarray(measurement_function(self._state, self.parameters), dtype=np.float64).reshape(-1)
+    if predicted.shape != measurement.shape:
+      raise ValueError("measurement dimension mismatch")
+    innovation = measurement - predicted
+    state_jacobian = self._jacobian(lambda value: measurement_function(value, self.parameters), self._state)
+    observation_jacobian = state_jacobian @ self._error_projection(self._state)
+    innovation_covariance = observation_jacobian @ self._covariance @ observation_jacobian.T + noise
+    gain = np.linalg.solve(innovation_covariance, observation_jacobian @ self._covariance).T
+    delta = gain @ innovation
+    self._state = self._normalize(self._inject(self._state, delta))
+    identity = np.eye(self.model.error_size)
+    residual = identity - gain @ observation_jacobian
+    self._covariance = self._stabilize(residual @ self._covariance @ residual.T + gain @ noise @ gain.T)
+    self._require_finite()
+    return innovation
+
+  def _rewind(self, new_event: _Event):
+    events = sorted(self._events + [new_event], key=lambda event: (event.time, event.order))
+    base_index = max(i for i, snapshot in enumerate(self._snapshots) if math.isnan(snapshot.time) or snapshot.time <= new_event.time)
+    base = self._snapshots[base_index]
+    retained = self._events[:base_index]
+    retained_orders = {event.order for event in retained}
+    replay = [event for event in events if event.order not in retained_orders]
+    self._state = base.state.copy()
+    self._covariance = base.covariance.copy()
+    self._time = base.time
+    self._events = retained.copy()
+    self._snapshots = self._snapshots[:base_index + 1]
+    result = None
+    for event in replay:
+      current = self._apply_event(event)
+      self._events.append(event)
+      self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy()))
+      if event is new_event:
+        result = current
+    self._trim_history()
+    return result
+
+  def _trim_history(self) -> None:
+    if self.max_rewind_age <= 0.0 or math.isnan(self._time):
+      return
+    cutoff = self._time - self.max_rewind_age
+    remove = 0
+    while remove < len(self._events) and self._events[remove].time < cutoff:
+      remove += 1
+    if remove:
+      self._events = self._events[remove:]
+      self._snapshots = self._snapshots[remove:]
+
+  def _measurement_batch(self, kind: int, measurements: Array) -> Array:
+    measurement_function = self.model.measurements.get(kind)
+    if measurement_function is None:
+      raise KeyError(f"unknown observation kind {kind}")
+    if self.model.native_update is not None and kind in self.model.observation_noise:
+      expected = self.model.observation_noise[kind].shape[0]
+    else:
+      expected = np.asarray(measurement_function(self._state, self.parameters)).size
+    values = np.asarray(measurements, dtype=np.float64)
+    if values.ndim == 1:
+      values = values.reshape(1, -1)
+    elif values.ndim != 2:
+      raise ValueError("measurements must be one or two dimensional")
+    if values.shape[1] != expected:
+      raise ValueError("measurement dimension mismatch")
+    return values
+
+  def _noise_batch(self, kind: int, count: int, noise: Array | None) -> Array:
+    if noise is None:
+      base = self.model.observation_noise.get(kind)
+      if base is None:
+        raise KeyError(f"missing observation noise for kind {kind}")
+      return np.repeat(np.asarray(base, dtype=np.float64)[None, :, :], count, axis=0)
+    noises = np.asarray(noise, dtype=np.float64)
+    if noises.ndim == 2:
+      noises = noises[None, :, :]
+    if noises.shape[0] == 1 and count > 1:
+      noises = np.repeat(noises, count, axis=0)
+    if noises.shape[0] != count:
+      raise ValueError("observation noise batch mismatch")
+    return noises
+
+  def _jacobian(self, function: Callable[[Array], Array], value: Array) -> Array:
+    output = np.asarray(function(value), dtype=np.float64).reshape(-1)
+    result = np.empty((output.size, value.size), dtype=np.float64)
+    for index in range(value.size):
+      step = np.cbrt(np.finfo(np.float64).eps) * max(1.0, abs(value[index]))
+      upper = value.copy()
+      lower = value.copy()
+      upper[index] += step
+      lower[index] -= step
+      result[:, index] = (np.asarray(function(upper)).reshape(-1) - np.asarray(function(lower)).reshape(-1)) / (2.0 * step)
+    return result
+
+  def _inject(self, state: Array, delta: Array) -> Array:
+    if self.model.inject_error is None:
+      return state + delta
+    return self.model.inject_error(state, delta)
+
+  def _error_projection(self, state: Array) -> Array:
+    if self.model.error_projection is None:
+      return np.eye(self.model.state_size, self.model.error_size)
+    return np.asarray(self.model.error_projection(state), dtype=np.float64)
+
+  def _normalize(self, state: Array) -> Array:
+    if self.model.normalize is None:
+      return np.asarray(state, dtype=np.float64).reshape(-1)
+    return np.asarray(self.model.normalize(state), dtype=np.float64).reshape(-1)
+
+  def _stabilize(self, covariance: Array) -> Array:
+    covariance = (covariance + covariance.T) * 0.5
+    eigenvalues, eigenvectors = np.linalg.eigh(covariance)
+    if eigenvalues[0] < -1e-10:
+      raise FloatingPointError("covariance is not positive semidefinite")
+    return (eigenvectors * np.maximum(eigenvalues, 0.0)) @ eigenvectors.T
+
+  def _validate_state(self, state: Array, covariance: Array) -> None:
+    if state.shape != (self.model.state_size,):
+      raise ValueError("state dimension mismatch")
+    if covariance.shape != (self.model.error_size, self.model.error_size):
+      raise ValueError("covariance dimension mismatch")
+    if self.model.process_noise.shape != covariance.shape:
+      raise ValueError("process noise dimension mismatch")
+    if not np.isfinite(state).all() or not np.isfinite(covariance).all():
+      raise ValueError("state and covariance must be finite")
+
+  def _require_finite(self) -> None:
+    if not np.isfinite(self._state).all() or not np.isfinite(self._covariance).all():
+      raise FloatingPointError("estimator produced non-finite values")
+
+
+class EstimatorModel:
+  def __init__(self, estimator: StateEstimator):
+    self.filter = estimator
+
+  @property
+  def x(self) -> Array:
+    return self.filter.x
+
+  @property
+  def P(self) -> Array:
+    return self.filter.P
+
+  @property
+  def t(self) -> float:
+    return self.filter.t
+
+  def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None:
+    self.filter.init_state(state, covs, filter_time)
+
+  def predict(self, time: float) -> None:
+    self.filter.predict(time)
+
+  def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None):
+    return self.filter.predict_and_observe(time, kind, measurements, noise)

iqpilot/selfdrive/state_estimation/native_binding_pyx.pyx

@@ -0,0 +1,48 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+cimport numpy as np
+
+
+cdef extern from "iqpilot/selfdrive/state_estimation/native_kernels.h":
+  void iq_estimator_car_predict(double *, double *, const double *, double, const double *)
+  void iq_estimator_car_update(double *, double *, int, const double *, const double *, double *)
+  void iq_estimator_pose_predict(double *, double *, const double *, double)
+  void iq_estimator_pose_update(double *, double *, int, const double *, const double *, double *)
+
+
+def car_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
+                np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
+                np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise,
+                double dt,
+                np.ndarray[np.float64_t, ndim=1, mode="c"] parameters):
+  iq_estimator_car_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt, &parameters[0])
+
+
+def car_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
+               np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
+               int kind,
+               np.ndarray[np.float64_t, ndim=1, mode="c"] measurement,
+               np.ndarray[np.float64_t, ndim=2, mode="c"] noise):
+  cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size)
+  iq_estimator_car_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0])
+  return innovation
+
+
+def pose_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
+                 np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
+                 np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise,
+                 double dt):
+  iq_estimator_pose_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt)
+
+
+def pose_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
+                np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
+                int kind,
+                np.ndarray[np.float64_t, ndim=1, mode="c"] measurement,
+                np.ndarray[np.float64_t, ndim=2, mode="c"] noise):
+  cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size)
+  iq_estimator_pose_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0])
+  return innovation

iqpilot/selfdrive/state_estimation/native_kernels.cc

@@ -0,0 +1,231 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
+#include "iqpilot/selfdrive/state_estimation/native_kernels.h"
+
+#include <array>
+#include <cmath>
+#include <stdexcept>
+
+#include <eigen3/Eigen/Dense>
+
+namespace {
+
+using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+using Vector = Eigen::VectorXd;
+
+template <int N>
+using FixedMatrix = Eigen::Matrix<double, N, N, Eigen::RowMajor>;
+
+template <int N>
+using FixedVector = Eigen::Matrix<double, N, 1>;
+
+template <int N, typename Function>
+void predict(double *state_data, double *covariance_data, const double *noise_data, double dt, Function function) {
+  Eigen::Map<FixedVector<N>> state(state_data);
+  Eigen::Map<FixedMatrix<N>> covariance(covariance_data);
+  Eigen::Map<const FixedMatrix<N>> noise(noise_data);
+  const FixedVector<N> previous = state;
+  FixedMatrix<N> jacobian;
+  for (int index = 0; index < N; ++index) {
+    const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(previous(index)));
+    FixedVector<N> upper = previous;
+    FixedVector<N> lower = previous;
+    upper(index) += step;
+    lower(index) -= step;
+    jacobian.col(index) = (function(upper, dt) - function(lower, dt)) / (2.0 * step);
+  }
+  state = function(previous, dt);
+  covariance = jacobian * covariance * jacobian.transpose() + dt * noise;
+  covariance = (covariance + covariance.transpose()).eval() * 0.5;
+}
+
+template <int N, int Z>
+void update(double *state_data, double *covariance_data, const double *measurement_data, const double *noise_data,
+            double *innovation_data, const Eigen::Matrix<double, Z, N> &jacobian, const Eigen::Matrix<double, Z, 1> &expected) {
+  Eigen::Map<FixedVector<N>> state(state_data);
+  Eigen::Map<FixedMatrix<N>> covariance(covariance_data);
+  Eigen::Map<const Eigen::Matrix<double, Z, 1>> measurement(measurement_data);
+  Eigen::Map<const Eigen::Matrix<double, Z, Z, Eigen::RowMajor>> noise(noise_data);
+  const Eigen::Matrix<double, Z, Z> innovation_covariance = jacobian * covariance * jacobian.transpose() + noise;
+  const Eigen::Matrix<double, N, Z> gain = innovation_covariance.ldlt().solve(jacobian * covariance).transpose();
+  const Eigen::Matrix<double, Z, 1> innovation = measurement - expected;
+  state += gain * innovation;
+  const FixedMatrix<N> residual = FixedMatrix<N>::Identity() - gain * jacobian;
+  covariance = residual * covariance * residual.transpose() + gain * noise * gain.transpose();
+  covariance = (covariance + covariance.transpose()).eval() * 0.5;
+  Eigen::Map<Eigen::Matrix<double, Z, 1>> innovation_output(innovation_data);
+  innovation_output = innovation;
+}
+
+FixedVector<9> car_transition(const FixedVector<9> &state, double dt, const double *values) {
+  FixedVector<9> result = state;
+  const double stiffness = state(0);
+  const double steer_ratio = state(1);
+  const double angle = state(7) - state(2) - state(3);
+  const double speed = state(4);
+  const double lateral_speed = state(5);
+  const double yaw_rate = state(6);
+  const double mass = values[0];
+  const double inertia = values[1];
+  const double front = values[2];
+  const double rear = values[3];
+  const double front_stiffness = stiffness * values[4];
+  const double rear_stiffness = stiffness * values[5];
+  double 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) - 9.81 * state(8);
+  double yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed);
+  yaw_dot -= (front_stiffness * front * front + rear_stiffness * rear * rear) * 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;
+}
+
+Eigen::Matrix3d rotation(const Eigen::Vector3d &euler) {
+  return (Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) *
+          Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX())).toRotationMatrix();
+}
+
+Eigen::Vector3d euler(const Eigen::Matrix3d &matrix) {
+  const double pitch = std::asin(-matrix(2, 0));
+  return {std::atan2(matrix(2, 1), matrix(2, 2)), pitch, std::atan2(matrix(1, 0), matrix(0, 0))};
+}
+
+Eigen::Matrix<double, 3, 6> pose_orientation_jacobian(const Eigen::Vector3d &orientation,
+                                                      const Eigen::Vector3d &angular_velocity, double dt) {
+  const Eigen::Matrix3d x_rotation = Eigen::AngleAxisd(orientation.x(), Eigen::Vector3d::UnitX()).toRotationMatrix();
+  const Eigen::Matrix3d y_rotation = Eigen::AngleAxisd(orientation.y(), Eigen::Vector3d::UnitY()).toRotationMatrix();
+  const Eigen::Matrix3d z_rotation = Eigen::AngleAxisd(orientation.z(), Eigen::Vector3d::UnitZ()).toRotationMatrix();
+  const Eigen::Vector3d delta = dt * angular_velocity;
+  const Eigen::Matrix3d delta_x = Eigen::AngleAxisd(delta.x(), Eigen::Vector3d::UnitX()).toRotationMatrix();
+  const Eigen::Matrix3d delta_y = Eigen::AngleAxisd(delta.y(), Eigen::Vector3d::UnitY()).toRotationMatrix();
+  const Eigen::Matrix3d delta_z = Eigen::AngleAxisd(delta.z(), Eigen::Vector3d::UnitZ()).toRotationMatrix();
+  const Eigen::Matrix3d first = z_rotation * y_rotation * x_rotation;
+  const Eigen::Matrix3d second = delta_z * delta_y * delta_x;
+  const Eigen::Matrix3d combined = first * second;
+  Eigen::Matrix3d generator_x = Eigen::Matrix3d::Zero();
+  Eigen::Matrix3d generator_y = Eigen::Matrix3d::Zero();
+  Eigen::Matrix3d generator_z = Eigen::Matrix3d::Zero();
+  generator_x(1, 2) = -1.0;
+  generator_x(2, 1) = 1.0;
+  generator_y(0, 2) = 1.0;
+  generator_y(2, 0) = -1.0;
+  generator_z(0, 1) = -1.0;
+  generator_z(1, 0) = 1.0;
+  std::array<Eigen::Matrix3d, 6> derivatives = {
+    z_rotation * y_rotation * x_rotation * generator_x * second,
+    z_rotation * y_rotation * generator_y * x_rotation * second,
+    z_rotation * generator_z * y_rotation * x_rotation * second,
+    first * delta_z * delta_y * delta_x * generator_x * dt,
+    first * delta_z * delta_y * generator_y * delta_x * dt,
+    first * delta_z * generator_z * delta_y * delta_x * dt,
+  };
+  Eigen::Matrix<double, 3, 6> result;
+  for (int index = 0; index < 6; ++index) {
+    const Eigen::Matrix3d &derivative = derivatives[index];
+    result(0, index) = (combined(2, 2) * derivative(2, 1) - combined(2, 1) * derivative(2, 2)) /
+                       (combined(2, 1) * combined(2, 1) + combined(2, 2) * combined(2, 2));
+    result(1, index) = -derivative(2, 0) / std::sqrt(1.0 - combined(2, 0) * combined(2, 0));
+    result(2, index) = (combined(0, 0) * derivative(1, 0) - combined(1, 0) * derivative(0, 0)) /
+                       (combined(1, 0) * combined(1, 0) + combined(0, 0) * combined(0, 0));
+  }
+  return result;
+}
+
+FixedVector<18> pose_transition(const FixedVector<18> &state, double dt) {
+  FixedVector<18> result = state;
+  result.segment<3>(3) += dt * state.segment<3>(12);
+  result.segment<3>(0) = euler(rotation(state.segment<3>(0)) * rotation(dt * state.segment<3>(6)));
+  return result;
+}
+
+Eigen::Vector3d pose_acceleration(const FixedVector<18> &state) {
+  return rotation(state.segment<3>(0)).transpose() * Eigen::Vector3d(0.0, 0.0, -9.81) + state.segment<3>(12) +
+         state.segment<3>(6).cross(state.segment<3>(3)) + state.segment<3>(15);
+}
+
+}
+
+extern "C" void iq_estimator_car_predict(double *state, double *covariance, const double *process_noise, double dt, const double *parameters) {
+  predict<9>(state, covariance, process_noise, dt, [parameters](const FixedVector<9> &value, double step) {
+    return car_transition(value, step, parameters);
+  });
+}
+
+extern "C" void iq_estimator_car_update(double *state_data, double *covariance, int kind, const double *measurement, const double *noise, double *innovation) {
+  Eigen::Map<FixedVector<9>> state(state_data);
+  if (kind == 24) {
+    Eigen::Matrix<double, 2, 9> jacobian = Eigen::Matrix<double, 2, 9>::Zero();
+    jacobian(0, 4) = 1.0;
+    jacobian(1, 5) = 1.0;
+    update<9, 2>(state_data, covariance, measurement, noise, innovation, jacobian, state.segment<2>(4));
+    return;
+  }
+  int index = -1;
+  if (kind == 25) index = 6;
+  if (kind == 30) index = 4;
+  if (kind == 26) index = 7;
+  if (kind == 27) index = 3;
+  if (kind == 29) index = 1;
+  if (kind == 28) index = 0;
+  if (kind == 31) index = 8;
+  if (index < 0) throw std::invalid_argument("unknown car observation");
+  Eigen::Matrix<double, 1, 9> jacobian = Eigen::Matrix<double, 1, 9>::Zero();
+  jacobian(0, index) = 1.0;
+  Eigen::Matrix<double, 1, 1> expected;
+  expected(0) = state(index);
+  update<9, 1>(state_data, covariance, measurement, noise, innovation, jacobian, expected);
+}
+
+extern "C" void iq_estimator_pose_predict(double *state, double *covariance, const double *process_noise, double dt) {
+  Eigen::Map<FixedVector<18>> mapped_state(state);
+  Eigen::Map<FixedMatrix<18>> mapped_covariance(covariance);
+  Eigen::Map<const FixedMatrix<18>> noise(process_noise);
+  const FixedVector<18> previous = mapped_state;
+  FixedMatrix<18> jacobian = FixedMatrix<18>::Identity();
+  const Eigen::Matrix<double, 3, 6> orientation_jacobian = pose_orientation_jacobian(previous.segment<3>(0), previous.segment<3>(6), dt);
+  jacobian.block<3, 3>(0, 0) = orientation_jacobian.leftCols<3>();
+  jacobian.block<3, 3>(0, 6) = orientation_jacobian.rightCols<3>();
+  jacobian.block<3, 3>(3, 12) = Eigen::Matrix3d::Identity() * dt;
+  mapped_state = pose_transition(previous, dt);
+  mapped_covariance = jacobian * mapped_covariance * jacobian.transpose() + dt * noise;
+  mapped_covariance = (mapped_covariance + mapped_covariance.transpose()).eval() * 0.5;
+}
+
+extern "C" void iq_estimator_pose_update(double *state_data, double *covariance, int kind, const double *measurement, const double *noise, double *innovation) {
+  Eigen::Map<FixedVector<18>> state(state_data);
+  Eigen::Matrix<double, 3, 18> jacobian = Eigen::Matrix<double, 3, 18>::Zero();
+  Eigen::Vector3d expected;
+  if (kind == 4) {
+    jacobian.block<3, 3>(0, 6).setIdentity();
+    jacobian.block<3, 3>(0, 9).setIdentity();
+    expected = state.segment<3>(6) + state.segment<3>(9);
+  } else if (kind == 10) {
+    expected = pose_acceleration(state);
+    for (int index = 0; index < 3; ++index) {
+      const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(state(index)));
+      FixedVector<18> upper = state;
+      FixedVector<18> lower = state;
+      upper(index) += step;
+      lower(index) -= step;
+      jacobian.col(index) = (pose_acceleration(upper) - pose_acceleration(lower)) / (2.0 * step);
+    }
+    const Eigen::Vector3d velocity = state.segment<3>(3);
+    const Eigen::Vector3d omega = state.segment<3>(6);
+    jacobian.block<3, 3>(0, 3) << 0.0, -omega.z(), omega.y(), omega.z(), 0.0, -omega.x(), -omega.y(), omega.x(), 0.0;
+    jacobian.block<3, 3>(0, 6) << 0.0, velocity.z(), -velocity.y(), -velocity.z(), 0.0, velocity.x(), velocity.y(), -velocity.x(), 0.0;
+    jacobian.block<3, 3>(0, 12).setIdentity();
+    jacobian.block<3, 3>(0, 15).setIdentity();
+  } else if (kind == 13) {
+    jacobian.block<3, 3>(0, 3).setIdentity();
+    expected = state.segment<3>(3);
+  } else if (kind == 14) {
+    jacobian.block<3, 3>(0, 6).setIdentity();
+    expected = state.segment<3>(6);
+  } else {
+    throw std::invalid_argument("unknown pose observation");
+  }
+  update<18, 3>(state_data, covariance, measurement, noise, innovation, jacobian, expected);
+}

iqpilot/selfdrive/state_estimation/native_kernels.h

@@ -0,0 +1,11 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+*/
+#pragma once
+
+extern "C" {
+void iq_estimator_car_predict(double *state, double *covariance, const double *process_noise, double dt, const double *parameters);
+void iq_estimator_car_update(double *state, double *covariance, int kind, const double *measurement, const double *noise, double *innovation);
+void iq_estimator_pose_predict(double *state, double *covariance, const double *process_noise, double dt);
+void iq_estimator_pose_update(double *state, double *covariance, int kind, const double *measurement, const double *noise, double *innovation);
+}

iqpilot/selfdrive/state_estimation/test_estimator.py

@@ -0,0 +1,92 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+import pytest
+
+from iqpilot.selfdrive.state_estimation import ModelDefinition, StateEstimator
+
+
+def linear_model(process_noise: float = 0.2, observation_noise: float = 0.5) -> ModelDefinition:
+  return ModelDefinition(
+    state_size=2,
+    error_size=2,
+    transition=lambda state, dt, _: np.array([state[0] + dt * state[1], state[1]]),
+    measurements={1: lambda state, _: state[:1]},
+    process_noise=np.eye(2) * process_noise,
+    observation_noise={1: np.array([[observation_noise]])},
+  )
+
+
+def test_linear_prediction_matches_closed_form() -> None:
+  estimator = StateEstimator(linear_model(), np.array([2.0, 3.0]), np.diag([4.0, 5.0]))
+  estimator.init_state(np.array([2.0, 3.0]), np.diag([4.0, 5.0]), 1.0)
+  estimator.predict(1.25)
+  transition = np.array([[1.0, 0.25], [0.0, 1.0]])
+  np.testing.assert_allclose(estimator.x, np.array([2.75, 3.0]), atol=1e-10)
+  np.testing.assert_allclose(estimator.P, transition @ np.diag([4.0, 5.0]) @ transition.T + 0.25 * np.eye(2) * 0.2, atol=1e-10)
+
+
+def test_linear_update_matches_closed_form() -> None:
+  estimator = StateEstimator(linear_model(), np.array([0.0, 0.0]), np.diag([2.0, 3.0]))
+  estimator.init_state(np.array([0.0, 0.0]), np.diag([2.0, 3.0]), 0.0)
+  estimator.predict_and_observe(0.0, 1, np.array([4.0]))
+  gain = 2.0 / 2.5
+  np.testing.assert_allclose(estimator.x, np.array([gain * 4.0, 0.0]), atol=1e-10)
+  np.testing.assert_allclose(estimator.P, np.diag([(1.0 - gain) * 2.0, 3.0]), atol=1e-10)
+
+
+def test_zero_innovation_does_not_change_state() -> None:
+  estimator = StateEstimator(linear_model(), np.array([4.0, 2.0]), np.eye(2))
+  estimator.init_state(np.array([4.0, 2.0]), np.eye(2), 0.0)
+  estimator.predict_and_observe(0.0, 1, np.array([4.0]))
+  np.testing.assert_array_equal(estimator.x, np.array([4.0, 2.0]))
+
+
+def test_larger_observation_noise_reduces_correction() -> None:
+  low = StateEstimator(linear_model(observation_noise=0.1), np.zeros(2), np.eye(2))
+  high = StateEstimator(linear_model(observation_noise=10.0), np.zeros(2), np.eye(2))
+  low.predict_and_observe(0.0, 1, np.array([1.0]))
+  high.predict_and_observe(0.0, 1, np.array([1.0]))
+  assert abs(low.x[0]) > abs(high.x[0])
+
+
+def test_larger_process_noise_increases_uncertainty() -> None:
+  low = StateEstimator(linear_model(process_noise=0.1), np.zeros(2), np.eye(2))
+  high = StateEstimator(linear_model(process_noise=2.0), np.zeros(2), np.eye(2))
+  low.init_state(np.zeros(2), np.eye(2), 0.0)
+  high.init_state(np.zeros(2), np.eye(2), 0.0)
+  low.predict(1.0)
+  high.predict(1.0)
+  assert np.all(np.diag(high.P) > np.diag(low.P))
+
+
+def test_batch_update_preserves_covariance_properties() -> None:
+  estimator = StateEstimator(linear_model(), np.zeros(2), np.eye(2))
+  estimator.predict_and_observe(0.0, 1, np.array([[1.0], [0.5], [-0.2]]))
+  np.testing.assert_allclose(estimator.P, estimator.P.T, atol=1e-12)
+  assert np.linalg.eigvalsh(estimator.P).min() >= -1e-10
+  assert np.isfinite(estimator.x).all()
+  assert np.isfinite(estimator.P).all()
+
+
+def test_delayed_observation_replays_deterministically() -> None:
+  chronological = StateEstimator(linear_model(), np.zeros(2), np.eye(2), max_rewind_age=2.0)
+  delayed = StateEstimator(linear_model(), np.zeros(2), np.eye(2), max_rewind_age=2.0)
+  chronological.init_state(np.zeros(2), np.eye(2), 0.0)
+  delayed.init_state(np.zeros(2), np.eye(2), 0.0)
+  chronological.predict_and_observe(0.5, 1, np.array([1.0]))
+  chronological.predict_and_observe(1.0, 1, np.array([2.0]))
+  delayed.predict_and_observe(1.0, 1, np.array([2.0]))
+  delayed.predict_and_observe(0.5, 1, np.array([1.0]))
+  np.testing.assert_allclose(delayed.x, chronological.x, atol=1e-10)
+  np.testing.assert_allclose(delayed.P, chronological.P, atol=1e-10)
+
+
+def test_invalid_dimensions_fail_deterministically() -> None:
+  with pytest.raises(ValueError, match="state dimension mismatch"):
+    StateEstimator(linear_model(), np.zeros(3), np.eye(2))
+  estimator = StateEstimator(linear_model(), np.zeros(2), np.eye(2))
+  with pytest.raises(ValueError, match="measurement dimension mismatch"):
+    estimator.predict_and_observe(0.0, 1, np.zeros(2))

iqpilot/selfdrive/state_estimation/test_models.py

@@ -0,0 +1,58 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+
+from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, States as CarStates
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States as PoseStates
+
+
+def configured_car() -> CarKalman:
+  estimator = CarKalman()
+  estimator.set_globals(1800.0, 2500.0, 1.2, 1.6, 90000.0, 100000.0)
+  estimator.init_state(CarKalman.initial_x, CarKalman.P_initial, 0.0)
+  return estimator
+
+
+def test_car_mutable_parameters_affect_prediction() -> None:
+  light = configured_car()
+  heavy = configured_car()
+  heavy.set_globals(3600.0, 5000.0, 1.2, 1.6, 90000.0, 100000.0)
+  state = CarKalman.initial_x.copy()
+  state[CarStates.STEER_ANGLE] = 0.1
+  light.init_state(state, CarKalman.P_initial, 0.0)
+  heavy.init_state(state, CarKalman.P_initial, 0.0)
+  light.predict(0.01)
+  heavy.predict(0.01)
+  assert abs(light.x[CarStates.YAW_RATE].item()) > abs(heavy.x[CarStates.YAW_RATE].item())
+
+
+def test_car_long_sequence_stays_finite() -> None:
+  estimator = configured_car()
+  for index in range(500):
+    time = index * 0.01
+    estimator.predict_and_observe(time, ObservationKind.STEER_ANGLE, np.array([0.02 * np.sin(time)]))
+    estimator.predict_and_observe(time, ObservationKind.ROAD_FRAME_X_SPEED, np.array([15.0]))
+  assert np.isfinite(estimator.x).all()
+  assert np.isfinite(estimator.P).all()
+  assert np.linalg.eigvalsh(estimator.P).min() >= -1e-10
+
+
+def test_pose_delayed_sensor_sequence_is_stable() -> None:
+  estimator = PoseKalman(0.8)
+  estimator.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0)
+  estimator.predict_and_observe(0.02, ObservationKind.PHONE_GYRO, np.array([0.01, -0.02, 0.03]))
+  estimator.predict_and_observe(0.04, ObservationKind.PHONE_ACCEL, np.array([0.0, 0.0, -9.81]))
+  estimator.predict_and_observe(0.03, ObservationKind.CAMERA_ODO_ROTATION, np.array([0.01, -0.02, 0.03]))
+  assert np.isfinite(estimator.x).all()
+  assert np.isfinite(estimator.P).all()
+  np.testing.assert_allclose(estimator.P, estimator.P.T, atol=1e-12)
+
+
+def test_pose_zero_rotation_preserves_orientation() -> None:
+  estimator = PoseKalman(0.8)
+  estimator.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0)
+  estimator.predict(1.0)
+  np.testing.assert_allclose(estimator.x[PoseStates.NED_ORIENTATION], np.zeros(3), atol=1e-12)

iqpilot/system/ble-transportd.service

@@ -5,8 +5,10 @@ Description=Konn3kt BLE Device Settings Transport
 Documentation=https://gitlvb.teallvbs.xyz/teal/iqpilot
 After=bluetooth.service dbus.service
 Wants=bluetooth.service dbus.service
-StartLimitIntervalSec=300
-StartLimitBurst=10
+# Never give up: /data/openpilot is a symlink created by the boot-time rename
+# migration, so early starts fail "failed to locate repo root". With a burst
+# limit those failures permanently kill BLE for the whole boot.
+StartLimitIntervalSec=0
 
 [Service]
 Type=simple
@@ -18,6 +20,7 @@ Environment="PYTHONSAFEPATH=1"
 Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin"
 WorkingDirectory=/data/openpilot
 ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1'
+ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1'
 ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi'
 ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.hephaestus.ble_transportd --daemon-name ble_transportd
 TimeoutStartSec=600

iqpilot/system/flockd.service

@@ -5,8 +5,7 @@ Description=Konn3kt Flock/ALPR RF Detector
 Documentation=https://gitlvb.teallvbs.xyz/teal/iqpilot
 After=bluetooth.service dbus.service NetworkManager.service
 Wants=bluetooth.service dbus.service
-StartLimitIntervalSec=300
-StartLimitBurst=10
+StartLimitIntervalSec=0
 
 [Service]
 Type=simple
@@ -18,6 +17,7 @@ Environment="PYTHONSAFEPATH=1"
 Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin"
 WorkingDirectory=/data/openpilot
 ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1'
+ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1'
 ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi'
 ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.flockd.flockd --daemon-name flockd
 TimeoutStartSec=600

iqpilot/system/hephaestusd.service

@@ -16,6 +16,7 @@ Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin"
 WorkingDirectory=/data/openpilot
 
 ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1'
+ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1'
 ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi'
 ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.hephaestus.manage_hephaestusd --daemon-name manage_hephaestusd
 

iqpilot/system/proprietary_runtime/rootfs_integrity.json

@@ -39,15 +39,15 @@
       "mode": 420,
       "path": "/usr/lib/systemd/system/hephaestusd.service",
       "relative_to": "absolute",
-      "sha256": "4ae9a2ad76345019633c2fcf9698f48089576c9b7c009078a732e2a7c36006e4",
-      "size": 1418
+      "sha256": "3b70e8873c7eb813e70798300b76c153dddda9947c1301cbba7e103962461fa2",
+      "size": 1627
     },
     {
       "mode": 420,
       "path": "/usr/lib/systemd/system/ble-transportd.service",
       "relative_to": "absolute",
-      "sha256": "dfb9e18320e264c847efc01a2d58ee1850a83c49ed614527eafa7634de4ac51a",
-      "size": 1500
+      "sha256": "6b5bf61ebfebffe3d125bb766c13c391b83e8feda04b89f47de2725c6fe186e3",
+      "size": 1907
     }
   ]
 }

iqpilot/system/proprietary_runtime/rootfs_integrity.json.sig

@@ -1 +1 @@
-dmb/R9bZCCHK8J72JIgm8TMY7nBVVo1exmvFIodw0LSFVcGZGDrZNB74xhQTAXgzAgeYVdFQyXs0qdwpvQa2Aw==
+exS3jBbVETgFQtzbRGWymtflNgeWLbjHvcMVsKs5NrwpmMdx9sWpcZ2FO/pB6RQaCniXGDwT7GhjvGxpfrP5Cg==

iqpilot/system/runtime_package_sources.py

@@ -4,7 +4,7 @@ import sys
 from pathlib import Path
 
 
-PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "rednose", "teleoprtc", "tinygrad")
+PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "teleoprtc", "tinygrad")
 root = Path(sys.argv[1]).resolve()
 text = (root / "pyproject.toml").read_text()
 for name in PACKAGE_NAMES:

iqpilot/system/runtime_packages_verify.py

@@ -11,7 +11,7 @@ import sys
 from importlib import metadata
 from pathlib import Path
 
-PACKAGES = ("iqdbc", "msgq", "panda", "rednose", "teleoprtc", "tinygrad")
+PACKAGES = ("iqdbc", "msgq", "panda", "teleoprtc", "tinygrad")
 
 missing = []
 for name in PACKAGES:

iqpilot/system/version.py

@@ -33,8 +33,13 @@ def get_version(path: str = BASEDIR) -> str:
 
 
 def get_release_notes(path: str = BASEDIR) -> str:
-  with open(os.path.join(path, "iqpilot", "docs", "CHANGELOG.md")) as f:
-    return f.read().split('\n\n', 1)[0]
+  for rel in (("iqpilot", "docs", "CHANGELOG.md"), ("docs", "CHANGELOG.md")):
+    try:
+      with open(os.path.join(path, *rel)) as f:
+        return f.read().split('\n\n', 1)[0]
+    except OSError:
+      continue
+  return ""
 
 
 @cache