estima 0.3.0

Kalman estimator
Documentation 
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

use nalgebra::{
    allocator::Allocator,
    base::{OMatrix, OVector},
    DefaultAllocator, DimMin, DimName, RealField,
};

pub fn unscented_transform<N, E, T>(
    sigma_pts: &OMatrix<T, N, E>, // N rows, E columns
    w_mean: &OVector<T, E>,
    w_covar: &OVector<T, E>,
) -> (OVector<T, N>, OMatrix<T, N, N>)
where
    N: DimName,
    E: DimName + DimMin<N, Output = N>,
    <E as DimMin<N>>::Output: DimName,
    T: RealField + Copy,
    DefaultAllocator: Allocator<<E as DimMin<N>>::Output>
        + Allocator<N>
        + Allocator<E>
        + Allocator<N, E>
        + Allocator<E, N>
        + Allocator<<E as DimMin<N>>::Output, N>
        + Allocator<N, <E as DimMin<N>>::Output>
        + Allocator<N, N>,
{
    let mut mean = OVector::<T, N>::zeros();
    for i in 0..E::dim() {
        mean.axpy(w_mean[i], &sigma_pts.column(i), T::one());
    }

    let mut dev = OMatrix::<T, N, E>::zeros();
    for i in 0..E::dim() {
        let f = w_covar[i].sqrt();
        let mut col = dev.column_mut(i);
        col.copy_from(&(sigma_pts.column(i) - &mean));
        col.scale_mut(f);
    }

    let qr = dev.transpose().qr();
    let r_mat = qr.r();

    let r_t = r_mat.transpose();

    (mean, r_t)
}