bayes_estimate 0.20.0

#![allow(non_snake_case)]

//! Bayesian estimation models.
//!
//! Linear models are represented as structs.
//! Common Bayesian discrete system estimation operations are defined as traits.

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

use crate::noise::CorrelatedNoise;

/// A state estimator.
pub trait Estimator<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D>,
{
    /// The estimator's estimate of the system's state.
    fn state<'e>(&self) -> Result<OVector<N, D>, &'e str>;
}

/// A functional state predictor.
///
/// Uses a functional state prediction model with additive noise.
pub trait FunctionalPredictor<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// Uses the functional state prediction model with additive noise.
    fn predict(
        &mut self,
        f: impl Fn(&OVector<N, D>) -> OVector<N, D>,
        noise: &CorrelatedNoise<N, D>,
    ) -> Result<(), &str>;
}

/// A functional state observer.
///
/// Uses a functional state observation model with additive noise.
pub trait FunctionalObserver<N: RealField, D: Dim, ZD: Dim>
where
    DefaultAllocator:
        Allocator<ZD, D> + Allocator<ZD, ZD> + Allocator<D> + Allocator<ZD>,
{
    /// Uses the functional state observation with additive noise.
    fn observe(
        &mut self,
        z: &OVector<N, ZD>,
        h: impl Fn(&OVector<N, D>) -> OVector<N, ZD>,
        noise: &CorrelatedNoise<N, ZD>,
    ) -> Result<(), &str>;
}

/// Kalman state.
///
/// Linear representation as a state vector and the state covariance (symmetric positive semi-definite) matrix.
#[derive(PartialEq, Clone)]
pub struct KalmanState<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// State vector
    pub x: OVector<N, D>,
    /// State covariance matrix (symmetric positive semi-definite)
    pub X: OMatrix<N, D, D>,
}

/// Information state.
///
/// Linear representation as an information state vector and the information (symmetric positive semi-definite) matrix.
/// For a given [KalmanState] the information state I == inverse(X), i == I.x
#[derive(PartialEq, Clone)]
pub struct InformationState<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// Information state vector
    pub i: OVector<N, D>,
    /// Information matrix (symmetric positive semi-definite)
    pub I: OMatrix<N, D, D>,
}

/// A Kalman estimator.
///
/// The linear Kalman state representation x,X is used to represent the system.
pub trait KalmanEstimator<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// The estimator's estimate of the system's KalmanState.
    fn kalman_state<'e>(&self) -> Result<KalmanState<N, D>, &'e str>;
}

/// An extended linear predictor.
///
/// Uses a non-linear state prediction with linearised estimation model and additive noise.
pub trait ExtendedLinearPredictor<N: RealField, D: Dim>
where
    DefaultAllocator: Allocator<D, D> + Allocator<D>,
{
    /// Uses a non-linear state prediction with linear estimation model and additive noise.
    fn predict<'e>(
        &mut self,
        x_pred: &OVector<N, D>,
        Fx: &OMatrix<N, D, D>, // State transition matrix
        noise: &CorrelatedNoise<N, D>,
    ) -> Result<(), &'e str>;
}

/// An extended linear observer.
///
/// Uses a non-linear state observation with linearised estimation model and additive noise.
pub trait ExtendedLinearObserver<N: RealField, D: Dim, ZD: Dim>
where
    DefaultAllocator: Allocator<ZD, D> + Allocator<ZD, ZD> + Allocator<ZD>,
{
    /// Uses a non-linear state observation with linear estimation model and additive noise.
    fn observe_innovation<'e>(
        &mut self,
        s: &OVector<N, ZD>,
        Hx: &OMatrix<N, ZD, D>, // Observation matrix
        noise: &CorrelatedNoise<N, ZD>,
    ) -> Result<(), &'e str>;
}