diff --git a/src/lib.rs b/src/lib.rs index 7ab50b4..f7afd22 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -24,7 +24,9 @@ #![allow(non_snake_case)] #[cfg(feature = "std")] use log::trace; -use na::{OMatrix, OVector}; +use na::{Matrix, Vector}; +use na::{RawStorageMut, Storage, StorageMut}; + use nalgebra as na; use nalgebra::base::dimension::DimMin; @@ -81,52 +83,37 @@ mod state_and_covariance; pub use state_and_covariance::StateAndCovariance; /// A linear model of process dynamics with no control inputs -pub trait TransitionModelLinearNoControl +pub trait TransitionModelLinearNoControl where R: RealField, SS: DimName, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + S1: Clone + Storage + StorageMut, + S2: Clone + Storage, { /// Get the state transition model, `F`. - fn F(&self) -> &OMatrix; + fn F(&self) -> &Matrix; /// Get the transpose of the state transition model, `FT`. - fn FT(&self) -> &OMatrix; + fn FT(&self) -> &Matrix; /// Get the process covariance, `Q`. - fn Q(&self) -> &OMatrix; + fn Q(&self) -> &Matrix; /// Predict new state from previous estimate. - fn predict(&self, previous_estimate: &StateAndCovariance) -> StateAndCovariance { + fn predict( + &self, + previous_estimate: &StateAndCovariance, + ) -> StateAndCovariance { // The prior. let P = previous_estimate.state(); let F = self.F(); - let state = F * P; + let mut state = P.clone(); // allocate output + F.mul_to(P, &mut state); let covariance = ((F * previous_estimate.covariance()) * self.FT()) + self.Q(); StateAndCovariance::new(state, covariance) } - - /// Get the state transition model, `F`. - #[deprecated(since = "0.8.0", note = "Please use the F function instead")] - #[inline] - fn transition_model(&self) -> &OMatrix { - self.F() - } - - /// Get the transpose of the state transition model, `FT`. - #[deprecated(since = "0.8.0", note = "Please use the FT function instead")] - #[inline] - fn transition_model_transpose(&self) -> &OMatrix { - self.FT() - } - - /// Get the transition noise covariance. - #[deprecated(since = "0.8.0", note = "Please use the Q function instead")] - #[inline] - fn transition_noise_covariance(&self) -> &OMatrix { - self.Q() - } } /// An observation model, potentially non-linear. @@ -136,7 +123,7 @@ where /// as the basis for a `ObservationModel` implementation. This would be done /// every timestep. For an example, see /// [`nonlinear_observation.rs`](https://github.com/strawlab/adskalman-rs/blob/main/examples/src/bin/nonlinear_observation.rs). -pub trait ObservationModel +pub trait ObservationModel where R: RealField, SS: DimName, @@ -148,6 +135,10 @@ where DefaultAllocator: Allocator, DefaultAllocator: Allocator, DefaultAllocator: Allocator<(usize, usize), OS>, + // >::Buffer = S2, + S1: Clone + Storage + Storage, + S2: Clone + Storage + Storage + Storage + Storage, + Matrix: One, { /// For a given state, predict the observation. /// @@ -162,29 +153,29 @@ where /// this trait and must be evaluated for a state for which no observation is /// possible.) Observations with NaN values are treated as missing /// observations. - fn predict_observation(&self, state: &OVector) -> OVector { + fn predict_observation(&self, state: &Vector) -> Vector { self.H() * state } /// Get the observation matrix, `H`. - fn H(&self) -> &OMatrix; + fn H(&self) -> &Matrix; /// Get the transpose of the observation matrix, `HT`. - fn HT(&self) -> &OMatrix; + fn HT(&self) -> &Matrix; /// Get the observation noise covariance, `R`. // TODO: ensure this is positive definite? - fn R(&self) -> &OMatrix; + fn R(&self) -> &Matrix; /// Given prior state and observation, estimate the posterior state. /// /// This is the *update* step in the Kalman filter literature. fn update( &self, - prior: &StateAndCovariance, - observation: &OVector, + prior: &StateAndCovariance, + observation: &Vector, covariance_method: CovarianceUpdateMethod, - ) -> Result, Error> { + ) -> Result, Error> { let h = self.H(); trace!("h {}", pretty_print!(h)); @@ -217,28 +208,28 @@ where return Err(ErrorKind::CovarianceNotPositiveSemiDefinite.into()); } }; - let s_inv: OMatrix = s_chol.inverse(); + let s_inv: Matrix = s_chol.inverse(); trace!("s_inv {}", pretty_print!(s_inv)); - let k_gain: OMatrix = p * ht * s_inv; + let k_gain: Matrix = p * ht * s_inv; // let k_gain: OMatrix = solve!( (p*ht), s ); trace!("k_gain {}", pretty_print!(k_gain)); - let predicted: OVector = self.predict_observation(prior.state()); + let predicted: Vector = self.predict_observation(prior.state()); trace!("predicted {}", pretty_print!(predicted)); trace!("observation {}", pretty_print!(observation)); - let innovation: OVector = observation - predicted; + let innovation: Vector = observation - predicted; trace!("innovation {}", pretty_print!(innovation)); - let state: OVector = prior.state() + &k_gain * innovation; + let state: Vector = prior.state() + &k_gain * innovation; trace!("state {}", pretty_print!(state)); trace!("self.observation_matrix() {}", pretty_print!(self.H())); - let kh: OMatrix = &k_gain * self.H(); + let kh: Matrix = &k_gain * self.H(); trace!("kh {}", pretty_print!(kh)); - let one_minus_kh = OMatrix::::one() - kh; + let one_minus_kh = Matrix::::one() - kh; trace!("one_minus_kh {}", pretty_print!(one_minus_kh)); - let covariance: OMatrix = match covariance_method { + let covariance: Matrix = match covariance_method { CovarianceUpdateMethod::JosephForm => { // Joseph form of covariance update keeps covariance matrix symmetric. @@ -261,43 +252,6 @@ where Ok(StateAndCovariance::new(state, covariance)) } - - /// Get the observation matrix, `H`. - #[deprecated(since = "0.8.0", note = "Please use the H function instead")] - #[inline] - fn observation_matrix(&self) -> &OMatrix { - self.H() - } - - /// Get the transpose of the observation matrix, `HT`. - #[deprecated(since = "0.8.0", note = "Please use the HT function instead")] - #[inline] - fn observation_matrix_transpose(&self) -> &OMatrix { - self.HT() - } - - /// Get the observation noise covariance, `R`. - #[deprecated(since = "0.8.0", note = "Please use the R function instead")] - #[inline] - fn observation_noise_covariance(&self) -> &OMatrix { - self.R() - } - - /// For a given state, predict the observation. - /// - /// If an observation is not possible, this returns NaN values. (This - /// happens, for example, when a non-linear observation model implements - /// this trait and must be evaluated for a state for which no observation is - /// possible.) Observations with NaN values are treated as missing - /// observations. - #[deprecated( - since = "0.8.0", - note = "Please use the predict_observation function instead" - )] - #[inline] - fn evaluate(&self, state: &OVector) -> OVector { - self.predict_observation(state) - } } /// Specifies the approach used for updating the covariance matrix @@ -325,28 +279,30 @@ pub enum CovarianceUpdateMethod { /// 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. -pub struct KalmanFilterNoControl<'a, R, SS, OS> +pub struct KalmanFilterNoControl<'a, R, SS, OS, S1, S2> where R: RealField, SS: DimName, OS: DimName, { - transition_model: &'a dyn TransitionModelLinearNoControl, - observation_matrix: &'a dyn ObservationModel, + transition_model: &'a dyn TransitionModelLinearNoControl, + observation_matrix: &'a dyn ObservationModel, } -impl<'a, R, SS, OS> KalmanFilterNoControl<'a, R, SS, OS> +impl<'a, R, SS, OS, S1, S2> KalmanFilterNoControl<'a, R, SS, OS, S1, S2> where R: RealField, SS: DimName, OS: DimName + DimMin, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator<(usize, usize), OS>, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator, + // DefaultAllocator: Allocator<(usize, usize), OS>, + S1: Clone + Storage + Storage + RawStorageMut, + S2: Clone + Storage + Storage + Storage + Storage, { /// Initialize a new `KalmanFilterNoControl` struct. /// @@ -356,8 +312,8 @@ where /// including the measurement function `H` and the measurement covariance /// `R`. pub fn new( - transition_model: &'a dyn TransitionModelLinearNoControl, - observation_matrix: &'a dyn ObservationModel, + transition_model: &'a dyn TransitionModelLinearNoControl, + observation_matrix: &'a dyn ObservationModel, ) -> Self { Self { transition_model, @@ -380,9 +336,9 @@ where /// [step_with_options](struct.KalmanFilterNoControl.html#method.step_with_options). pub fn step( &self, - previous_estimate: &StateAndCovariance, - observation: &OVector, - ) -> Result, Error> { + previous_estimate: &StateAndCovariance, + observation: &Vector, + ) -> Result, Error> { self.step_with_options( previous_estimate, observation, @@ -401,10 +357,10 @@ where /// observation model using the specified covariance update method. pub fn step_with_options( &self, - previous_estimate: &StateAndCovariance, - observation: &OVector, + previous_estimate: &StateAndCovariance, + observation: &Vector, covariance_update_method: CovarianceUpdateMethod, - ) -> Result, Error> { + ) -> Result, Error> { let prior = self.transition_model.predict(previous_estimate); if observation.iter().any(|x| is_nan(x.clone())) { Ok(prior) @@ -425,9 +381,9 @@ where /// If any observation has a NaN component, it is treated as missing. pub fn filter_inplace( &self, - initial_estimate: &StateAndCovariance, - observations: &[OVector], - state_estimates: &mut [StateAndCovariance], + initial_estimate: &StateAndCovariance, + observations: &[Vector], + state_estimates: &mut [StateAndCovariance], ) -> Result<(), Error> { let mut previous_estimate = initial_estimate.clone(); assert!(state_estimates.len() >= observations.len()); @@ -448,11 +404,12 @@ where #[cfg(feature = "std")] pub fn filter( &self, - initial_estimate: &StateAndCovariance, - observations: &[OVector], - ) -> Result>, Error> { + initial_estimate: &StateAndCovariance, + observations: &[Vector], + ) -> Result>, Error> { let mut state_estimates = Vec::with_capacity(observations.len()); - let empty = StateAndCovariance::new(na::zero(), na::OMatrix::::identity()); + let empty: StateAndCovariance = + StateAndCovariance::new(S1::zero(), S2::identity()); for _ in 0..observations.len() { state_estimates.push(empty.clone()); } @@ -477,9 +434,9 @@ where #[cfg(feature = "std")] pub fn smooth( &self, - initial_estimate: &StateAndCovariance, - observations: &[OVector], - ) -> Result>, Error> { + initial_estimate: &StateAndCovariance, + observations: &[Vector], + ) -> Result>, Error> { let forward_results = self.filter(initial_estimate, observations)?; self.smooth_from_filtered(forward_results) } @@ -492,8 +449,8 @@ where #[cfg(feature = "std")] pub fn smooth_from_filtered( &self, - mut forward_results: Vec>, - ) -> Result>, Error> { + mut forward_results: Vec>, + ) -> Result>, Error> { forward_results.reverse(); let mut smoothed_backwards = Vec::with_capacity(forward_results.len()); @@ -512,9 +469,9 @@ where #[cfg(feature = "std")] fn smooth_step( &self, - smooth_future: &StateAndCovariance, - filt: &StateAndCovariance, - ) -> Result, Error> { + smooth_future: &StateAndCovariance, + filt: &StateAndCovariance, + ) -> Result, Error> { let prior = self.transition_model.predict(filt); let v_chol = match na::linalg::Cholesky::new(prior.covariance().clone()) { @@ -523,7 +480,7 @@ where return Err(ErrorKind::CovarianceNotPositiveSemiDefinite.into()); } }; - let inv_prior_covariance: OMatrix = v_chol.inverse(); + let inv_prior_covariance: Matrix = v_chol.inverse(); trace!( "inv_prior_covariance {}", pretty_print!(inv_prior_covariance) diff --git a/src/state_and_covariance.rs b/src/state_and_covariance.rs index b8aadfe..2fccac3 100644 --- a/src/state_and_covariance.rs +++ b/src/state_and_covariance.rs @@ -1,33 +1,34 @@ -use na::allocator::Allocator; -use na::{DefaultAllocator, DimName, RealField}; -use na::{OMatrix, OVector}; +use na::base::dimension::U1; +use na::base::storage::Owned; +use na::{DimName, RealField}; +use na::{Matrix, Vector}; use nalgebra as na; /// State and covariance pair for a given estimate #[derive(Debug, Clone)] -pub struct StateAndCovariance +pub struct StateAndCovariance, S2 = Owned> where R: RealField, SS: DimName, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + S1: Clone, + S2: Clone, { - state: OVector, - covariance: OMatrix, + state: Vector, + covariance: Matrix, } -impl StateAndCovariance +impl StateAndCovariance where R: RealField, SS: DimName, - DefaultAllocator: Allocator, - DefaultAllocator: Allocator, + S1: Clone, + S2: Clone, { /// Create a new `StateAndCovariance`. /// /// It is assumed that the covariance matrix is symmetric and positive /// semi-definite. - pub fn new(state: OVector, covariance: OMatrix) -> Self { + pub fn new(state: Vector, covariance: Matrix) -> Self { // In theory, checks could be run to ensure the covariance matrix is // both symmetric and positive semi-definite. The Cholesky decomposition // could be used to test if it is positive definite. However, matrices @@ -43,27 +44,27 @@ where } /// Get a reference to the state vector. #[inline] - pub fn state(&self) -> &OVector { + pub fn state(&self) -> &Vector { &self.state } /// Get a mut reference to the state vector. #[inline] - pub fn state_mut(&mut self) -> &mut OVector { + pub fn state_mut(&mut self) -> &mut Vector { &mut self.state } /// Get a reference to the covariance matrix. #[inline] - pub fn covariance(&self) -> &OMatrix { + pub fn covariance(&self) -> &Matrix { &self.covariance } /// Get a mutable reference to the covariance matrix. #[inline] - pub fn covariance_mut(&mut self) -> &mut OMatrix { + pub fn covariance_mut(&mut self) -> &mut Matrix { &mut self.covariance } /// Get the state vector and covariance matrix. #[inline] - pub fn inner(self) -> (OVector, OMatrix) { + pub fn inner(self) -> (Vector, Matrix) { (self.state, self.covariance) } }