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
97
98
99
100

#![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.
//!
//! [`KalmanState`]: ../models/struct.KalmanState.html

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

use crate::linalg::rcond;
use crate::matrix::check_non_negativ;
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<N, D, D> + Allocator<N, D>,
{
    fn state(&self) -> Result<OVector<N, D>, &str> {
        Ok(self.x.clone())
    }
}

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

        Ok(state)
    }
}

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

impl<N: RealField, D: Dim> ExtendedLinearPredictor<N, D> for KalmanState<N, D>
where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
    fn predict(
        &mut self,
        x_pred: &OVector<N, D>,
        fx: &OMatrix<N, D, D>,
        noise: &CorrelatedNoise<N, D>,
    ) -> Result<(), &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<N, D, D>
        + Allocator<N, ZD, ZD>
        + Allocator<N, ZD, D>
        + Allocator<N, D, ZD>
        + Allocator<N, D>
        + Allocator<N, ZD>,
{
    fn observe_innovation(
        &mut self,
        s: &OVector<N, ZD>,
        hx: &OMatrix<N, ZD, D>,
        noise: &CorrelatedNoise<N, ZD>,
    ) -> Result<(), &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(())
    }
}