```  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
101
102
103
```
```#![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, Const, DefaultAllocator, Dim, OMatrix, OVector, RealField};

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> KalmanState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
pub fn new_zero(d: D) -> KalmanState<N, D> {
KalmanState {
x: OVector::zeros_generic(d, Const::<1>),
X: OMatrix::zeros_generic(d, d),
}
}
}

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>, &'static str> {
Ok(self.x.clone())
}
}

impl<N: Copy + RealField, D: Dim> KalmanEstimator<N, D> for KalmanState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
fn init(&mut self, state: &KalmanState<N, D>) -> Result<(), &'static str> {
self.x = state.x.clone();
self.X = state.X.clone();
let rcond = rcond::rcond_symetric(&self.X);
check_non_negativ(rcond, "X not PSD")?;

Ok(())
}

fn kalman_state(&self) -> Result<KalmanState<N, D>, &'static 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<(), &'static 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<(), &'static 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(())
}
}

```