sensorfusion-tools 2026.0.0__tar.gz

sensorfusion_tools-2026.0.0/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2026 Gustaf Hendeby
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

sensorfusion_tools-2026.0.0/PKG-INFO

@@ -0,0 +1,119 @@
+Metadata-Version: 2.4
+Name: sensorfusion-tools
+Version: 2026.0.0
+Summary: Python helper library for the TSRT14 Sensor Fusion Course at LiU
+Author: Gustaf Hendeby
+Author-email: Gustaf Hendeby <hendeby@gmail.com>
+License-Expression: MIT
+License-File: LICENSE
+Classifier: Intended Audience :: Education
+Classifier: Intended Audience :: Science/Research
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.14
+Classifier: Topic :: Scientific/Engineering
+Requires-Dist: matplotlib>=3.10.8
+Requires-Dist: numpy>=2.4.4
+Requires-Dist: scipy>=1.17.1
+Requires-Dist: tqdm>=4.67.3
+Requires-Python: >=3.14
+Description-Content-Type: text/markdown
+
+# sensorfusion-tools
+
+Signal-systems utilities for estimation, modeling, and plotting.
+
+## Versioning
+
+This library uses calendar-style semantic versions: `YYYY.FEATURE.BUGFIX`.
+
+- `YYYY`: release year (for long-term timeline tracking)
+- `FEATURE`: increments for new functionality
+- `BUGFIX`: increments for patches and fixes
+
+## Framework And Tooling
+
+- Language: Python 3.14+
+- Build backend: `uv_build`
+- Package/dependency workflow: `uv`
+- Numerical stack: `numpy`, `scipy`
+- Plotting: `matplotlib`
+
+## Testing Framework
+
+- Test runner: `pytest`
+- Property-based testing: `hypothesis`
+- Coverage: `pytest-cov` (configured for `src/sftools`)
+
+Run tests:
+
+```bash
+uv run pytest
+```
+
+Run only sftools tests:
+
+```bash
+uv run pytest tests/sftools
+```
+
+## Code Quality
+
+- Linting: `ruff`
+
+Run lint:
+
+```bash
+uv run ruff check .
+```
+
+## CI (GitLab)
+
+The pipeline is defined in [.gitlab-ci.yml](.gitlab-ci.yml) and includes:
+
+- `quality` stage: `ruff` lint checks
+- `test` stage: `pytest` with JUnit artifact output
+
+CI uses `python:3.14-slim` and installs dependencies with:
+
+```bash
+uv sync --group dev
+```
+
+## License
+
+Distributed under the MIT License. See [LICENSE](LICENSE) for details.
+
+## Project Structure
+
+```text
+src/
+	sftools/        # Main signal-system models, estimators, helpers, plotting, shared types
+
+tests/
+	sftools/          # Pytest suite for src/sftools (one test file per source file)
+	fixtures/         # Regression test input/output fixtures
+```
+
+For sftools, tests mirror the source layout:
+
+- `src/sftools/model_obs.py` -> `tests/sftools/test_model.py`
+- `src/sftools/estimator.py` -> `tests/sftools/test_estimator.py`
+- `src/sftools/helper.py` -> `tests/sftools/test_helper.py`
+- `src/sftools/plot.py` -> `tests/sftools/test_plot.py`
+
+Shared state/trajectory datatypes are defined in `src/sftools/types.py`:
+
+- `GaussianState`
+- `ParticleState`
+- `StateTrajectory` (supports probabilistic trajectories via `write_state` and deterministic trajectories via `write_mean`)
+
+Useful `StateTrajectory` iteration helpers:
+
+- `for y in traj` yields state columns with shape `(n, 1)`
+- `traj.iter_samples(include_cov=True, include_time=True)` yields `(y, cov, t)` tuples when available
+
+Plotting helpers in `src/sftools/plot.py` use `StateTrajectory` inputs.
+
+Particle filter helper:
+
+- `ParticleFilter.init_state_gaussian(mean, cov, n_particles)` initializes a `ParticleState` from a Gaussian prior using the filter RNG.

