Struct SquareRootKalmanFilter 

pub struct SquareRootKalmanFilter<const N: usize, const M: usize> {
    pub x: [f32; N],
    pub s: [[f32; N]; N],
    pub f: [[f32; N]; N],
    pub s_q: [[f32; N]; N],
    pub h: [[f32; N]; M],
    pub s_r: [[f32; M]; M],
}

Square-Root Covariance Kalman Filter (SRKF) for $N$-state, $M$-measurement linear systems.

Propagates the lower-triangular Cholesky factor $S$ of the covariance matrix ($P = S S^T$). By operating directly on the square-root factors via orthogonal Givens transformations, the filter guarantees numerical positive-definiteness and never diverges due to roundoff error.

Fields

x: [f32; N]

State estimate vector $\hat{x} \in \mathbb{R}^N$.

s: [[f32; N]; N]

Lower-triangular Cholesky factor of state covariance $P = S S^T$.

f: [[f32; N]; N]

State transition matrix $F \in \mathbb{R}^{N \times N}$.

s_q: [[f32; N]; N]

Lower-triangular Cholesky factor of process noise covariance $Q = S_Q S_Q^T$.

h: [[f32; N]; M]

Measurement matrix $H \in \mathbb{R}^{M \times N}$.

s_r: [[f32; M]; M]

Lower-triangular Cholesky factor of measurement noise $R = S_R S_R^T$.

Implementations

impl<const N: usize, const M: usize> SquareRootKalmanFilter<N, M>

pub fn new( x0: [f32; N], s0: [[f32; N]; N], f: [[f32; N]; N], s_q: [[f32; N]; N], h: [[f32; N]; M], s_r: [[f32; M]; M], ) -> Self

Initialize a new Square-Root Kalman Filter from explicit Cholesky factors.

pub fn predict(&mut self)

Predict step: propagates state $\hat{x}^- = F \hat{x}$ and triangularizes $[F S \quad S_Q]$.

pub fn update(&mut self, z: &[f32; M]) -> Status

Update step: updates state $\hat{x}^+$ and factor $S^+$ given measurement vector $z \in \mathbb{R}^M$.

pub fn covariance(&self) -> [[f32; N]; N]

Reconstructs the full covariance matrix $P = S S^T$.

Trait Implementations

impl<const N: usize, const M: usize> Clone for SquareRootKalmanFilter<N, M>

fn clone(&self) -> SquareRootKalmanFilter<N, M>

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl<const N: usize, const M: usize> Debug for SquareRootKalmanFilter<N, M>

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.