Struct adskalman::KalmanFilterNoControl

source ·
pub struct KalmanFilterNoControl<'a, R, SS, OS>
where R: RealField, SS: DimName, OS: DimName,
{ /* private fields */ }
Expand description

A Kalman filter with no control inputs, a linear process model and linear observation model

Note that the structure is cheap to create, storing only references to the state transition model and the observation model. (The system state is passed as an argument to methods like Self::step.) Given the lifetime bound of this struct, a useful strategy to avoid requiring lifetime annotations is to construct it just before Self::step and then dropping it immediately afterward.

Implementations§

source§

impl<'a, R, SS, OS> KalmanFilterNoControl<'a, R, SS, OS>
where R: RealField, SS: DimName, OS: DimName + DimMin<OS, Output = OS>, DefaultAllocator: Allocator<R, SS, SS> + Allocator<R, SS> + Allocator<R, OS, SS> + Allocator<R, SS, OS> + Allocator<R, OS, OS> + Allocator<R, OS> + Allocator<(usize, usize), OS>,

source

pub fn new( transition_model: &'a dyn TransitionModelLinearNoControl<R, SS>, observation_matrix: &'a dyn ObservationModel<R, SS, OS> ) -> Self

Initialize a new KalmanFilterNoControl struct.

The first parameter, transition_model, specifies the state transition model, including the function F and the process covariance Q. The second parameter, observation_matrix, specifies the observation model, including the measurement function H and the measurement covariance R.

source

pub fn step( &self, previous_estimate: &StateAndCovariance<R, SS>, observation: &OVector<R, OS> ) -> Result<StateAndCovariance<R, SS>, Error>

Perform Kalman prediction and update steps with default values

If any component of the observation is NaN (not a number), the observation will not be used but rather the prior will be returned as the posterior without performing the update step.

This calls the prediction step of the transition model and then, if there is a (non-nan) observation, calls the update step of the observation model using the CovarianceUpdateMethod::JosephForm covariance update method.

This is a convenience method that calls step_with_options.

source

pub fn step_with_options( &self, previous_estimate: &StateAndCovariance<R, SS>, observation: &OVector<R, OS>, covariance_update_method: CovarianceUpdateMethod ) -> Result<StateAndCovariance<R, SS>, Error>

Perform Kalman prediction and update steps with default values

If any component of the observation is NaN (not a number), the observation will not be used but rather the prior will be returned as the posterior without performing the update step.

This calls the prediction step of the transition model and then, if there is a (non-nan) observation, calls the update step of the observation model using the specified covariance update method.

source

pub fn filter_inplace( &self, initial_estimate: &StateAndCovariance<R, SS>, observations: &[OVector<R, OS>], state_estimates: &mut [StateAndCovariance<R, SS>] ) -> Result<(), Error>

Kalman filter (operates on in-place data without allocating)

Operates on entire time series (by repeatedly calling step for each observation) and returns a vector of state estimates. To be mathematically correct, the interval between observations must be the dt specified in the motion model.

If any observation has a NaN component, it is treated as missing.

source

pub fn filter( &self, initial_estimate: &StateAndCovariance<R, SS>, observations: &[OVector<R, OS>] ) -> Result<Vec<StateAndCovariance<R, SS>>, Error>

Kalman filter

This is a convenience function that calls filter_inplace.

source

pub fn smooth( &self, initial_estimate: &StateAndCovariance<R, SS>, observations: &[OVector<R, OS>] ) -> Result<Vec<StateAndCovariance<R, SS>>, Error>

Rauch-Tung-Striebel (RTS) smoother

Operates on entire time series (by calling filter then smooth_from_filtered) and returns a vector of state estimates. To be mathematically correct, the interval between observations must be the dt specified in the motion model. Operates on entire time series in one shot and returns a vector of state estimates. To be mathematically correct, the interval between observations must be the dt specified in the motion model.

If any observation has a NaN component, it is treated as missing.

source

pub fn smooth_from_filtered( &self, forward_results: Vec<StateAndCovariance<R, SS>> ) -> Result<Vec<StateAndCovariance<R, SS>>, Error>

Rauch-Tung-Striebel (RTS) smoother using already Kalman filtered estimates

Operates on entire time series in one shot and returns a vector of state estimates. To be mathematically correct, the interval between observations must be the dt specified in the motion model.

Auto Trait Implementations§

§

impl<'a, R, SS, OS> Freeze for KalmanFilterNoControl<'a, R, SS, OS>

§

impl<'a, R, SS, OS> !RefUnwindSafe for KalmanFilterNoControl<'a, R, SS, OS>

§

impl<'a, R, SS, OS> !Send for KalmanFilterNoControl<'a, R, SS, OS>

§

impl<'a, R, SS, OS> !Sync for KalmanFilterNoControl<'a, R, SS, OS>

§

impl<'a, R, SS, OS> Unpin for KalmanFilterNoControl<'a, R, SS, OS>

§

impl<'a, R, SS, OS> !UnwindSafe for KalmanFilterNoControl<'a, R, SS, OS>

Blanket Implementations§

source§

impl<T> Any for T
where T: 'static + ?Sized,

source§

fn type_id(&self) -> TypeId

Gets the TypeId of self. Read more
source§

impl<T> Borrow<T> for T
where T: ?Sized,

source§

fn borrow(&self) -> &T

Immutably borrows from an owned value. Read more
source§

impl<T> BorrowMut<T> for T
where T: ?Sized,

source§

fn borrow_mut(&mut self) -> &mut T

Mutably borrows from an owned value. Read more
source§

impl<T> From<T> for T

source§

fn from(t: T) -> T

Returns the argument unchanged.

source§

impl<T, U> Into<U> for T
where U: From<T>,

source§

fn into(self) -> U

Calls U::from(self).

That is, this conversion is whatever the implementation of From<T> for U chooses to do.

source§

impl<T> Same for T

§

type Output = T

Should always be Self
source§

impl<SS, SP> SupersetOf<SS> for SP
where SS: SubsetOf<SP>,

source§

fn to_subset(&self) -> Option<SS>

The inverse inclusion map: attempts to construct self from the equivalent element of its superset. Read more
source§

fn is_in_subset(&self) -> bool

Checks if self is actually part of its subset T (and can be converted to it).
source§

fn to_subset_unchecked(&self) -> SS

Use with care! Same as self.to_subset but without any property checks. Always succeeds.
source§

fn from_subset(element: &SS) -> SP

The inclusion map: converts self to the equivalent element of its superset.
source§

impl<T, U> TryFrom<U> for T
where U: Into<T>,

§

type Error = Infallible

The type returned in the event of a conversion error.
source§

fn try_from(value: U) -> Result<T, <T as TryFrom<U>>::Error>

Performs the conversion.
source§

impl<T, U> TryInto<U> for T
where U: TryFrom<T>,

§

type Error = <U as TryFrom<T>>::Error

The type returned in the event of a conversion error.
source§

fn try_into(self) -> Result<U, <U as TryFrom<T>>::Error>

Performs the conversion.