sensorfusion_tools-2026.0.0/README.md

@@ -0,0 +1,99 @@
+# sensorfusion-tools
+
+Signal-systems utilities for estimation, modeling, and plotting.
+
+## Versioning
+
+This library uses calendar-style semantic versions: `YYYY.FEATURE.BUGFIX`.
+
+- `YYYY`: release year (for long-term timeline tracking)
+- `FEATURE`: increments for new functionality
+- `BUGFIX`: increments for patches and fixes
+
+## Framework And Tooling
+
+- Language: Python 3.14+
+- Build backend: `uv_build`
+- Package/dependency workflow: `uv`
+- Numerical stack: `numpy`, `scipy`
+- Plotting: `matplotlib`
+
+## Testing Framework
+
+- Test runner: `pytest`
+- Property-based testing: `hypothesis`
+- Coverage: `pytest-cov` (configured for `src/sftools`)
+
+Run tests:
+
+```bash
+uv run pytest
+```
+
+Run only sftools tests:
+
+```bash
+uv run pytest tests/sftools
+```
+
+## Code Quality
+
+- Linting: `ruff`
+
+Run lint:
+
+```bash
+uv run ruff check .
+```
+
+## CI (GitLab)
+
+The pipeline is defined in [.gitlab-ci.yml](.gitlab-ci.yml) and includes:
+
+- `quality` stage: `ruff` lint checks
+- `test` stage: `pytest` with JUnit artifact output
+
+CI uses `python:3.14-slim` and installs dependencies with:
+
+```bash
+uv sync --group dev
+```
+
+## License
+
+Distributed under the MIT License. See [LICENSE](LICENSE) for details.
+
+## Project Structure
+
+```text
+src/
+	sftools/        # Main signal-system models, estimators, helpers, plotting, shared types
+
+tests/
+	sftools/          # Pytest suite for src/sftools (one test file per source file)
+	fixtures/         # Regression test input/output fixtures
+```
+
+For sftools, tests mirror the source layout:
+
+- `src/sftools/model_obs.py` -> `tests/sftools/test_model.py`
+- `src/sftools/estimator.py` -> `tests/sftools/test_estimator.py`
+- `src/sftools/helper.py` -> `tests/sftools/test_helper.py`
+- `src/sftools/plot.py` -> `tests/sftools/test_plot.py`
+
+Shared state/trajectory datatypes are defined in `src/sftools/types.py`:
+
+- `GaussianState`
+- `ParticleState`
+- `StateTrajectory` (supports probabilistic trajectories via `write_state` and deterministic trajectories via `write_mean`)
+
+Useful `StateTrajectory` iteration helpers:
+
+- `for y in traj` yields state columns with shape `(n, 1)`
+- `traj.iter_samples(include_cov=True, include_time=True)` yields `(y, cov, t)` tuples when available
+
+Plotting helpers in `src/sftools/plot.py` use `StateTrajectory` inputs.
+
+Particle filter helper:
+
+- `ParticleFilter.init_state_gaussian(mean, cov, n_particles)` initializes a `ParticleState` from a Gaussian prior using the filter RNG.

sensorfusion_tools-2026.0.0/pyproject.toml

