IQ.Pilot Release Commit @ cd83f5a

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)