Struct Ckf 

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

Cubature Kalman Filter with third-degree spherical-radial cubature rule.

Uses 2N cubature points with equal weight 1/(2N). Unlike the UKF, the CKF has no tuning parameters — the cubature rule is uniquely determined by the state dimension.

N is the state dimension, M is the measurement dimension. Requires the alloc feature for temporary cubature point storage.

Example

use numeris::estimate::Ckf;
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 ckf = Ckf::<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]]);

ckf.predict(
    |x| ColumnVector::from_column([x[(0, 0)] + dt * x[(1, 0)], x[(1, 0)]]),
    Some(&q),
).unwrap();

ckf.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> Ckf<T, N, M>

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

Create a new CKF 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.

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_cubature + 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 cubature points, propagates through f, and reconstructs the predicted mean and covariance as γ · P_cubature + 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

Covariance update uses the symmetric P - K S Kᵀ form.

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