@@ -0,0 +1,60 @@
+[project]
+name = "sensorfusion-tools"
+version = "2026.0.0"
+description = "Python helper library for the TSRT14 Sensor Fusion Course at LiU"
+readme = "README.md"
+license = "MIT"
+license-files = ["LICENSE"]
+authors = [
+    { name = "Gustaf Hendeby", email = "hendeby@gmail.com" },
+]
+classifiers = [
+    "Intended Audience :: Education",
+    "Intended Audience :: Science/Research",
+    "Programming Language :: Python :: 3",
+    "Programming Language :: Python :: 3.14",
+    "Topic :: Scientific/Engineering",
+]
+requires-python = ">=3.14"
+dependencies = [
+    "matplotlib>=3.10.8",
+    "numpy>=2.4.4",
+    "scipy>=1.17.1",
+    "tqdm>=4.67.3",
+]
+
+[dependency-groups]
+dev = [
+    "hypothesis>=6.147.0",
+    "pre-commit>=4.5.1",
+    "pyqt5>=5.15.11",
+    "pytest>=9.0.1",
+    "pytest-cov>=7.1.0",
+    "ruff>=0.14.0",
+    "scriptweave>=2026.0.0",
+]
+
+[build-system]
+requires = ["uv_build>=0.9"]
+build-backend = "uv_build"
+
+[tool.uv.build-backend]
+module-name = "sftools"
+
+[tool.pytest.ini_options]
+addopts = "-ra --strict-markers --cov=src/sftools --cov-report=term-missing"
+testpaths = ["tests"]
+
+[tool.coverage.run]
+source = ["src/sftools"]
+branch = true
+
+[tool.coverage.report]
+show_missing = true
+skip_covered = false
+
+[[tool.uv.index]]
+name = "testpypi"
+url = "https://test.pypi.org/simple/"
+publish-url = "https://test.pypi.org/legacy/"
+explicit = true

sensorfusion_tools-2026.0.0/src/sftools/__init__.py

