1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

#![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 as na;
use na::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector};
use na::{SimdRealField};
use crate::noise::CorrelatedNoise;

/// Kalman state.
///
/// Linear representation as a state vector and the state covariance (symmetric positive semi-definite) matrix.
#[derive(PartialEq, Clone)]
pub struct KalmanState<N: SimdRealField, D: Dim>
where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, 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 a 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: SimdRealField, D: Dim>
where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
    /// Information state vector
    pub i: OVector<N, D>,
    /// Information matrix (symmetric positive semi-definite)
    pub I: OMatrix<N, D, D>,
}

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

/// A Kalman estimator.
///
/// The linear Kalman state representation x,X is used to represent the system.
pub trait KalmanEstimator<N: SimdRealField, D: Dim>
where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
    /// Initialise the estimator with a KalmanState.
    fn init(&mut self, state: &KalmanState<N, D>) -> Result<(), &'static str>;

    /// The estimator's estimate of the system's KalmanState.
    fn kalman_state(&self) -> Result<KalmanState<N, D>, &'static str>;
}

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

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