Struct bayes_estimate::estimators::ud::UDState

pub struct UDState<N: RealField, D: Dim> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,  {
    pub x: OVector<N, D>,
    pub UD: OMatrix<N, D, D>,
    // some fields omitted
}

UD State representation.

Linear representation as a state vector and ‘square root’ factorisation of the state covariance matrix. Numerically the this ‘square root’ factorisation is advantageous as conditioning for inverting is improved by the square root.

The state covariance is represented as a U.d.U’ factorisation, where U is upper triangular matrix (0 diagonal) and d is a diagonal vector. U and d are packed into a single UD Matrix, the lower Triangle ist not part of state representation.

Fields

x: OVector<N, D>

State vector

UD: OMatrix<N, D, D>

UD matrix representation of state covariance

Implementations

impl<N: Copy + RealField, D: Dim> UDState<N, D> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>, 

pub fn new(UD: OMatrix<N, D, D>, x: OVector<N, D>) -> Self

Create a UDState for given state dimensions.

D is the size of states vector and rows in UD.

pub fn new_zero(d: D) -> Self

Create a UDState for given state dimensions.

d is the size of states vector and rows in UD.

pub fn predict<QD: Dim>(
    &mut self,
    fx: &OMatrix<N, D, D>,
    x_pred: &OVector<N, D>,
    noise: &CoupledNoise<N, D, QD>
) -> Result<N, &'static str> where
    D: DimAdd<QD>,
    DefaultAllocator: Allocator<N, DimSum<D, QD>, U1> + Allocator<N, D, QD> + Allocator<N, QD>, 

pub fn observe_innovation<ZD: Dim>(
    &mut self,
    s: &OVector<N, ZD>,
    hx: &OMatrix<N, ZD, D>,
    noise: &UncorrelatedNoise<N, ZD>
) -> Result<N, &'static str> where
    DefaultAllocator: Allocator<N, ZD, D> + Allocator<N, ZD>, 

Implement observe using sequential observation updates.

Uncorrelated observations are applied sequentially in the order they appear in z.

impl<N: Copy + RealField, D: Dim> UDState<N, D> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>, 

pub fn observe_linear_correlated<ZD: Dim>(
    &mut self,
    z: &OVector<N, ZD>,
    hx: &OMatrix<N, ZD, D>,
    h_normalize: fn(_: &mut OVector<N, ZD>, _: &OVector<N, ZD>),
    noise_factor: &CorrelatedFactorNoise<N, ZD>
) -> Result<N, &'static str> where
    DefaultAllocator: Allocator<N, ZD, ZD> + Allocator<N, ZD, D> + Allocator<N, ZD>, 

Special Linear ‘hx’ observe for correlated factorised noise.

Observation predictions are made with the linear ‘hx’. This allows the observation noise to be decorrelated. Observations can then be applied for each element in the order they appear in z.

Return: Minimum rcond of all sequential observations

impl<N: Copy + RealField, D: Dim> UDState<N, D> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>, 

pub fn new_predict_scratch<QD: Dim>(&self, qd: QD) -> PredictScratch<N, D, QD> where
    D: DimAdd<QD>,
    DefaultAllocator: Allocator<N, D, QD> + Allocator<N, DimSum<D, QD>, U1>, 

pub fn new_observe_scratch(&self) -> ObserveScratch<N, D>

pub fn predict_use_scratch<QD: Dim>(
    &mut self,
    scratch: &mut PredictScratch<N, D, QD>,
    x_pred: &OVector<N, D>,
    fx: &OMatrix<N, D, D>,
    noise: &CoupledNoise<N, D, QD>
) -> Result<N, &'static str> where
    D: DimAdd<QD>,
    DefaultAllocator: Allocator<N, DimSum<D, QD>> + Allocator<N, D, QD> + Allocator<N, QD>, 

pub fn observe_innovation_use_scratch<ZD: Dim>(
    &mut self,
    scratch: &mut ObserveScratch<N, D>,
    s: &OVector<N, ZD>,
    hx: &OMatrix<N, ZD, D>,
    noise: &UncorrelatedNoise<N, ZD>
) -> Result<N, &'static str> where
    DefaultAllocator: Allocator<N, ZD, D> + Allocator<N, ZD>, 

Trait Implementations

impl<N: Copy + RealField, D: Dim> Estimator<N, D> for UDState<N, D> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>, 

fn state(&self) -> Result<OVector<N, D>, &'static str>

The estimator’s estimate of the system’s state.

impl<N: Copy + RealField, D: Dim> KalmanEstimator<N, D> for UDState<N, D> where
    DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>, 

fn init(&mut self, state: &KalmanState<N, D>) -> Result<(), &'static str>

Initialise the UDState with a KalmanState.

The covariance matrix X is factorised into a U.d.U’ as a UD matrix.

fn kalman_state(&self) -> Result<KalmanState<N, D>, &'static str>

Derive the KalmanState from the UDState.

The covariance matrix X is recomposed from U.d.U’ in the UD matrix.