@@ -0,0 +1,194 @@
+"""Entry points for models, estimators, and shared types.
+
+Most users should import from this package root, for example:
+
+- ``create_dynamics(...)`` and ``create_observation(...)`` to build models
+- ``StateTrajectory`` to store time-series state data
+- ``GaussianState`` and ``ParticleState`` for filter states
+
+Model classes are registered via decorators in ``model_dyn`` and ``model_obs``.
+"""
+
+from importlib.metadata import PackageNotFoundError
+from importlib.metadata import version as _pkg_version
+from typing import Any, Callable, ClassVar, Protocol, Self
+
+import numpy as np
+import numpy.typing as npt
+
+from sftools.types import GaussianState, ParticleState, StateLike, StateTrajectory
+
+try:
+  __version__ = _pkg_version("sensorfusion-tools")
+except PackageNotFoundError:
+  # Fallback when package metadata is unavailable (for example, direct source use).
+  __version__ = "0+unknown"
+
+
+class ObservationModel(Protocol):
+  """Protocol for measurement models used by estimators.
+
+  Implementations must provide deterministic measurement evaluation (``h``),
+  Jacobian evaluation (``H``), and measurement sampling (``random``).
+
+  State input ``x`` may be provided either as a single state vector ``(n,)`` /
+  ``(n, 1)`` or as batched columns ``(n, N)`` for models that support batched
+  evaluation.
+  """
+
+  R: npt.NDArray
+
+  def h(self: Self, x: npt.NDArray) -> npt.NDArray: ...
+  def H(self: Self, x: npt.NDArray) -> npt.NDArray: ...
+  def random(
+    self: Self,
+    x: npt.NDArray,
+    n: int = 1,
+    rng: np.random.Generator | None = None,
+  ) -> npt.NDArray: ...
+
+
+class DynamicModel(Protocol):
+  """Protocol for state transition models used by estimators.
+
+  Implementations must provide deterministic propagation (``f``), Jacobian
+  evaluation (``F``), process-noise gain (``G``), and stochastic propagation
+  (``random``).
+  """
+
+  Q: npt.NDArray
+
+  def f(self: Self, x: npt.NDArray) -> npt.NDArray: ...
+  def F(self: Self, x: npt.NDArray) -> npt.NDArray: ...
+  def G(self: Self, x: npt.NDArray) -> npt.NDArray: ...
+  def random(
+    self: Self,
+    n: int = 1,
+    x: npt.NDArray | None = None,
+    rng: np.random.Generator | None = None,
+  ) -> npt.NDArray: ...
+
+
+class ModelFactory:
+  """Registry-based factory for observation and dynamic model classes.
+
+  The factory powers the main helper functions:
+
+  - ``create_observation(name, ...)``
+  - ``create_dynamics(name, ...)``
+
+  and supports case-insensitive model names.
+  """
+
+  _registry: ClassVar[dict[str, dict[str, type]]] = {"obs": {}, "dyn": {}}
+
+  @classmethod
+  def register(cls: type[Self], cls_type: str, name: str) -> Callable[..., type]:
+    """Return decorator that registers a model class under `name`."""
+    cls_type = cls_type.lower()
+
+    if cls_type not in cls._registry:
+      msg = f"Unknown model type '{cls_type}'"
+      raise ValueError(msg)
+
+    def _decorator(model_cls: type) -> type:
+      key = name.lower()
+      if (
+        key in cls._registry[cls_type] and cls._registry[cls_type][key] is not model_cls
+      ):
+        msg = f"{cls_type} model '{name}' is already registered"
+        raise ValueError(msg)
+      cls._registry[cls_type][key] = model_cls
+      return model_cls
+
+    return _decorator
+
+  @classmethod
+  def create(
+    cls: type[Self], cls_type: str, name: str, *args: Any, **kwargs: Any
+  ) -> Any:
+    """Instantiate a registered model by kind and name."""
+    key = name.lower()
+
+    if cls_type is not None:
+      cls_type = cls_type.lower()
+      if cls_type not in cls._registry:
+        msg = f"Unknown model type '{cls_type}'"
+        raise KeyError(msg)
+      if key not in cls._registry[cls_type]:
+        available = ", ".join(sorted(cls._registry[cls_type]))
+        msg = f"Unknown {cls_type} model '{name}'. Available models: {available}"
+        raise KeyError(msg)
+      return cls._registry[cls_type][key](*args, **kwargs)
+
+    available = set()
+    for k in ("obs", "dyn"):
+      available.update(cls._registry[k].keys())
+    available = ", ".join(sorted(available))
+    msg = f"Unknown model '{name}'. Available models: {available}"
+    raise KeyError(msg)
+
+  @classmethod
+  def registered(cls: type[Self], cls_type: str | None = None) -> tuple[str, ...]:
+    """Return sorted registered model names for a kind or globally."""
+    if cls_type is not None:
+      cls_type = cls_type.lower()
+      if cls_type not in cls._registry:
+        msg = f"Unknown model kind '{cls_type}'"
+        raise KeyError(msg)
+      return tuple(sorted(cls._registry[cls_type].keys()))
+    names: set[str] = set()
+    for v in cls._registry.values():
+      names.update(v.keys())
+    return tuple(sorted(names))
+
+
+def register_observation(name: str):
+  """Decorator helper to register an observation model class.
+
+  Example:
+    ``@register_observation("toa")``
+  """
+  return ModelFactory.register("obs", name)
+
+
+def create_observation(name: str, *args: Any, **kwargs: Any) -> ObservationModel:
+  """Create a registered observation model instance by name."""
+  return ModelFactory.create("obs", name, *args, **kwargs)
+
+
+def register_dynamics(name: str):
+  """Decorator helper to register a dynamics model class.
+
+  Example:
+    ``@register_dynamics("cv")``
+  """
+  return ModelFactory.register("dyn", name)
+
+
+def create_dynamics(name: str, *args: Any, **kwargs: Any) -> DynamicModel:
+  """Create a registered dynamics model instance by name."""
+  return ModelFactory.create("dyn", name, *args, **kwargs)
+
+
+__all__ = [
+  "DynamicModel",
+  "GaussianState",
+  "ObservationModel",
+  "ParticleState",
+  "StateLike",
+  "StateTrajectory",
+  "__version__",
+  "create_dynamics",
+  "create_observation",
+  "helper",
+  "model_dyn",
+  "model_obs",
+  "register_dynamics",
+  "register_observation",
+]
+
+# Import model modules for side-effect registration via decorators.
+import sftools.helper as helper  # noqa: E402
+import sftools.model_dyn as model_dyn  # noqa: E402
+import sftools.model_obs as model_obs  # noqa: E402

sensorfusion_tools-2026.0.0/src/sftools/estimator.py

