Prebuilt Filters

.github/workflows/ci.yml

@@ -0,0 +1,67 @@
+name: CI
+
+on:
+  push:
+    branches: [master, develop]
+  pull_request:
+
+jobs:
+  test:
+    name: Tests (${{ matrix.os }})
+    strategy:
+      fail-fast: false
+      matrix:
+        os: [ubuntu-latest, macos-latest]
+    runs-on: ${{ matrix.os }}
+    steps:
+      - uses: actions/checkout@v4
+      - uses: cachix/install-nix-action@v31
+
+      - name: Rust tests
+        run: nix develop .#ci --command cargo test
+
+      - name: Rust examples
+        run: |
+          nix develop .#ci --command bash -euo pipefail -c '
+            for f in examples/*.rs; do
+              e=$(basename "$f" .rs)
+              echo "== $e"
+              cargo run --quiet --example "$e"
+            done'
+
+      - name: Build Python bindings
+        run: |
+          nix develop .#ci --command bash -euo pipefail -c '
+            python -m venv --system-site-packages .venv
+            . .venv/bin/activate
+            maturin develop --release'
+
+      - name: Python tests
+        run: nix develop .#ci --command bash -euo pipefail -c '. .venv/bin/activate && python -m pytest tests_py'
+
+      - name: Python examples
+        run: |
+          nix develop .#ci --command bash -euo pipefail -c '
+            . .venv/bin/activate
+            cd examples
+            for f in *.py; do
+              # renders a video and needs manim, which the CI shell leaves out
+              [ "$f" = manim_two_mass_spring_damper.py ] && continue
+              echo "== $f"
+              python "$f"
+            done'
+
+  lint:
+    name: Lint
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - uses: cachix/install-nix-action@v31
+
+      - name: Clippy
+        run: nix develop .#ci --command cargo clippy --all-targets
+
+      # the code has not been run through rustfmt yet; report only for now
+      - name: Rustfmt
+        continue-on-error: true
+        run: nix develop .#ci --command cargo fmt --check

README.md

@@ -3,9 +3,12 @@ A lightweight library for some basic filtering methods
 
 ## Documentation
 
-This crate implements small, statically-sized Kalman and Extended Kalman Filter
-implementations using nalgebra's `SMatrix` for predictable, allocation-free
-linear algebra.
+This crate implements small Kalman and Extended Kalman Filters on nalgebra
+matrices. The models are generic over the dimension type, so the same
+implementation serves statically sized matrices (`KalmanFilterNM<N, M>`,
+`LinearUpdateNM<N, M>`, ... for predictable, allocation-free linear algebra)
+and runtime sized ones (`KalmanFilterDyn`, `ExtendedKalmanFilterDyn`), which
+the Python bindings wrap.
 
 Generate API documentation locally with:
 
@@ -20,6 +23,15 @@ The main modules are:
 - `crate::measurement` — measurement/observation helpers.
 - `crate::models` — traits for state transition and measurement models used by EKF.
 
+## Development environment
+
+A [Nix](https://nixos.org) flake provides the Rust toolchain, Python (with `numpy`, `pytest` and [Manim Community](https://www.manim.community) including its native dependencies such as cairo and pango) and `maturin`:
+
+```bash
+nix develop
+cargo test
+```
+
 ## Python
 
 Build and install the bindings into the current virtualenv with [maturin](https://www.maturin.rs):
@@ -31,7 +43,44 @@ maturin develop --release      # editable-style install for development
 pip install .                  # builds and installs a wheel (needs a Rust toolchain)
 ```
 
-Then `import determined as dt`. Build a distributable wheel with `maturin build --release`.
+Then `import determined as dt`.
+
+Six filters are available from Python:
+
+| Class | Models | Maths runs in |
+|---|---|---|
+| `dt.KalmanFilter(state, *, f, q, h, r)` | numpy matrices | Rust only |
+| `dt.ExtendedKalmanFilter(state, *, f, jac_f, q, h, jac_h, r)` | Python functions | Rust, calling back into Python for the model functions |
+| `dt.UnscentedKalmanFilter(state, *, f, q, h, r, alpha=1, beta=2, kappa=0)` | Python functions, no Jacobians | Rust, calling back into Python for the model functions |
+| `dt.UDKalmanFilter(state, *, f, q, h, r)` | numpy matrices | Rust only, in UD factorised form |
+| `dt.UDExtendedKalmanFilter(state, *, f, jac_f, q, h, jac_h, r)` | Python functions | Rust, in UD factorised form, calling back into Python for the model functions |
+| `dt.CustomFilter(update)` | a Python `UpdateModel` you implement | Python (the filter only delegates `predict`/`update`) |
+
+```python
+kf = dt.KalmanFilter(state, f=F, q=Q, h=H, r=R)
+kf.predict(dt.Epoch(1))
+kf.update(dt.Observation(z, epoch=dt.Epoch(1)))
+```
+
+`dt.ExtendedKalmanFilter` runs the filter maths in Rust and calls back into Python for the model:
+
+```python
+ekf = dt.ExtendedKalmanFilter(state, f=f, jac_f=jac_f, q=q, h=h, jac_h=jac_h, r=r)
+ekf.predict(dt.Epoch(1))
+ekf.update(dt.Observation(z, epoch=dt.Epoch(1)))
+```
+
+- **Model functions** are `f(x, t)`, `jac_f(x, t)`, `h(x, t)` and `jac_h(x, t)`. `x` is the state value (a numpy array) and `t` the integer epoch: `f` advances the state from epoch `t` to `t + 1`, and `h` is evaluated at the measurement epoch. They may be time-varying, and may return anything array-like, so Jacobians can come from an autodiff library (e.g. `jax.jacfwd`, which differentiates `x` and not `t`).
+- **Same implementation as Rust:** each Python filter wraps the crate's `...Dyn` filter of the same name (`KalmanFilterDyn`, `ExtendedKalmanFilterDyn`, `UnscentedKalmanFilterDyn`, `UDKalmanFilterDyn`, `UDExtendedKalmanFilterDyn`), so they behave exactly like the Rust filters.
+- **UD filters:** the covariance is held as factors `P = U D Uᵀ` (`U` unit upper triangular, `D` diagonal), propagated by weighted modified Gram–Schmidt and updated one scalar measurement at a time with the modified Agee–Turner update, following Ramos et al., [*A summary on the UD Kalman Filter*](https://arxiv.org/abs/2203.06105). A non-diagonal `R` is decorrelated first. This keeps `P` symmetric positive semi-definite by construction; the results equal the standard filters up to rounding. The state still carries `P = U D Uᵀ`, and setting it from outside refactorises.
+- **Unscented filter:** `f` and `h` are evaluated at 2N + 1 sigma points per step (van der Merwe scaled points), so they must not keep state between calls. The default `alpha=1, beta=2, kappa=0` keeps all sigma point weights non-negative; a very small `alpha` (e.g. `1e-3`) gives a large negative centre weight and is numerically fragile. The covariance must be strictly positive definite (and `kappa > -N`): a semi-definite covariance, such as one with a component known exactly, or a missing one makes `predict`/`update` log an error and leave the state unchanged.
+- **Epochs:** both filters advance the state one epoch at a time and do nothing when asked for the current epoch. Predicting backwards uses `F⁻¹` for `KalmanFilter`; the other filters log a warning and leave the state unchanged.
+- **Errors in model functions:** if `f`, `h` or a Jacobian raises (or returns the wrong shape), the exception propagates and the filter state is left as it was before the call.
+- **Keyword-only arguments:** constructors take their first argument positionally and the rest by keyword, e.g. `dt.State(value, covariance=cov, epoch=epoch)`.
+- **States are live views.** To avoid copying, the `State` returned by `predict`, `update` and `.state` shares memory with the filter's internal state, so it changes on the next call (and is the same object you passed in). Copy the arrays (`state.value.copy()`) if you need to keep a snapshot.
+- **Logging:** messages from the Rust side go through Python's `logging` under the name `determined`, e.g. `logging.getLogger("determined").setLevel(logging.DEBUG)`.
+
+See `examples/ekf.py`, `examples/ukf.py` and `examples/ud.py` (Rust equivalents: `simple_ekf.rs`, `simple_ukf.rs`, `simple_ud.rs`). Build a distributable wheel with `maturin build --release`.
 
 ## Examples
 <img src="docs/assets/TwoMassSpringDamperScene_ManimCE_v0.19.1.gif" alt="spring damper example"/>
@@ -46,3 +95,7 @@ To run the visualisation example using `python` and `manim` use:
 manim -pql examples/manim_two_mass_spring_damper.py TwoMassSpringDamperScene
 ```
 
+## Roadmap
+
+- **Gating:** expose the innovation `y = z - h(x)`, its covariance `S` and the normalised innovation squared `yᵀ S⁻¹ y` from `update`, so outlier measurements can be rejected against a chi-squared threshold. Not exposed yet.
+- **Automatic Jacobians:** optional `jac_f` / `jac_h` computed by autodiff or finite differences.

examples/algorithm.py

@@ -7,10 +7,10 @@
 value = np.array([1.0, 0.0])
 epoch = dt.Epoch(0)
 
-state = dt.State(value, np.eye(value.size), epoch)
-obs = dt.Observation(np.array([0.0, 1.0]), epoch)
+state = dt.State(value, covariance=np.eye(value.size), epoch=epoch)
+obs = dt.Observation(np.array([0.0, 1.0]), epoch=epoch)
 
-filter = dt.KalmanFilter(update)
+filter = dt.CustomFilter(update)
 
 if __name__ == "__main__":
 

examples/ekf.py

@@ -0,0 +1,40 @@
+"""Extended Kalman filter tracking a static 2D target from range-only measurements.
+
+The model functions take the state value `x` and the integer epoch `t`, and
+return plain numpy arrays, so the Jacobians could equally come from an autodiff
+library, e.g. `jac_h = jax.jacfwd(h)` (which differentiates `x`, not `t`).
+"""
+import numpy as np
+
+import determined as dt
+
+# initial guess (2, 2) with a wide covariance; the target is really at (3, 4)
+state = dt.State(np.array([2.0, 2.0]), covariance=np.eye(2) * 4.0, epoch=dt.Epoch(0))
+
+
+def f(x, t):
+    return x                      # static target
+
+
+def jac_f(x, t):
+    return np.eye(2)
+
+
+def h(x, t):
+    return np.array([np.linalg.norm(x)])
+
+
+def jac_h(x, t):
+    return (x / np.linalg.norm(x))[None, :]
+
+
+ekf = dt.ExtendedKalmanFilter(
+    state,
+    f=f, jac_f=jac_f, q=np.zeros((2, 2)),
+    h=h, jac_h=jac_h, r=np.array([[0.01]]),
+)
+
+for t in range(1, 6):
+    ekf.predict(dt.Epoch(t))
+    estimate = ekf.update(dt.Observation(np.array([5.0]), epoch=dt.Epoch(t)))
+    print(f"t={t} x={estimate.value} range={np.linalg.norm(estimate.value):.4f}")

examples/manim_two_mass_spring_damper.py

@@ -3,7 +3,7 @@
 
 This scene runs the same simulation as the Python example but renders a
 textbook-style diagram: two masses as boxes and springs as zig-zag lines.
-It uses the Rust `PyKalmanFilter` (PyO3) to compute the filter estimates.
+The filter estimates are computed in Rust by `determined.KalmanFilter`.
 
 Run (recommended, low-quality preview):
   pip install manim  # community edition
@@ -25,10 +25,6 @@
 
 import determined as dt
 
-from transition import LinearTransition
-from measurement import LinearMeasurement
-from update import LinearUpdate
-
 
 def setup_two_mass_spring_damper_params():
     m = 0.1
@@ -37,7 +33,7 @@ def setup_two_mass_spring_damper_params():
     b = 0.0036
 
     x0 = np.array([1.0, 0.0, 0.0, 0.0], dtype=float)
-    initial_state =  dt.State(x0, np.eye(x0.size), dt.Epoch(0))
+    initial_state =  dt.State(x0, covariance=np.eye(x0.size), epoch=dt.Epoch(0))
 
     a = np.array([
         [0.0, 1.0, 0.0, 0.0],
@@ -54,9 +50,6 @@ def setup_two_mass_spring_damper_params():
     sigma_q = np.diag([0.3 * _dt, 0.05 * _dt, 0.3 * _dt, 0.05 * _dt])
     q = sigma_q @ sigma_q.T
 
-    transition_model = LinearTransition(f, q, initial_state)
-    transition = dt.TransitionModel(transition_model, initial_state) 
-
     # measurement model
     h = np.array([
         [1.0, 0.0, 0.0, 0.0],
@@ -66,15 +59,8 @@ def setup_two_mass_spring_damper_params():
     sigma_r = np.diag([0.1, 0.1])
     r = sigma_r @ sigma_r.T
 
-    measurement_model = LinearMeasurement(h, r)
-    measurement = dt.MeasurementModel(measurement_model)
-
-    # update model
-    update_model = LinearUpdate(transition, measurement)
-    update = dt.UpdateModel(update_model, transition, measurement)
-
     # filter
-    kf = dt.KalmanFilter(update)
+    kf = dt.KalmanFilter(initial_state, f=f, q=q, h=h, r=r)
 
     # system models
     process_noise=0.00,
@@ -100,7 +86,7 @@ def simulate_rust(num_steps=100):
         x_true = model(x_true) # true system dynamics + process noise
         z = h(x_true) # measurement + measurement noise
 
-        _z = dt.Observation(z, dt.Epoch(step))
+        _z = dt.Observation(z, epoch=dt.Epoch(step))
         state_vec = kf.update(_z)
 
         traj_true[step] = x_true

examples/measurement.py

@@ -11,20 +11,23 @@ def __init__(self, h, r):
 
     def projection(self, state: dt.State):
         z = self.h @ state.value
-        return dt.Observation(z, state.epoch)
+        return dt.Observation(z, epoch=state.epoch)
 
     def inverse(self, observation: dt.Observation):
         x = self.h_inv @ observation.value
         cov = np.eye(x.size)
-        return dt.State(x, cov, observation.epoch)
+        return dt.State(x, covariance=cov, epoch=observation.epoch)
+
+    def noise(self):
+        return self.r
 
     def jacobian(self, state: dt.State):
         return self.jac
 
 
 value = np.array([1.0, 0.0])
 epoch = dt.Epoch(0)
-state = dt.State(value, np.eye(value.size), epoch)
+state = dt.State(value, covariance=np.eye(value.size), epoch=epoch)
 
 h = np.array([
     [1.0, 0.0],
@@ -39,6 +42,7 @@ def jacobian(self, state: dt.State):
     obs = measurement.projection(state)
     inv = measurement.inverse(obs)
     jac = measurement.jacobian(state)
+    r = measurement.noise()
 
     print(state)
     print(obs)

examples/simple_ekf.rs

@@ -0,0 +1,51 @@
+//! Extended Kalman filter tracking a static 2D target from range-only
+//! measurements, a nonlinear sensor h(x) = |x|.
+
+use determined::algorithms::ExtendedKalmanFilterNM;
+use determined::common::na;
+use determined::epoch::Epoch;
+use determined::filter::Filter;
+use determined::measurement::Observation;
+use determined::models::*;
+use determined::state::State;
+
+fn main() {
+    // initial guess (2, 2) with a wide covariance; the target is really at (3, 4)
+    let state = State::<na::Const<2>>::new(
+        vec![2.0, 2.0],
+        vec![vec![4.0, 0.0], vec![0.0, 4.0]],
+        Epoch::new(0),
+    )
+    .ptr();
+
+    // static target: x(t + 1) = x(t)
+    let transition = NonLinearTransitionN::<2>::new(
+        state.clone(),
+        Box::new(|state| state.value),
+        Box::new(|_| na::SMatrix::<f64, 2, 2>::identity()),
+        na::SMatrix::<f64, 2, 2>::zeros(),
+    );
+
+    // range sensor and its Jacobian
+    let measurement = NonLinearMeasurementNM::<2, 1>::new(
+        Box::new(|state| na::SMatrix::<f64, 1, 1>::new(state.value.norm())),
+        Box::new(|state| {
+            let range = state.value.norm();
+            na::SMatrix::<f64, 1, 2>::new(state.value[0] / range, state.value[1] / range)
+        }),
+        na::SMatrix::<f64, 1, 1>::new(0.01),
+    );
+
+    let mut filter =
+        ExtendedKalmanFilterNM::new(NonLinearUpdateNM::<2, 1>::new(measurement, transition));
+
+    for t in 1..=5 {
+        let obs = Observation {
+            value: na::SMatrix::<f64, 1, 1>::new(5.0),
+            epoch: Epoch::new(t),
+        };
+        filter.predict(&obs.epoch);
+        let estimate = filter.update(&obs);
+        println!("t={} {}", t, estimate.read().unwrap());
+    }
+}