#![allow(non_snake_case)]
use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, OVector, RealField};
use crate::linalg::rcond;
use crate::matrix::check_positive;
use crate::models::{
Estimator, ExtendedLinearObserver, ExtendedLinearPredictor, InformationState, KalmanEstimator,
KalmanState,
};
use crate::noise::{CorrelatedNoise, CoupledNoise};
impl<N: RealField, D: Dim> Estimator<N, D> for InformationState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
fn state(&self) -> Result<OVector<N, D>, &str> {
KalmanEstimator::kalman_state(self).map(|r| r.x)
}
}
impl<N: RealField, D: Dim> TryFrom<KalmanState<N, D>> for InformationState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
type Error = &'static str;
fn try_from(state: KalmanState<N, D>) -> Result<Self, Self::Error> {
let I = state.X.clone().cholesky().ok_or("X not PD")?.inverse();
let i = &I * &state.x;
Ok(InformationState { i, I })
}
}
impl<N: RealField, D: Dim> KalmanEstimator<N, D> for InformationState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
fn kalman_state(&self) -> Result<KalmanState<N, D>, &str> {
let X = self.I.clone().cholesky().ok_or("Y not PD")?.inverse();
let x = &X * &self.i;
Ok(KalmanState { x, X })
}
}
impl<N: RealField, D: Dim> ExtendedLinearPredictor<N, D> for InformationState<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> {
let mut X = self.I.clone().cholesky().ok_or("I not PD in predict")?.inverse();
X.quadform_tr(N::one(), &fx, &X.clone(), N::zero());
X += &noise.Q;
self.I = X.cholesky().ok_or("X not PD")?.inverse();
self.i = &self.I * x_pred;
return Ok(());
}
}
impl<N: Copy + RealField, D: Dim, ZD: Dim> ExtendedLinearObserver<N, D, ZD>
for InformationState<N, D>
where
DefaultAllocator: Allocator<N, D, D>
+ Allocator<N, D, ZD>
+ Allocator<N, ZD, D>
+ Allocator<N, ZD, 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> {
let x = self.state().unwrap();
let noise_inv = noise
.Q
.clone()
.cholesky()
.ok_or("Q not PD in observe")?
.inverse();
let info = self.observe_info(hx, &noise_inv, &(s + hx * x));
self.add_information(&info);
return Ok(());
}
}
impl<N: Copy + RealField, D: Dim> InformationState<N, D>
where
DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,
{
pub fn predict_linear<QD: Dim>(
&mut self,
pred_inv: OMatrix<N, D, D>, noise: &CoupledNoise<N, D, QD>,
) -> Result<N, &str>
where
DefaultAllocator:
Allocator<N, QD, QD> + Allocator<N, D, QD> + Allocator<N, QD, D> + Allocator<N, QD>,
{
let I_shape = self.I.shape_generic();
let A = (&self.I * &pred_inv).tr_mul(&pred_inv);
let mut B = (&A * &noise.G).tr_mul(&noise.G);
for i in 0..noise.q.nrows() {
B[(i, i)] += N::one() / noise.q[i];
}
B = B.cholesky().ok_or("B not PD")?.inverse();
let rcond = rcond::rcond_symetric(&B);
check_positive(rcond, "(G'invFx'.I.inv(Fx).G + inv(Q)) not PD")?;
self.I.quadform_tr(N::one(), &noise.G, &B, N::zero());
let ig = OMatrix::identity_generic(I_shape.0, I_shape.1) - &A * &self.I;
self.I = &ig * &A;
let y = pred_inv.tr_mul(&self.i);
self.i = &ig * &y;
Ok(rcond)
}
pub fn add_information(&mut self, information: &InformationState<N, D>) {
self.i += &information.i;
self.I += &information.I;
}
pub fn observe_info<ZD: Dim>(
&self,
hx: &OMatrix<N, ZD, D>,
noise_inv: &OMatrix<N, ZD, ZD>, z: &OVector<N, ZD>,
) -> InformationState<N, D>
where
DefaultAllocator:
Allocator<N, ZD, ZD> + Allocator<N, ZD, D> + Allocator<N, D, ZD> + Allocator<N, ZD>,
{
let HxTZI = hx.tr_mul(noise_inv);
let ii = &HxTZI * z;
let II = &HxTZI * hx; InformationState { i: ii, I: II }
}
}