Struct Ekf 

pub struct Ekf<T: FloatScalar, const N: usize, const M: usize> {
    pub x: Vector<T, N>,
    pub p: Matrix<T, N, N>,
    /* private fields */
}

Extended Kalman Filter with const-generic state and measurement dimensions.

N is the state dimension, M is the measurement dimension. The EKF linearizes nonlinear dynamics and measurement models via user-supplied or finite-difference Jacobians.

All operations are stack-allocated — no heap, fully no-std compatible.

Robustness features

• Joseph form covariance update: P = (I-KH)P(I-KH)ᵀ + KRKᵀ
• Cholesky with jitter: retries with small diagonal ε·I when S is near-singular
• Covariance floor: set_min_variance clamps P diagonal entries to a minimum
• Fading memory: set_fading_memory scales predicted covariance by γ≥1
• Innovation gating: update_gated / update_fd_gated reject outlier measurements
• Iterated EKF: update_iterated / update_fd_iterated re-linearize at each step

Example

use numeris::estimate::Ekf;
use numeris::{Vector, Matrix};

// Constant-velocity model: state = [position, velocity]
let x0 = Vector::from_array([0.0_f64, 1.0]);
let p0 = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
let mut ekf = Ekf::<f64, 2, 1>::new(x0, p0);

let dt = 0.1;
let q = Matrix::new([[0.01, 0.0], [0.0, 0.01]]);
let r = Matrix::new([[0.5]]);

// Predict (with process noise)
ekf.predict(
    |x| Vector::from_array([x[0] + dt * x[1], x[1]]),
    |_x| Matrix::new([[1.0, dt], [0.0, 1.0]]),
    Some(&q),
);

// Update with position measurement
ekf.update(
    &Vector::from_array([0.12]),
    |x| Vector::from_array([x[0]]),
    |_x| Matrix::new([[1.0, 0.0]]),
    &r,
).unwrap();

Fields

x: Vector<T, N>

State estimate.

p: Matrix<T, N, N>

State covariance.

Implementations

impl<T: FloatScalar, const N: usize, const M: usize> Ekf<T, N, M>

pub fn new(x0: Vector<T, N>, p0: Matrix<T, N, N>) -> Self

Create a new EKF with initial state x0 and covariance p0.

pub fn with_min_variance(self, min_variance: T) -> Self

Set a minimum diagonal variance floor applied after every predict/update.

Prevents covariance from degenerating to zero or going negative from accumulated numerical subtractions. Pass 0 to disable (default).

pub fn with_fading_memory(self, gamma: T) -> Self

Set a fading-memory factor γ ≥ 1 applied to the propagated covariance.

The predicted covariance becomes γ · F P Fᵀ + Q. Values γ > 1 inflate uncertainty after prediction to compensate for unmodeled dynamics. Default is 1.0 (standard filter).

pub fn state(&self) -> &Vector<T, N>

Reference to the current state estimate.

pub fn covariance(&self) -> &Matrix<T, N, N>

Reference to the current state covariance.

pub fn predict( &mut self, f: impl Fn(&Vector<T, N>) -> Vector<T, N>, fj: impl Fn(&Vector<T, N>) -> Matrix<T, N, N>, q: Option<&Matrix<T, N, N>>, )

Predict step with explicit dynamics Jacobian.

• f — state transition: x_{k+1} = f(x_k)
• fj — Jacobian of f evaluated at x_k: F = ∂f/∂x
• q — process noise covariance (pass None for zero process noise)

Updates: x = f(x), P = γ · F P Fᵀ + Q.

pub fn predict_fd( &mut self, f: impl Fn(&Vector<T, N>) -> Vector<T, N>, q: Option<&Matrix<T, N, N>>, )

Predict step with finite-difference Jacobian.

Computes the Jacobian of f automatically using forward differences. Pass None for q if there is no process noise.

pub fn update( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, hj: impl Fn(&Vector<T, N>) -> Matrix<T, M, N>, r: &Matrix<T, M, M>, ) -> Result<T, EstimateError>

Update step with explicit measurement Jacobian.

• z — measurement vector
• h — measurement model: z = h(x)
• hj — Jacobian of h evaluated at x: H = ∂h/∂x
• r — measurement noise covariance

Uses Joseph form for numerical stability: P = (I - KH) P (I - KH)ᵀ + K R Kᵀ

Returns the Normalized Innovation Squared (NIS): yᵀ S⁻¹ y.

pub fn update_fd( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, r: &Matrix<T, M, M>, ) -> Result<T, EstimateError>

Update step with finite-difference measurement Jacobian.

Computes the Jacobian of h automatically using forward differences.

Returns the Normalized Innovation Squared (NIS): yᵀ S⁻¹ y.

pub fn update_gated( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, hj: impl Fn(&Vector<T, N>) -> Matrix<T, M, N>, r: &Matrix<T, M, M>, gate: T, ) -> Result<Option<T>, EstimateError>

Update with innovation gating — skips state update if NIS exceeds gate.

Returns Ok(None) when the measurement is rejected (outlier), or Ok(Some(nis)) when accepted and applied.

A chi-squared table gives typical gate thresholds: M=1 → 99%: 6.63 | M=2 → 9.21 | M=3 → 11.34 | M=6 → 16.81

pub fn update_fd_gated( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, r: &Matrix<T, M, M>, gate: T, ) -> Result<Option<T>, EstimateError>

Update with gating and finite-difference Jacobian.

Returns Ok(None) when rejected, Ok(Some(nis)) when accepted.

pub fn update_iterated( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, hj: impl Fn(&Vector<T, N>) -> Matrix<T, M, N>, r: &Matrix<T, M, M>, max_iter: usize, tol: T, ) -> Result<T, EstimateError>

Iterated EKF update — re-linearizes at the current iterate until convergence.

Substantially more accurate than the standard EKF for highly nonlinear measurement models (e.g., angle/range measurements, attitude-to-pixel projection).

At each iteration the linearization point advances: x_{i+1} = x̄ + K_i (z - h(x_i) - H_i (x̄ - x_i))

Iteration stops when ‖x_{i+1} - x_i‖² < tol². The final covariance is updated with the Joseph form at the converged linearization point.

Returns the Normalized Innovation Squared at the converged solution.

pub fn update_fd_iterated( &mut self, z: &Vector<T, M>, h: impl Fn(&Vector<T, N>) -> Vector<T, M>, r: &Matrix<T, M, M>, max_iter: usize, tol: T, ) -> Result<T, EstimateError>

Iterated EKF update with finite-difference Jacobian.

See update_iterated for details.