bayes_estimate 0.20.0

#![allow(non_snake_case)]

//! Covariance state estimation.
//!
//! A discrete Bayesian estimator that uses the 'Kalman' linear representation [`KalmanState`] of the system for estimation.
//!
//! The linear representation can also be used for non-linear systems by using linearised models.

use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector, RealField};
use num_traits::FromPrimitive;

use crate::linalg::rcond;
use crate::models::{
    Estimator, ExtendedLinearObserver, ExtendedLinearPredictor, KalmanEstimator, KalmanState,
};
use crate::noise::CorrelatedNoise;

impl<N: RealField, D: Dim> Estimator<N, D> for KalmanState<N, D>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    fn state<'e>(&self) -> Result<OVector<N, D>, &'e str> {
        Ok(self.x.clone())
    }
}

impl<N: Copy + FromPrimitive + RealField, D: Dim> KalmanState<N, D>
    where
        DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    pub fn validate_PSD(state: KalmanState<N, D>) -> Result<Self, &'static str> {
        rcond::check_non_negative(rcond::rcond_symmetric(&state.X), "X not PSD")?;

        Ok(state)
    }
}

impl<N: Copy + RealField, D: Dim> KalmanEstimator<N, D> for KalmanState<N, D>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    fn kalman_state<'e>(&self) -> Result<KalmanState<N, D>, &'e str> {
        Ok(self.clone())
    }
}

impl<N: RealField, D: Dim> ExtendedLinearPredictor<N, D> for KalmanState<N, D>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    fn predict<'e>(
        &mut self,
        x_pred: &OVector<N, D>,
        fx: &OMatrix<N, D, D>,
        noise: &CorrelatedNoise<N, D>,
    ) -> Result<(), &'e str> {
        self.x = x_pred.clone();
        // X = Fx.X.FX' + Q
        self.X.quadform_tr(N::one(), &fx, &self.X.clone(), N::zero());
        self.X += &noise.Q;

        Ok(())
    }
}

impl<N: RealField, D: Dim, ZD: Dim> ExtendedLinearObserver<N, D, ZD> for KalmanState<N, D>
where
    DefaultAllocator: Allocator<D, D>
        + Allocator<ZD, ZD>
        + Allocator<ZD, D>
        + Allocator<D, ZD>
        + Allocator<D>
        + Allocator<ZD>,
{
    fn observe_innovation<'e>(
        &mut self,
        s: &OVector<N, ZD>,
        hx: &OMatrix<N, ZD, D>,
        noise: &CorrelatedNoise<N, ZD>,
    ) -> Result<(), &'e str> {
        // S = Hx.X.Hx' + Q
        let mut S = noise.Q.clone();
        S.quadform_tr(N::one(), hx, &self.X, N::one());

        // Inverse innovation covariance
        let SI = S.clone().cholesky().ok_or("S not PD in observe")?.inverse();
        // Kalman gain, X*Hx'*SI
        let W = &self.X * hx.transpose() * SI;
        // State update
        self.x += &W * s;
        // X -= W.S.W'
        self.X.quadform_tr(N::one().neg(), &W, &S, N::one());

        Ok(())
    }
}