Struct Ukf 

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

Unscented Kalman Filter with Merwe-scaled sigma points.

N is the state dimension, M is the measurement dimension. Uses 2N+1 sigma points to capture mean and covariance through nonlinear transformations without requiring Jacobians.

Requires the alloc feature for temporary sigma point storage in predict and update.

Default parameters

alpha=1.0, beta=2.0, kappa=0.0 — all sigma-point weights are non-negative, sigma points are placed at distance √N · L[:,i] from the mean. For tight sigma point clustering, reduce alpha toward 0.1.

Example

use numeris::estimate::Ukf;
use numeris::{ColumnVector, Matrix};

let x0 = ColumnVector::from_column([0.0_f64, 1.0]);
let p0 = Matrix::new([[1.0, 0.0], [0.0, 1.0]]);
let mut ukf = Ukf::<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
ukf.predict(
    |x| ColumnVector::from_column([x[(0, 0)] + dt * x[(1, 0)], x[(1, 0)]]),
    Some(&q),
).unwrap();

// Update
ukf.update(
    &ColumnVector::from_column([0.12]),
    |x| ColumnVector::from_column([x[(0, 0)]]),
    &r,
).unwrap();

Fields

x: ColumnVector<T, N>

State estimate.

p: Matrix<T, N, N>

State covariance.

Implementations

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

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

Create a new UKF with default Merwe parameters: alpha=1.0, beta=2.0, kappa=0.0.

These defaults yield non-negative sigma-point weights for all state dimensions. For tighter sigma point spread, use with_params to reduce alpha.

pub fn with_params( x0: ColumnVector<T, N>, p0: Matrix<T, N, N>, alpha: T, beta: T, kappa: T, ) -> Self

Create a new UKF with custom scaling parameters.

• alpha — spread of sigma points around mean (1.0 gives non-negative weights; values < ~0.52 give negative central weight wc_0)
• beta — prior distribution knowledge (2.0 optimal for Gaussian)
• kappa — secondary scaling (0 or 3-N)

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

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

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

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

The predicted covariance becomes γ · P_sigma + Q where P_sigma is the sigma-point weighted covariance (without Q). Default is 1.0.

pub fn state(&self) -> &ColumnVector<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(&ColumnVector<T, N>) -> ColumnVector<T, N>, q: Option<&Matrix<T, N, N>>, ) -> Result<(), EstimateError>

Predict step.

• f — state transition: x_{k+1} = f(x_k)
• q — process noise covariance (pass None for zero process noise)

Generates 2N+1 sigma points, propagates through f, and reconstructs the predicted mean and covariance as γ · P_sigma + Q.

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

Update step.

• z — measurement vector
• h — measurement model: z = h(x)
• r — measurement noise covariance

Generates sigma points from the predicted state, transforms through h, and computes the Kalman gain via the cross-covariance and innovation covariance.

Covariance update uses the symmetric P - K S Kᵀ form (equivalent to P - K Pxzᵀ but manifestly PSD-subtracted).

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

pub fn update_gated( &mut self, z: &ColumnVector<T, M>, h: impl Fn(&ColumnVector<T, N>) -> ColumnVector<T, M>, 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 rejected, Ok(Some(nis)) when accepted.

Chi-squared thresholds: M=1 → 99%: 6.63 | M=2 → 9.21 | M=3 → 11.34