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>,
    /* private fields */
}

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 predict<QD: Dim>( &mut self, fx: &OMatrix<N, D, D>, x_pred: &OVector<N, D>, noise: &CoupledNoise<N, D, QD> ) -> Result<N, &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, &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, &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, &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, &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>, &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 kalman_state(&self) -> Result<KalmanState<N, D>, &str>

Derive the KalmanState from the UDState.

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

impl<N: Copy + RealField, D: Dim> TryFrom<KalmanState<N, D>> for UDState<N, D>

where DefaultAllocator: Allocator<N, D, D> + Allocator<N, D>,

fn try_from(state: KalmanState<N, D>) -> Result<Self, Self::Error>

Construct the UDState with a KalmanState.

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

type Error = &'static str

The type returned in the event of a conversion error.