@@ -0,0 +1,195 @@
+from dataclasses import dataclass, field
+from typing import Self
+
+import numpy as np
+import numpy.typing as npt
+from scipy.optimize import least_squares
+from scipy.special import logsumexp
+
+from sftools import (
+  DynamicModel,
+  ObservationModel,
+)
+from sftools.helper import ut
+from sftools.types import GaussianState, ParticleState
+
+
+def ls(
+  model: ObservationModel, y: npt.NDArray, x0: npt.NDArray | None = None
+) -> GaussianState:
+  """Compute linear least-squares state estimate and covariance.
+
+  Args:
+    model: Observation model providing linearized measurement Jacobian via `H`.
+    y: Measurement column vector.
+    x0: Optional linearization point forwarded to `model.H`.
+
+  Returns:
+    GaussianState with estimate and covariance approximation.
+  """
+  H = model.H(x0)
+  x_hat = np.linalg.lstsq(H, y, rcond=None)[0]
+  P_ls = np.linalg.inv(H.T @ np.linalg.lstsq(model.R, H, rcond=None)[0])
+  return GaussianState(x_hat, P_ls)
+
+
+def wls(
+  model: ObservationModel, y: npt.NDArray, x0: npt.NDArray | None = None
+) -> GaussianState:
+  """Compute weighted least-squares estimate using measurement covariance."""
+  H = model.H(x0)
+  L = np.linalg.cholesky(model.R)
+  y_ = np.linalg.solve(L, y)
+  H_ = np.linalg.solve(L, H)
+  x_hat = np.linalg.lstsq(H_, y_, rcond=None)[0]
+  P_wls = np.linalg.inv(H_.T @ H_)
+  return GaussianState(x_hat, P_wls)
+
+
+def nls(model: ObservationModel, y: npt.NDArray, x0: npt.NDArray) -> GaussianState:
+  """Solve nonlinear least-squares by Levenberg-Marquardt with analytic Jacobian.
+
+  Returns the converged state estimate and a local covariance approximation.
+  """
+  L = np.linalg.cholesky(model.R)
+
+  def eps(x_flat: npt.NDArray) -> npt.NDArray:
+    x = x_flat[:, None]
+    return np.linalg.solve(L, y - model.h(x)).ravel()
+
+  def d_eps(x_flat: npt.NDArray) -> npt.NDArray:
+    x = x_flat[:, None]
+    return -np.linalg.solve(L, model.H(x))
+
+  result = least_squares(eps, x0=x0.ravel(), jac=d_eps, method="lm")
+  H = model.H(result.x[:, None])
+  P_nls = np.linalg.inv(H.T @ np.linalg.solve(model.R, H))
+  return GaussianState(result.x[:, None], P_nls)
+
+
+@dataclass
+class KalmanFilter:
+  """Classic Kalman filter with explicit prediction and correction steps."""
+
+  dyn_model: DynamicModel
+
+  def time_update(self: Self, state: GaussianState) -> GaussianState:
+    """Propagate Gaussian belief through the dynamic model."""
+    xk, Pk = state.mean(), state.P
+    return GaussianState(
+      self.dyn_model.f(xk),
+      self.dyn_model.F(xk) @ Pk @ self.dyn_model.F(xk).T
+      + self.dyn_model.G(xk) @ self.dyn_model.Q @ self.dyn_model.G(xk).T,
+    )
+
+  def meas_update(
+    self: Self,
+    state: GaussianState,
+    obs_model: ObservationModel,
+    y: npt.NDArray[np.float64],
+  ) -> GaussianState:
+    """Condition Gaussian belief on one measurement."""
+    xk, Pk = state.mean(), state.P
+    S = obs_model.H(xk) @ Pk @ obs_model.H(xk).T + obs_model.R
+    K = np.linalg.solve(S, (Pk @ obs_model.H(xk).T).T).T
+    ImKH = np.eye(len(xk)) - K @ obs_model.H(xk)
+    return GaussianState(
+      xk + K @ (y - obs_model.h(xk)), ImKH @ Pk @ ImKH.T + K @ obs_model.R @ K.T
+    )
+
+
+@dataclass
+class UnscentedKalmanFilter:
+  """Unscented Kalman filter using the unscented transform for propagation."""
+
+  dyn_model: DynamicModel
+
+  def time_update(self: Self, state: GaussianState) -> GaussianState:
+    """Propagate Gaussian belief using sigma-point transformation."""
+    Xhat = ut(state, self.dyn_model.f)
+    G = self.dyn_model.G(state.mean())
+    return GaussianState(
+      Xhat.x,
+      Xhat.P + G @ self.dyn_model.Q @ G.T,
+    )
+
+  def meas_update(
+    self: Self,
+    state: GaussianState,
+    obs_model: ObservationModel,
+    y: npt.NDArray[np.float64],
+  ) -> GaussianState:
+    """Apply measurement update using augmented-state unscented transform."""
+    xk, Pk = state.mean(), state.P
+    n_x = xk.shape[0]
+
+    def fun(x):
+      return np.vstack([x[:n_x], obs_model.h(x[:n_x]) + x[n_x:]])
+
+    x = np.vstack((xk, np.zeros((obs_model.R.shape[0], 1))))
+    P = np.block(
+      [
+        [Pk, np.zeros((Pk.shape[0], obs_model.R.shape[1]))],
+        [np.zeros((obs_model.R.shape[0], Pk.shape[1])), obs_model.R],
+      ]
+    )
+    Xhat = ut(GaussianState(x, P), fun)
+    x_aug = Xhat.x
+    P_aug = Xhat.P
+    xk_hat, yhat = x_aug[:n_x], x_aug[n_x:]
+    Pxy = P_aug[:n_x, n_x:]
+    S = P_aug[n_x:, n_x:]
+    K = np.linalg.solve(S, Pxy.T).T
+    innov = y - yhat[:, None]
+    return GaussianState(xk_hat + (K @ innov).ravel(), Pk - K @ S @ K.T)
+
+
+@dataclass
+class ParticleFilter:
+  """Bootstrap particle filter with log-weights and multinomial resampling."""
+
+  dyn_model: DynamicModel
+  rng: np.random.Generator = field(default_factory=lambda: np.random.default_rng(0))
+
+  def init_state_gaussian(
+    self: Self,
+    mean: npt.NDArray[np.float64],
+    cov: npt.NDArray[np.float64],
+    n_particles: int,
+  ) -> ParticleState:
+    """Initialize particles from a Gaussian prior with uniform weights."""
+    if n_particles <= 0:
+      msg = "n_particles must be positive"
+      raise ValueError(msg)
+
+    mu = np.asarray(mean, dtype=np.float64).reshape(-1)
+    P = np.asarray(cov, dtype=np.float64)
+    particles = self.rng.multivariate_normal(mean=mu, cov=P, size=n_particles).T
+    logw = np.full(n_particles, -np.log(n_particles), dtype=np.float64)
+    return ParticleState(particles, logw)
+
+  def time_update(self: Self, state: ParticleState) -> ParticleState:
+    """Sample propagated particles from the transition model."""
+    return ParticleState(
+      self.dyn_model.random(x=state.x, n=state.x.shape[1], rng=self.rng), state.logw
+    )
+
+  def meas_update(
+    self: Self,
+    state: ParticleState,
+    obs_model: ObservationModel,
+    y: npt.NDArray[np.float64],
+  ) -> ParticleState:
+    """Update particle log-weights from Gaussian measurement likelihood."""
+    xk, wk = state.x, state.logw
+    L = np.linalg.cholesky(obs_model.R)
+    eps = np.linalg.solve(L, y - obs_model.h(xk))
+    log_weights = wk + -0.5 * np.sum(eps**2, axis=0)
+    log_weights -= logsumexp(log_weights)
+    return ParticleState(xk, log_weights)
+
+  def resample(self: Self, state: ParticleState) -> ParticleState:
+    """Resample particles according to normalized weights."""
+    N = len(state.logw)
+    indices = self.rng.choice(N, size=N, replace=True, p=np.exp(state.logw))
+    return ParticleState(state.x[:, indices], -np.log(N) * np.ones(N))