diff --git a/crates/minikalman/benches/gravity.rs b/crates/minikalman/benches/gravity.rs index 3643332..4a50162 100644 --- a/crates/minikalman/benches/gravity.rs +++ b/crates/minikalman/benches/gravity.rs @@ -26,6 +26,7 @@ fn criterion_benchmark(c: &mut Criterion) { let mut gravity_x = BufferBuilder::state_vector_x::().new(); let mut gravity_A = BufferBuilder::system_matrix_A::().new(); let mut gravity_P = BufferBuilder::estimate_covariance_P::().new(); + let mut gravity_Q = BufferBuilder::direct_process_noise_covariance_Q::().new(); // Control buffers. // let mut gravity_u = BufferBuilder::control_vector_u::().new(); @@ -56,6 +57,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); @@ -100,6 +102,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); @@ -138,6 +141,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); diff --git a/crates/minikalman/benches/gravity_fixed.rs b/crates/minikalman/benches/gravity_fixed.rs index 4c3d946..fe32e54 100644 --- a/crates/minikalman/benches/gravity_fixed.rs +++ b/crates/minikalman/benches/gravity_fixed.rs @@ -65,6 +65,7 @@ fn criterion_benchmark(c: &mut Criterion) { let mut gravity_x = BufferBuilder::state_vector_x::().new(); let mut gravity_A = BufferBuilder::system_matrix_A::().new(); let mut gravity_P = BufferBuilder::estimate_covariance_P::().new(); + let mut gravity_Q = BufferBuilder::direct_process_noise_covariance_Q::().new(); // Observation buffers. let mut gravity_z = BufferBuilder::measurement_vector_z::().new(); @@ -89,6 +90,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); @@ -133,6 +135,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); @@ -171,6 +174,7 @@ fn criterion_benchmark(c: &mut Criterion) { StateTransitionMatrixMutBuffer::from(gravity_A.as_mut_slice()), StateVectorBuffer::from(gravity_x.as_mut_slice()), EstimateCovarianceMatrixBuffer::from(gravity_P.as_mut_slice()), + DirectProcessNoiseCovarianceMatrixBuffer::from(gravity_Q.as_mut_slice()), PredictedStateEstimateVectorBuffer::from(gravity_temp_x.as_mut_slice()), TemporaryStateMatrixBuffer::from(gravity_temp_P.as_mut_slice()), ); diff --git a/crates/minikalman/examples/gravity.rs b/crates/minikalman/examples/gravity.rs index 266cf08..fb114aa 100644 --- a/crates/minikalman/examples/gravity.rs +++ b/crates/minikalman/examples/gravity.rs @@ -181,7 +181,7 @@ fn initialize_control_matrix(filter: &mut impl ControlMatrixMut, + filter: &mut impl ControlProcessNoiseCovarianceMatrixMut, ) { filter.as_matrix_mut().apply(|mat| { mat[0] = 1.0; // :) diff --git a/crates/minikalman/src/buffers/builder.rs b/crates/minikalman/src/buffers/builder.rs index 5cef5cc..aca8906 100644 --- a/crates/minikalman/src/buffers/builder.rs +++ b/crates/minikalman/src/buffers/builder.rs @@ -24,6 +24,13 @@ impl BufferBuilder { EstimateCovarianceMatrixBufferBuilder } + #[allow(non_snake_case)] + #[doc(alias = "process_noise_covariance_Q")] + pub fn direct_process_noise_covariance_Q( + ) -> DirectProcessNoiseCovarianceMatrixBufferBuilder { + DirectProcessNoiseCovarianceMatrixBufferBuilder + } + pub fn control_vector_u() -> ControlVectorBufferBuilder { ControlVectorBufferBuilder } @@ -36,9 +43,9 @@ impl BufferBuilder { #[allow(non_snake_case)] #[doc(alias = "control_covariance_Q")] - pub fn process_noise_covariance_Q( - ) -> ProcessNoiseCovarianceMatrixBufferBuilder { - ProcessNoiseCovarianceMatrixBufferBuilder + pub fn control_process_noise_covariance_Q( + ) -> ControlProcessNoiseCovarianceMatrixBufferBuilder { + ControlProcessNoiseCovarianceMatrixBufferBuilder } #[doc(alias = "observation_vector_z")] @@ -128,15 +135,18 @@ pub struct StateTransitionMatrixBufferBuilder; #[doc(alias = "SystemCovarianceMatrixBufferBuilder")] pub struct EstimateCovarianceMatrixBufferBuilder; +/// A builder for direct process noise covariance matrices (`num_states` × `num_states`). +pub struct DirectProcessNoiseCovarianceMatrixBufferBuilder; + /// A builder for control vectors (`num_controls` × `1`). pub struct ControlVectorBufferBuilder; /// A builder for control transition matrices (`num_states` × `num_controls`). pub struct ControlTransitionMatrixBufferBuilder; -/// A builder for control covariance matrices (`num_controls` × `num_controls`). +/// A builder for control process noise covariance matrices (`num_controls` × `num_controls`). #[doc(alias = "ControlCovarianceMatrixBufferBuilder")] -pub struct ProcessNoiseCovarianceMatrixBufferBuilder; +pub struct ControlProcessNoiseCovarianceMatrixBufferBuilder; /// A builder for measurement vectors (`num_measurements` × `1`). pub struct ObservationVectorBufferBuilder; @@ -455,22 +465,96 @@ impl } } -/// The type of owned control matrix buffers. +/// The type of owned direct process noise matrix buffers. #[cfg_attr(docsrs, doc(cfg(feature = "alloc")))] -pub type ControlCovarianceMatrixBufferOwnedType = - ProcessNoiseCovarianceMatrixMutBuffer>; +pub type DirectProcessNoiseCovarianceMatrixBufferOwnedType = + DirectProcessNoiseCovarianceMatrixMutBuffer>; -impl ProcessNoiseCovarianceMatrixBufferBuilder { - /// Builds a new [`ProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. +impl DirectProcessNoiseCovarianceMatrixBufferBuilder { + /// Builds a new [`DirectProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. /// /// ## Example /// ``` - /// use minikalman::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer; + /// use minikalman::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer; /// use minikalman::prelude::*; /// - /// let buffer = BufferBuilder::process_noise_covariance_Q::<2>().new(); + /// let buffer = BufferBuilder::direct_process_noise_covariance_Q::<3>().new(); /// - /// let buffer: ProcessNoiseCovarianceMatrixMutBuffer<2, f32, _> = buffer; + /// let buffer: DirectProcessNoiseCovarianceMatrixMutBuffer<3, f32, _> = buffer; + /// assert_eq!(buffer.len(), 9); + /// ``` + #[allow(clippy::new_ret_no_self, clippy::wrong_self_convention)] + #[cfg_attr(docsrs, doc(cfg(feature = "alloc")))] + #[cfg(feature = "alloc")] + pub fn new( + &self, + ) -> DirectProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataBoxed, + > + where + T: Copy + Default, + { + self.new_with(T::default()) + } + + /// Builds a new [`DirectProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. + /// + /// ## Example + /// ``` + /// use minikalman::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer; + /// use minikalman::prelude::*; + /// + /// let buffer = BufferBuilder::direct_process_noise_covariance_Q::<3>().new_with(0.0); + /// + /// let buffer: DirectProcessNoiseCovarianceMatrixMutBuffer<3, f32, _> = buffer; + /// assert_eq!(buffer.len(), 9); + /// ``` + #[allow(clippy::new_ret_no_self, clippy::wrong_self_convention)] + #[cfg_attr(docsrs, doc(cfg(feature = "alloc")))] + #[cfg(feature = "alloc")] + pub fn new_with( + &self, + init: T, + ) -> DirectProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataBoxed, + > + where + T: Copy, + { + DirectProcessNoiseCovarianceMatrixMutBuffer::< + CONTROLS, + T, + MatrixDataBoxed, + >::new(MatrixData::new_boxed::( + alloc::vec![init; CONTROLS * CONTROLS], + )) + } +} + +/// The type of owned control process noise matrix buffers. +#[cfg_attr(docsrs, doc(cfg(feature = "alloc")))] +pub type ControlProcessNoiseCovarianceMatrixBufferOwnedType = + ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataBoxed, + >; + +impl ControlProcessNoiseCovarianceMatrixBufferBuilder { + /// Builds a new [`ControlProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. + /// + /// ## Example + /// ``` + /// use minikalman::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer; + /// use minikalman::prelude::*; + /// + /// let buffer = BufferBuilder::control_process_noise_covariance_Q::<2>().new(); + /// + /// let buffer: ControlProcessNoiseCovarianceMatrixMutBuffer<2, f32, _> = buffer; /// assert_eq!(buffer.len(), 4); /// ``` #[allow(clippy::new_ret_no_self, clippy::wrong_self_convention)] @@ -478,23 +562,27 @@ impl ProcessNoiseCovarianceMatrixBufferBuilder #[cfg(feature = "alloc")] pub fn new( &self, - ) -> ProcessNoiseCovarianceMatrixMutBuffer> + ) -> ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataBoxed, + > where T: Copy + Default, { self.new_with(T::default()) } - /// Builds a new [`ProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. + /// Builds a new [`ControlProcessNoiseCovarianceMatrixMutBuffer`] that owns its data. /// /// ## Example /// ``` - /// use minikalman::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer; + /// use minikalman::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer; /// use minikalman::prelude::*; /// - /// let buffer = BufferBuilder::process_noise_covariance_Q::<2>().new_with(0.0); + /// let buffer = BufferBuilder::control_process_noise_covariance_Q::<2>().new_with(0.0); /// - /// let buffer: ProcessNoiseCovarianceMatrixMutBuffer<2, f32, _> = buffer; + /// let buffer: ControlProcessNoiseCovarianceMatrixMutBuffer<2, f32, _> = buffer; /// assert_eq!(buffer.len(), 4); /// ``` #[allow(clippy::new_ret_no_self, clippy::wrong_self_convention)] @@ -503,13 +591,21 @@ impl ProcessNoiseCovarianceMatrixBufferBuilder pub fn new_with( &self, init: T, - ) -> ProcessNoiseCovarianceMatrixMutBuffer> + ) -> ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataBoxed, + > where T: Copy, { - ProcessNoiseCovarianceMatrixMutBuffer::>::new( - MatrixData::new_boxed::(alloc::vec![init; CONTROLS * CONTROLS]), - ) + ControlProcessNoiseCovarianceMatrixMutBuffer::< + CONTROLS, + T, + MatrixDataBoxed, + >::new(MatrixData::new_boxed::( + alloc::vec![init; CONTROLS * CONTROLS], + )) } } diff --git a/crates/minikalman/src/buffers/types.rs b/crates/minikalman/src/buffers/types.rs index bf5b5a1..667cfc4 100644 --- a/crates/minikalman/src/buffers/types.rs +++ b/crates/minikalman/src/buffers/types.rs @@ -1,5 +1,7 @@ mod control_matrix_buffer; +mod control_process_noise_covariance_buffer; mod control_vector_buffer; +mod direct_process_noise_covariance_buffer; mod estimate_covariance_matrix_buffer; mod innovation_covariance_matrix_buffer; mod innovation_vector_buffer; @@ -7,7 +9,6 @@ mod kalman_gain_matrix_buffer; mod measurement_noise_covariance_matrix_buffer; mod measurement_vector_buffer; mod observation_matrix_buffer; -mod process_noise_covariance_buffer; mod state_prediction_vector_buffer; mod state_transition_matrix_buffer; mod state_vector_buffer; @@ -19,7 +20,9 @@ mod temporary_residual_covariance_inverted_matrix_buffer; mod temporary_state_matrix_buffer; pub use crate::buffers::types::control_matrix_buffer::*; +pub use crate::buffers::types::control_process_noise_covariance_buffer::*; pub use crate::buffers::types::control_vector_buffer::*; +pub use crate::buffers::types::direct_process_noise_covariance_buffer::*; pub use crate::buffers::types::estimate_covariance_matrix_buffer::*; pub use crate::buffers::types::innovation_covariance_matrix_buffer::*; pub use crate::buffers::types::innovation_vector_buffer::*; @@ -27,7 +30,6 @@ pub use crate::buffers::types::kalman_gain_matrix_buffer::*; pub use crate::buffers::types::measurement_noise_covariance_matrix_buffer::*; pub use crate::buffers::types::measurement_vector_buffer::*; pub use crate::buffers::types::observation_matrix_buffer::*; -pub use crate::buffers::types::process_noise_covariance_buffer::*; pub use crate::buffers::types::state_prediction_vector_buffer::*; pub use crate::buffers::types::state_transition_matrix_buffer::*; pub use crate::buffers::types::state_vector_buffer::*; diff --git a/crates/minikalman/src/buffers/types/process_noise_covariance_buffer.rs b/crates/minikalman/src/buffers/types/control_process_noise_covariance_buffer.rs similarity index 64% rename from crates/minikalman/src/buffers/types/process_noise_covariance_buffer.rs rename to crates/minikalman/src/buffers/types/control_process_noise_covariance_buffer.rs index 242147d..f961df0 100644 --- a/crates/minikalman/src/buffers/types/process_noise_covariance_buffer.rs +++ b/crates/minikalman/src/buffers/types/control_process_noise_covariance_buffer.rs @@ -1,55 +1,71 @@ use core::marker::PhantomData; use core::ops::{Index, IndexMut}; -use crate::kalman::{ProcessNoiseCovarianceMatrix, ProcessNoiseCovarianceMatrixMut}; +use crate::kalman::{ControlProcessNoiseCovarianceMatrix, ControlProcessNoiseCovarianceMatrixMut}; use crate::matrix::{IntoInnerData, MatrixData, MatrixDataArray, MatrixDataMut, MatrixDataRef}; use crate::matrix::{Matrix, MatrixMut}; use crate::prelude::{RowMajorSequentialData, RowMajorSequentialDataMut}; -/// Immutable buffer for the control / process noise covariance matrix (`num_controls` × `num_controls`). +/// Immutable buffer for the control process noise covariance matrix (`num_controls` × `num_controls`). /// -/// Represents the uncertainty in the state transition process. +/// This matrix represents the control process noise covariance. It quantifies the +/// uncertainty introduced by the control inputs, reflecting how much the true state +/// is expected to deviate from the predicted state due to noise and variations +/// in the control process. The matrix is calculated as B×Q×Bᵀ, where B +/// represents the control input model, and Q is the process noise covariance (this matrix). /// /// ## Example /// ``` -/// use minikalman::buffers::types::ProcessNoiseCovarianceMatrixBuffer; +/// use minikalman::buffers::types::ControlProcessNoiseCovarianceMatrixBuffer; /// use minikalman::prelude::*; /// /// // From owned data -/// let buffer = ProcessNoiseCovarianceMatrixBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); +/// let buffer = ControlProcessNoiseCovarianceMatrixBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); /// /// // From a reference /// let data = [0.0; 4]; -/// let buffer = ProcessNoiseCovarianceMatrixBuffer::<2, f32, _>::from(data.as_ref()); +/// let buffer = ControlProcessNoiseCovarianceMatrixBuffer::<2, f32, _>::from(data.as_ref()); /// ``` #[doc(alias = "ControlCovarianceMatrixBuffer")] -pub struct ProcessNoiseCovarianceMatrixBuffer(M, PhantomData) +pub struct ControlProcessNoiseCovarianceMatrixBuffer( + M, + PhantomData, +) where M: Matrix; -/// Mutable buffer for the control / process noise covariance matrix (`num_controls` × `num_controls`). +/// Mutable buffer for the control process noise covariance matrix (`num_controls` × `num_controls`). +/// +/// This matrix represents the control process noise covariance. It quantifies the +/// uncertainty introduced by the control inputs, reflecting how much the true state +/// is expected to deviate from the predicted state due to noise and variations +/// in the control process. The matrix is calculated as B×Q×Bᵀ, where B +/// represents the control input model, and Q is the process noise covariance (this matrix). /// /// ## Example /// ``` -/// use minikalman::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer; +/// use minikalman::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer; /// use minikalman::prelude::*; /// /// // From owned data -/// let buffer = ProcessNoiseCovarianceMatrixMutBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); +/// let buffer = ControlProcessNoiseCovarianceMatrixMutBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); /// /// // From a reference /// let mut data = [0.0; 4]; -/// let buffer = ProcessNoiseCovarianceMatrixMutBuffer::<2, f32, _>::from(data.as_mut_slice()); +/// let buffer = ControlProcessNoiseCovarianceMatrixMutBuffer::<2, f32, _>::from(data.as_mut_slice()); /// ``` #[doc(alias = "ControlCovarianceMatrixMutBuffer")] -pub struct ProcessNoiseCovarianceMatrixMutBuffer(M, PhantomData) +pub struct ControlProcessNoiseCovarianceMatrixMutBuffer( + M, + PhantomData, +) where M: MatrixMut; // ----------------------------------------------------------- impl From<[T; TOTAL]> - for ProcessNoiseCovarianceMatrixBuffer< + for ControlProcessNoiseCovarianceMatrixBuffer< CONTROLS, T, MatrixDataArray, @@ -65,7 +81,11 @@ impl From<[T; TOTAL]> } impl<'a, const CONTROLS: usize, T> From<&'a [T]> - for ProcessNoiseCovarianceMatrixBuffer> + for ControlProcessNoiseCovarianceMatrixBuffer< + CONTROLS, + T, + MatrixDataRef<'a, CONTROLS, CONTROLS, T>, + > { fn from(value: &'a [T]) -> Self { #[cfg(not(feature = "no_assert"))] @@ -77,7 +97,11 @@ impl<'a, const CONTROLS: usize, T> From<&'a [T]> } impl<'a, const CONTROLS: usize, T> From<&'a mut [T]> - for ProcessNoiseCovarianceMatrixBuffer> + for ControlProcessNoiseCovarianceMatrixBuffer< + CONTROLS, + T, + MatrixDataRef<'a, CONTROLS, CONTROLS, T>, + > { fn from(value: &'a mut [T]) -> Self { #[cfg(not(feature = "no_assert"))] @@ -89,7 +113,11 @@ impl<'a, const CONTROLS: usize, T> From<&'a mut [T]> } impl<'a, const CONTROLS: usize, T> From<&'a mut [T]> - for ProcessNoiseCovarianceMatrixMutBuffer> + for ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataMut<'a, CONTROLS, CONTROLS, T>, + > { fn from(value: &'a mut [T]) -> Self { #[cfg(not(feature = "no_assert"))] @@ -101,7 +129,7 @@ impl<'a, const CONTROLS: usize, T> From<&'a mut [T]> } impl From<[T; TOTAL]> - for ProcessNoiseCovarianceMatrixMutBuffer< + for ControlProcessNoiseCovarianceMatrixMutBuffer< CONTROLS, T, MatrixDataArray, @@ -118,7 +146,7 @@ impl From<[T; TOTAL]> // ----------------------------------------------------------- -impl ProcessNoiseCovarianceMatrixBuffer +impl ControlProcessNoiseCovarianceMatrixBuffer where M: Matrix, { @@ -136,7 +164,7 @@ where } impl RowMajorSequentialData - for ProcessNoiseCovarianceMatrixBuffer + for ControlProcessNoiseCovarianceMatrixBuffer where M: Matrix, { @@ -147,14 +175,14 @@ where } impl Matrix - for ProcessNoiseCovarianceMatrixBuffer + for ControlProcessNoiseCovarianceMatrixBuffer where M: Matrix, { } -impl ProcessNoiseCovarianceMatrix - for ProcessNoiseCovarianceMatrixBuffer +impl ControlProcessNoiseCovarianceMatrix + for ControlProcessNoiseCovarianceMatrixBuffer where M: Matrix, { @@ -165,7 +193,7 @@ where } } -impl ProcessNoiseCovarianceMatrixMutBuffer +impl ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -188,7 +216,7 @@ where } impl RowMajorSequentialData - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -199,7 +227,7 @@ where } impl RowMajorSequentialDataMut - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -210,21 +238,21 @@ where } impl Matrix - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { } impl MatrixMut - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { } -impl ProcessNoiseCovarianceMatrix - for ProcessNoiseCovarianceMatrixMutBuffer +impl ControlProcessNoiseCovarianceMatrix + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -235,8 +263,8 @@ where } } -impl ProcessNoiseCovarianceMatrixMut - for ProcessNoiseCovarianceMatrixMutBuffer +impl ControlProcessNoiseCovarianceMatrixMut + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -248,7 +276,7 @@ where } impl Index - for ProcessNoiseCovarianceMatrixBuffer + for ControlProcessNoiseCovarianceMatrixBuffer where M: Matrix, { @@ -260,7 +288,7 @@ where } impl Index - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -272,7 +300,7 @@ where } impl IndexMut - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut, { @@ -284,7 +312,7 @@ where // ----------------------------------------------------------- impl IntoInnerData - for ProcessNoiseCovarianceMatrixBuffer + for ControlProcessNoiseCovarianceMatrixBuffer where M: MatrixMut + IntoInnerData, { @@ -296,7 +324,7 @@ where } impl IntoInnerData - for ProcessNoiseCovarianceMatrixMutBuffer + for ControlProcessNoiseCovarianceMatrixMutBuffer where M: MatrixMut + IntoInnerData, { @@ -313,7 +341,7 @@ mod tests { #[test] fn test_from_array() { - let value: ProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 100].into(); + let value: ControlProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 100].into(); assert_eq!(value.len(), 25); assert!(!value.is_empty()); assert!(value.is_valid()); @@ -322,7 +350,7 @@ mod tests { #[test] fn test_from_ref() { let data = [0.0_f32; 100]; - let value: ProcessNoiseCovarianceMatrixBuffer<5, f32, _> = data.as_ref().into(); + let value: ControlProcessNoiseCovarianceMatrixBuffer<5, f32, _> = data.as_ref().into(); assert_eq!(value.len(), 25); assert!(value.is_valid()); assert!(!value.is_empty()); @@ -332,7 +360,8 @@ mod tests { #[test] fn test_from_mut() { let mut data = [0.0_f32; 100]; - let value: ProcessNoiseCovarianceMatrixBuffer<5, f32, _> = data.as_mut_slice().into(); + let value: ControlProcessNoiseCovarianceMatrixBuffer<5, f32, _> = + data.as_mut_slice().into(); assert_eq!(value.len(), 25); assert!(value.is_valid()); assert!(!value.is_empty()); @@ -342,13 +371,13 @@ mod tests { #[test] #[cfg(feature = "no_assert")] fn test_from_array_invalid_size() { - let value: ProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 1].into(); + let value: ControlProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 1].into(); assert!(!value.is_valid()); } #[test] fn test_mut_from_array() { - let value: ProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 100].into(); + let value: ControlProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 100].into(); assert_eq!(value.len(), 25); assert!(!value.is_empty()); assert!(value.is_valid()); @@ -357,7 +386,8 @@ mod tests { #[test] fn test_mut_from_mut() { let mut data = [0.0_f32; 100]; - let value: ProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = data.as_mut_slice().into(); + let value: ControlProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = + data.as_mut_slice().into(); assert_eq!(value.len(), 25); assert!(!value.is_empty()); assert!(value.is_valid()); @@ -367,14 +397,14 @@ mod tests { #[test] #[cfg(feature = "no_assert")] fn test_mut_from_array_invalid_size() { - let value: ProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 1].into(); + let value: ControlProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 1].into(); assert!(!value.is_valid()); } #[test] #[rustfmt::skip] fn test_access() { - let mut value: ProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 25].into(); + let mut value: ControlProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 25].into(); // Set values. { diff --git a/crates/minikalman/src/buffers/types/direct_process_noise_covariance_buffer.rs b/crates/minikalman/src/buffers/types/direct_process_noise_covariance_buffer.rs new file mode 100644 index 0000000..1c9d13c --- /dev/null +++ b/crates/minikalman/src/buffers/types/direct_process_noise_covariance_buffer.rs @@ -0,0 +1,413 @@ +use core::marker::PhantomData; +use core::ops::{Index, IndexMut}; + +use crate::kalman::{DirectProcessNoiseCovarianceMatrix, DirectProcessNoiseCovarianceMatrixMut}; +use crate::matrix::{IntoInnerData, MatrixData, MatrixDataArray, MatrixDataMut, MatrixDataRef}; +use crate::matrix::{Matrix, MatrixMut}; +use crate::prelude::{RowMajorSequentialData, RowMajorSequentialDataMut}; + +/// Immutable buffer for the direct process noise covariance matrix (`num_states` × `num_states`). +/// +/// Represents the uncertainty in the state transition process. +/// +/// ## Example +/// ``` +/// use minikalman::buffers::types::DirectProcessNoiseCovarianceMatrixBuffer; +/// use minikalman::prelude::*; +/// +/// // From owned data +/// let buffer = DirectProcessNoiseCovarianceMatrixBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); +/// +/// // From a reference +/// let data = [0.0; 4]; +/// let buffer = DirectProcessNoiseCovarianceMatrixBuffer::<2, f32, _>::from(data.as_ref()); +/// ``` +pub struct DirectProcessNoiseCovarianceMatrixBuffer(M, PhantomData) +where + M: Matrix; + +/// Mutable buffer for the direct process noise covariance matrix (`num_states` × `num_states`). +/// +/// ## Example +/// ``` +/// use minikalman::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer; +/// use minikalman::prelude::*; +/// +/// // From owned data +/// let buffer = DirectProcessNoiseCovarianceMatrixMutBuffer::new(MatrixData::new_array::<2, 2, 4, f32>([0.0; 4])); +/// +/// // From a reference +/// let mut data = [0.0; 4]; +/// let buffer = DirectProcessNoiseCovarianceMatrixMutBuffer::<2, f32, _>::from(data.as_mut_slice()); +/// ``` +pub struct DirectProcessNoiseCovarianceMatrixMutBuffer( + M, + PhantomData, +) +where + M: MatrixMut; + +// ----------------------------------------------------------- + +impl From<[T; TOTAL]> + for DirectProcessNoiseCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + > +{ + fn from(value: [T; TOTAL]) -> Self { + #[cfg(not(feature = "no_assert"))] + { + debug_assert!(STATES * STATES <= TOTAL); + } + Self::new(MatrixData::new_array::(value)) + } +} + +impl<'a, const STATES: usize, T> From<&'a [T]> + for DirectProcessNoiseCovarianceMatrixBuffer> +{ + fn from(value: &'a [T]) -> Self { + #[cfg(not(feature = "no_assert"))] + { + debug_assert!(STATES * STATES <= value.len()); + } + Self::new(MatrixData::new_ref::(value)) + } +} + +impl<'a, const STATES: usize, T> From<&'a mut [T]> + for DirectProcessNoiseCovarianceMatrixBuffer> +{ + fn from(value: &'a mut [T]) -> Self { + #[cfg(not(feature = "no_assert"))] + { + debug_assert!(STATES * STATES <= value.len()); + } + Self::new(MatrixData::new_ref::(value)) + } +} + +impl<'a, const STATES: usize, T> From<&'a mut [T]> + for DirectProcessNoiseCovarianceMatrixMutBuffer> +{ + fn from(value: &'a mut [T]) -> Self { + #[cfg(not(feature = "no_assert"))] + { + debug_assert!(STATES * STATES <= value.len()); + } + Self::new(MatrixData::new_mut::(value)) + } +} + +impl From<[T; TOTAL]> + for DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + > +{ + fn from(value: [T; TOTAL]) -> Self { + #[cfg(not(feature = "no_assert"))] + { + debug_assert!(STATES * STATES <= TOTAL); + } + Self::new(MatrixData::new_array::(value)) + } +} + +// ----------------------------------------------------------- + +impl DirectProcessNoiseCovarianceMatrixBuffer +where + M: Matrix, +{ + pub const fn new(matrix: M) -> Self { + Self(matrix, PhantomData) + } + + pub const fn len(&self) -> usize { + STATES * STATES + } + + pub const fn is_empty(&self) -> bool { + STATES == 0 + } +} + +impl RowMajorSequentialData + for DirectProcessNoiseCovarianceMatrixBuffer +where + M: Matrix, +{ + #[inline(always)] + fn as_slice(&self) -> &[T] { + self.0.as_slice() + } +} + +impl Matrix + for DirectProcessNoiseCovarianceMatrixBuffer +where + M: Matrix, +{ +} + +impl DirectProcessNoiseCovarianceMatrix + for DirectProcessNoiseCovarianceMatrixBuffer +where + M: Matrix, +{ + type Target = M; + + fn as_matrix(&self) -> &Self::Target { + &self.0 + } +} + +impl DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + pub const fn new(matrix: M) -> Self { + Self(matrix, PhantomData) + } + + pub const fn len(&self) -> usize { + STATES * STATES + } + + pub const fn is_empty(&self) -> bool { + STATES == 0 + } + + /// Ensures the underlying buffer has enough space for the expected number of values. + pub fn is_valid(&self) -> bool { + self.0.is_valid() + } +} + +impl RowMajorSequentialData + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + #[inline(always)] + fn as_slice(&self) -> &[T] { + self.0.as_slice() + } +} + +impl RowMajorSequentialDataMut + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + #[inline(always)] + fn as_mut_slice(&mut self) -> &mut [T] { + self.0.as_mut_slice() + } +} + +impl Matrix + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ +} + +impl MatrixMut + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ +} + +impl DirectProcessNoiseCovarianceMatrix + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + type Target = M; + + fn as_matrix(&self) -> &Self::Target { + &self.0 + } +} + +impl DirectProcessNoiseCovarianceMatrixMut + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + type TargetMut = M; + + fn as_matrix_mut(&mut self) -> &mut Self::TargetMut { + &mut self.0 + } +} + +impl Index + for DirectProcessNoiseCovarianceMatrixBuffer +where + M: Matrix, +{ + type Output = T; + + fn index(&self, index: usize) -> &Self::Output { + self.0.index(index) + } +} + +impl Index + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + type Output = T; + + fn index(&self, index: usize) -> &Self::Output { + self.0.index(index) + } +} + +impl IndexMut + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut, +{ + fn index_mut(&mut self, index: usize) -> &mut Self::Output { + self.0.index_mut(index) + } +} + +// ----------------------------------------------------------- + +impl IntoInnerData + for DirectProcessNoiseCovarianceMatrixBuffer +where + M: MatrixMut + IntoInnerData, +{ + type Target = M::Target; + + fn into_inner(self) -> Self::Target { + self.0.into_inner() + } +} + +impl IntoInnerData + for DirectProcessNoiseCovarianceMatrixMutBuffer +where + M: MatrixMut + IntoInnerData, +{ + type Target = M::Target; + + fn into_inner(self) -> Self::Target { + self.0.into_inner() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_from_array() { + let value: DirectProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 100].into(); + assert_eq!(value.len(), 25); + assert!(!value.is_empty()); + assert!(value.is_valid()); + } + + #[test] + fn test_from_ref() { + let data = [0.0_f32; 100]; + let value: DirectProcessNoiseCovarianceMatrixBuffer<5, f32, _> = data.as_ref().into(); + assert_eq!(value.len(), 25); + assert!(value.is_valid()); + assert!(!value.is_empty()); + assert!(core::ptr::eq(value.as_slice(), &data)); + } + + #[test] + fn test_from_mut() { + let mut data = [0.0_f32; 100]; + let value: DirectProcessNoiseCovarianceMatrixBuffer<5, f32, _> = data.as_mut_slice().into(); + assert_eq!(value.len(), 25); + assert!(value.is_valid()); + assert!(!value.is_empty()); + assert!(core::ptr::eq(value.as_slice(), &data)); + } + + #[test] + #[cfg(feature = "no_assert")] + fn test_from_array_invalid_size() { + let value: DirectProcessNoiseCovarianceMatrixBuffer<5, f32, _> = [0.0; 1].into(); + assert!(!value.is_valid()); + } + + #[test] + fn test_mut_from_array() { + let value: DirectProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 100].into(); + assert_eq!(value.len(), 25); + assert!(!value.is_empty()); + assert!(value.is_valid()); + } + + #[test] + fn test_mut_from_mut() { + let mut data = [0.0_f32; 100]; + let value: DirectProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = + data.as_mut_slice().into(); + assert_eq!(value.len(), 25); + assert!(!value.is_empty()); + assert!(value.is_valid()); + assert!(core::ptr::eq(value.as_slice(), &data)); + } + + #[test] + #[cfg(feature = "no_assert")] + fn test_mut_from_array_invalid_size() { + let value: DirectProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 1].into(); + assert!(!value.is_valid()); + } + + #[test] + #[rustfmt::skip] + fn test_access() { + let mut value: DirectProcessNoiseCovarianceMatrixMutBuffer<5, f32, _> = [0.0; 25].into(); + + // Set values. + { + let matrix = value.as_matrix_mut(); + for i in 0..matrix.cols() { + matrix.set_symmetric(0, i, i as _); + matrix.set_at(i, i, i as _); + } + } + + // Update values. + for i in 0..value.len() { + value[i] += 10.0; + } + + // Get values. + { + let matrix = value.as_matrix(); + for i in 0..matrix.rows() { + assert_eq!(matrix.get_at(0, i), 10.0 + i as f32); + assert_eq!(matrix.get_at(i, 0), 10.0 + i as f32); + } + } + + assert_eq!(value.into_inner(), + [ + 10.0, 11.0, 12.0, 13.0, 14.0, + 11.0, 11.0, 10.0, 10.0, 10.0, + 12.0, 10.0, 12.0, 10.0, 10.0, + 13.0, 10.0, 10.0, 13.0, 10.0, + 14.0, 10.0, 10.0, 10.0, 14.0, + ]); + } +} diff --git a/crates/minikalman/src/controls.rs b/crates/minikalman/src/controls.rs index d38b1f1..1b68ca6 100644 --- a/crates/minikalman/src/controls.rs +++ b/crates/minikalman/src/controls.rs @@ -34,7 +34,7 @@ impl ControlBuilder { T: MatrixDataType, B: ControlMatrix, U: ControlVector, - Q: ProcessNoiseCovarianceMatrix, + Q: ControlProcessNoiseCovarianceMatrix, TempBQ: TemporaryBQMatrix, { Control:: { @@ -153,7 +153,7 @@ where impl Control where - Q: ProcessNoiseCovarianceMatrix, + Q: ControlProcessNoiseCovarianceMatrix, { /// Gets a reference to the control covariance matrix Q. /// @@ -170,7 +170,7 @@ where impl Control where - Q: ProcessNoiseCovarianceMatrixMut, + Q: ControlProcessNoiseCovarianceMatrixMut, { /// Gets a mutable reference to the control covariance matrix Q. /// @@ -190,7 +190,7 @@ impl where U: ControlVector, B: ControlMatrix, - Q: ProcessNoiseCovarianceMatrix, + Q: ControlProcessNoiseCovarianceMatrix, TempBQ: TemporaryBQMatrix, T: MatrixDataType, { @@ -291,7 +291,7 @@ impl KalmanFilterProcessNoiseCovariance for Control where - Q: ProcessNoiseCovarianceMatrix, + Q: ControlProcessNoiseCovarianceMatrix, { type ProcessNoiseCovarianceMatrix = Q; @@ -303,7 +303,7 @@ where impl KalmanFilterControlCovarianceMut for Control where - Q: ProcessNoiseCovarianceMatrixMut, + Q: ControlProcessNoiseCovarianceMatrixMut, { type ProcessNoiseCovarianceMatrixMut = Q; @@ -317,7 +317,7 @@ impl where U: ControlVector, B: ControlMatrix, - Q: ProcessNoiseCovarianceMatrix, + Q: ControlProcessNoiseCovarianceMatrix, TempBQ: TemporaryBQMatrix, T: MatrixDataType, { @@ -365,11 +365,12 @@ mod tests { let x = BufferBuilder::state_vector_x::().new(); let A = BufferBuilder::system_matrix_A::().new(); let P = BufferBuilder::estimate_covariance_P::().new(); + let Q_direct = BufferBuilder::direct_process_noise_covariance_Q::().new(); // Control buffers. let u = BufferBuilder::control_vector_u::().new(); let B = BufferBuilder::control_matrix_B::().new(); - let Q = BufferBuilder::process_noise_covariance_Q::().new(); + let Q_control = BufferBuilder::control_process_noise_covariance_Q::().new(); // Filter temporaries. let temp_x = BufferBuilder::state_prediction_temp_x::().new(); @@ -378,8 +379,10 @@ mod tests { // Control temporaries let temp_BQ = BufferBuilder::temp_BQ::().new(); - let mut filter = RegularKalmanBuilder::new::(A, x, P, temp_x, temp_P); - let mut control = ControlBuilder::new::(B, u, Q, temp_BQ); + let mut filter = + RegularKalmanBuilder::new::(A, x, P, Q_direct, temp_x, temp_P); + let mut control = + ControlBuilder::new::(B, u, Q_control, temp_BQ); // State transition is identity. filter.state_transition_mut().apply(|mat| { diff --git a/crates/minikalman/src/kalman/extended.rs b/crates/minikalman/src/kalman/extended.rs index c0bc16f..24c4211 100644 --- a/crates/minikalman/src/kalman/extended.rs +++ b/crates/minikalman/src/kalman/extended.rs @@ -7,29 +7,27 @@ use core::marker::PhantomData; /// A builder for a [`ExtendedKalman`] filter instances. #[allow(clippy::type_complexity)] -pub struct ExtendedKalmanBuilder { +pub struct ExtendedKalmanBuilder { _phantom: ( PhantomData, PhantomData, PhantomData

, + PhantomData, PhantomData, PhantomData, ), } -impl ExtendedKalmanBuilder { +impl ExtendedKalmanBuilder { /// Initializes a Kalman filter instance. /// /// ## Arguments - /// * `A` - The state transition matrix (`STATES` × `STATES`), or the Jacobian in an EKF. + /// * `A` - The Jacobian of the state transition matrix (`STATES` × `STATES`). /// * `x` - The state vector (`STATES` × `1`). - /// * `B` - The control transition matrix (`STATES` × `CONTROLS`). - /// * `u` - The control vector (`CONTROLS` × `1`). /// * `P` - The state covariance matrix (`STATES` × `STATES`). - /// * `Q` - The control covariance matrix (`CONTROLS` × `CONTROLS`). + /// * `Q` - The direct process noise matrix (`STATES` × `STATES`). /// * `predictedX` - The temporary vector for predicted states (`STATES` × `1`). /// * `temp_P` - The temporary vector for P calculation (`STATES` × `STATES`). - /// * `temp_BQ` - The temporary vector for B×Q calculation (`STATES` × `CONTROLS`). /// /// ## Example /// @@ -44,6 +42,7 @@ impl ExtendedKalmanBuilder { /// impl_buffer_x!(mut gravity_x, NUM_STATES, f32, 0.0); /// impl_buffer_A!(mut gravity_A, NUM_STATES, f32, 0.0); /// impl_buffer_P!(mut gravity_P, NUM_STATES, f32, 0.0); + /// impl_buffer_Q_direct!(mut gravity_Q, NUM_STATES, f32, 0.0); /// /// // Filter temporaries. /// impl_buffer_temp_x!(mut gravity_temp_x, NUM_STATES, f32, 0.0); @@ -53,6 +52,7 @@ impl ExtendedKalmanBuilder { /// gravity_A, /// gravity_x, /// gravity_P, + /// gravity_Q, /// gravity_temp_x, /// gravity_temp_P, /// ); @@ -62,21 +62,24 @@ impl ExtendedKalmanBuilder { A: A, x: X, P: P, + Q: Q, predicted_x: PX, temp_P: TempP, - ) -> ExtendedKalman + ) -> ExtendedKalman where T: MatrixDataType, A: StateTransitionMatrix, X: StateVectorMut, P: EstimateCovarianceMatrix, + Q: DirectProcessNoiseCovarianceMatrix, PX: PredictedStateEstimateVector, TempP: TemporaryStateMatrix, { - ExtendedKalman:: { + ExtendedKalman:: { x, A, P, + Q, predicted_x, temp_P, _phantom: Default::default(), @@ -86,7 +89,7 @@ impl ExtendedKalmanBuilder { /// Kalman Filter structure. See [`ExtendedKalmanBuilder`] for construction. #[allow(non_snake_case, unused)] -pub struct ExtendedKalman { +pub struct ExtendedKalman { /// State vector. x: X, @@ -95,11 +98,16 @@ pub struct ExtendedKalman { /// See also [`P`]. A: A, - /// System covariance matrix. + /// Estimation covariance matrix. /// /// See also [`A`]. P: P, + /// Direct process noise matrix. + /// + /// See also [`P`]. + Q: Q, + /// x-sized temporary vector. predicted_x: PX, @@ -111,14 +119,16 @@ pub struct ExtendedKalman { _phantom: PhantomData, } -impl ExtendedKalman { +impl + ExtendedKalman +{ /// Returns the number of states. pub const fn states(&self) -> usize { STATES } } -impl ExtendedKalman +impl ExtendedKalman where X: StateVector, { @@ -132,7 +142,7 @@ where } } -impl ExtendedKalman +impl ExtendedKalman where X: StateVectorMut, { @@ -147,7 +157,7 @@ where } } -impl ExtendedKalman +impl ExtendedKalman where A: StateTransitionMatrix, { @@ -167,7 +177,7 @@ where } } -impl ExtendedKalman +impl ExtendedKalman where A: StateTransitionMatrixMut, { @@ -192,7 +202,7 @@ where } } -impl ExtendedKalman +impl ExtendedKalman where P: EstimateCovarianceMatrix, { @@ -218,7 +228,41 @@ where } } -impl ExtendedKalman { +impl ExtendedKalman +where + P: DirectProcessNoiseCovarianceMatrix, +{ + /// Gets a reference to the direct process noise matrix Q. + /// + /// This matrix represents the process noise covariance. It quantifies the uncertainty + /// introduced by the model dynamics and process variations, providing a measure of + /// how much the true state is expected to deviate from the predicted state due to + /// inherent system noise and external influences. + #[inline(always)] + pub fn direct_process_noise(&self) -> &Q { + &self.Q + } +} + +impl ExtendedKalman +where + P: DirectProcessNoiseCovarianceMatrixMut, +{ + /// Gets a mutable reference to the direct process noise matrix Q. + /// + /// This matrix represents the process noise covariance. It quantifies the uncertainty + /// introduced by the model dynamics and process variations, providing a measure of + /// how much the true state is expected to deviate from the predicted state due to + /// inherent system noise and external influences. + #[inline(always)] + pub fn direct_process_noise_mut(&mut self) -> &mut Q { + &mut self.Q + } +} + +impl + ExtendedKalman +{ /// Performs a nonlinear state transition only involving the current state. /// /// ## Extended Kalman Filters @@ -245,6 +289,7 @@ impl ExtendedKalman ExtendedKalman ExtendedKalman KalmanFilterNumStates - for ExtendedKalman +impl KalmanFilterNumStates + for ExtendedKalman { } -impl KalmanFilterStateVector - for ExtendedKalman +impl KalmanFilterStateVector + for ExtendedKalman where X: StateVector, { @@ -483,8 +529,8 @@ where } } -impl KalmanFilterStateVectorMut - for ExtendedKalman +impl KalmanFilterStateVectorMut + for ExtendedKalman where X: StateVectorMut, { @@ -496,8 +542,8 @@ where } } -impl ExtendedKalmanFilterStateTransition - for ExtendedKalman +impl ExtendedKalmanFilterStateTransition + for ExtendedKalman where A: StateTransitionMatrix, { @@ -509,8 +555,9 @@ where } } -impl ExtendedKalmanFilterStateTransitionMut - for ExtendedKalman +impl + ExtendedKalmanFilterStateTransitionMut + for ExtendedKalman where A: StateTransitionMatrixMut, { @@ -522,8 +569,8 @@ where } } -impl KalmanFilterEstimateCovariance - for ExtendedKalman +impl KalmanFilterEstimateCovariance + for ExtendedKalman where P: EstimateCovarianceMatrix, { @@ -535,8 +582,8 @@ where } } -impl KalmanFilterEstimateCovarianceMut - for ExtendedKalman +impl KalmanFilterEstimateCovarianceMut + for ExtendedKalman where P: EstimateCovarianceMatrix, { @@ -548,8 +595,8 @@ where } } -impl KalmanFilterNonlinearPredict - for ExtendedKalman +impl KalmanFilterNonlinearPredict + for ExtendedKalman where X: StateVectorMut, A: StateTransitionMatrix, @@ -569,8 +616,8 @@ where } } -impl KalmanFilterNonlinearUpdate - for ExtendedKalman +impl KalmanFilterNonlinearUpdate + for ExtendedKalman where P: EstimateCovarianceMatrix, X: StateVectorMut, diff --git a/crates/minikalman/src/kalman/filter_trait.rs b/crates/minikalman/src/kalman/filter_trait.rs index 5331012..9337738 100644 --- a/crates/minikalman/src/kalman/filter_trait.rs +++ b/crates/minikalman/src/kalman/filter_trait.rs @@ -385,7 +385,7 @@ pub trait KalmanFilterControlTransitionMut { - type ProcessNoiseCovarianceMatrix: ProcessNoiseCovarianceMatrix; + type ProcessNoiseCovarianceMatrix: ControlProcessNoiseCovarianceMatrix; /// Gets a reference to the control covariance matrix Q. /// @@ -399,7 +399,7 @@ pub trait KalmanFilterProcessNoiseCovariance { pub trait KalmanFilterControlCovarianceMut: KalmanFilterProcessNoiseCovariance { - type ProcessNoiseCovarianceMatrixMut: ProcessNoiseCovarianceMatrixMut; + type ProcessNoiseCovarianceMatrixMut: ControlProcessNoiseCovarianceMatrixMut; /// Gets a mutable reference to the control covariance matrix Q. /// diff --git a/crates/minikalman/src/kalman/matrix_types.rs b/crates/minikalman/src/kalman/matrix_types.rs index 2c473d4..4797f16 100644 --- a/crates/minikalman/src/kalman/matrix_types.rs +++ b/crates/minikalman/src/kalman/matrix_types.rs @@ -122,10 +122,54 @@ pub trait ControlMatrixMut: /// Process noise covariance matrix. Represents the uncertainty in the state transition process. /// -/// Immutable variant. For a mutable variant, see [`ProcessNoiseCovarianceMatrixMut`]. +/// This matrix represents the direct process noise covariance. It quantifies the +/// uncertainty introduced by inherent system dynamics and external disturbances, +/// providing a measure of how much the true state is expected to deviate from the +/// predicted state due to these process variations. +/// +/// Immutable variant. For a mutable variant, see [`DirectProcessNoiseCovarianceMatrixMut`]. #[doc(alias = "ControlCovarianceMatrix")] #[doc(alias = "Prozessrauschkovarianzmatrix")] -pub trait ProcessNoiseCovarianceMatrix: +pub trait DirectProcessNoiseCovarianceMatrix: + RowMajorSequentialData + Index +{ + type Target: Matrix; + + fn as_matrix(&self) -> &Self::Target; +} + +/// Process noise covariance matrix. Represents the uncertainty in the state transition process. +/// +/// This matrix represents the direct process noise covariance. It quantifies the +/// uncertainty introduced by inherent system dynamics and external disturbances, +/// providing a measure of how much the true state is expected to deviate from the +/// predicted state due to these process variations. +/// +/// Mutable variant. For an immutable variant, see [`DirectProcessNoiseCovarianceMatrix`]. +#[doc(alias = "ControlCovarianceMatrixMut")] +#[doc(alias = "Prozessrauschkovarianzmatrix")] +pub trait DirectProcessNoiseCovarianceMatrixMut: + DirectProcessNoiseCovarianceMatrix + + RowMajorSequentialDataMut + + IndexMut +{ + type TargetMut: MatrixMut; + + fn as_matrix_mut(&mut self) -> &mut Self::TargetMut; +} + +/// Process noise covariance matrix. Represents the uncertainty in the state transition process. +/// +/// This matrix represents the control process noise covariance. It quantifies the +/// uncertainty introduced by the control inputs, reflecting how much the true state +/// is expected to deviate from the predicted state due to noise and variations +/// in the control process. The matrix is calculated as B×Q×Bᵀ, where B +/// represents the control input model, and Q is the process noise covariance (this matrix). +/// +/// Immutable variant. For a mutable variant, see [`ControlProcessNoiseCovarianceMatrixMut`]. +#[doc(alias = "ControlCovarianceMatrix")] +#[doc(alias = "Prozessrauschkovarianzmatrix")] +pub trait ControlProcessNoiseCovarianceMatrix: RowMajorSequentialData + Index { type Target: Matrix; @@ -135,11 +179,17 @@ pub trait ProcessNoiseCovarianceMatrix: /// Process noise covariance matrix. Represents the uncertainty in the state transition process. /// -/// Mutable variant. For an immutable variant, see [`ProcessNoiseCovarianceMatrix`]. +/// This matrix represents the control process noise covariance. It quantifies the +/// uncertainty introduced by the control inputs, reflecting how much the true state +/// is expected to deviate from the predicted state due to noise and variations +/// in the control process. The matrix is calculated as B×Q×Bᵀ, where B +/// represents the control input model, and Q is the process noise covariance (this matrix). +/// +/// Mutable variant. For an immutable variant, see [`ControlProcessNoiseCovarianceMatrix`]. #[doc(alias = "ControlCovarianceMatrixMut")] #[doc(alias = "Prozessrauschkovarianzmatrix")] -pub trait ProcessNoiseCovarianceMatrixMut: - ProcessNoiseCovarianceMatrix +pub trait ControlProcessNoiseCovarianceMatrixMut: + ControlProcessNoiseCovarianceMatrix + RowMajorSequentialDataMut + IndexMut { diff --git a/crates/minikalman/src/kalman/regular.rs b/crates/minikalman/src/kalman/regular.rs index d46c595..2ec7670 100644 --- a/crates/minikalman/src/kalman/regular.rs +++ b/crates/minikalman/src/kalman/regular.rs @@ -7,29 +7,27 @@ use core::marker::PhantomData; /// A builder for a [`RegularKalman`] filter instances. #[allow(clippy::type_complexity)] -pub struct RegularKalmanBuilder { +pub struct RegularKalmanBuilder { _phantom: ( PhantomData, PhantomData, PhantomData

, + PhantomData, PhantomData, PhantomData, ), } -impl RegularKalmanBuilder { +impl RegularKalmanBuilder { /// Initializes a Kalman filter instance. /// /// ## Arguments - /// * `A` - The state transition matrix (`STATES` × `STATES`), or the Jacobian in an EKF. + /// * `A` - The state transition matrix (`STATES` × `STATES`). /// * `x` - The state vector (`STATES` × `1`). - /// * `B` - The control transition matrix (`STATES` × `CONTROLS`). - /// * `u` - The control vector (`CONTROLS` × `1`). /// * `P` - The state covariance matrix (`STATES` × `STATES`). - /// * `Q` - The control covariance matrix (`CONTROLS` × `CONTROLS`). + /// * `Q` - The direct process noise matrix (`STATES` × `STATES`). /// * `predictedX` - The temporary vector for predicted states (`STATES` × `1`). /// * `temp_P` - The temporary vector for P calculation (`STATES` × `STATES`). - /// * `temp_BQ` - The temporary vector for B×Q calculation (`STATES` × `CONTROLS`). /// /// ## Example /// @@ -44,6 +42,7 @@ impl RegularKalmanBuilder { /// impl_buffer_x!(mut gravity_x, NUM_STATES, f32, 0.0); /// impl_buffer_A!(mut gravity_A, NUM_STATES, f32, 0.0); /// impl_buffer_P!(mut gravity_P, NUM_STATES, f32, 0.0); + /// impl_buffer_Q_direct!(mut gravity_Q, NUM_STATES, f32, 0.0); /// /// // Filter temporaries. /// impl_buffer_temp_x!(mut gravity_temp_x, NUM_STATES, f32, 0.0); @@ -53,6 +52,7 @@ impl RegularKalmanBuilder { /// gravity_A, /// gravity_x, /// gravity_P, + /// gravity_Q, /// gravity_temp_x, /// gravity_temp_P, /// ); @@ -62,21 +62,24 @@ impl RegularKalmanBuilder { A: A, x: X, P: P, + Q: Q, predicted_x: PX, temp_P: TempP, - ) -> RegularKalman + ) -> RegularKalman where T: MatrixDataType, A: StateTransitionMatrix, X: StateVectorMut, P: EstimateCovarianceMatrix, + Q: DirectProcessNoiseCovarianceMatrix, PX: PredictedStateEstimateVector, TempP: TemporaryStateMatrix, { - RegularKalman:: { + RegularKalman:: { x, A, P, + Q, predicted_x, temp_P, _phantom: Default::default(), @@ -86,7 +89,7 @@ impl RegularKalmanBuilder { /// Kalman Filter structure. See [`RegularKalmanBuilder`] for construction. #[allow(non_snake_case, unused)] -pub struct RegularKalman { +pub struct RegularKalman { /// State vector. x: X, @@ -95,11 +98,16 @@ pub struct RegularKalman { /// See also [`P`]. A: A, - /// System covariance matrix. + /// Estimation covariance matrix. /// /// See also [`A`]. P: P, + /// Direct process noise matrix. + /// + /// See also [`P`]. + Q: Q, + /// x-sized temporary vector. predicted_x: PX, @@ -111,14 +119,16 @@ pub struct RegularKalman { _phantom: PhantomData, } -impl RegularKalman { +impl + RegularKalman +{ /// Returns the number of states. pub const fn states(&self) -> usize { STATES } } -impl RegularKalman +impl RegularKalman where X: StateVector, { @@ -132,7 +142,7 @@ where } } -impl RegularKalman +impl RegularKalman where X: StateVectorMut, { @@ -147,7 +157,7 @@ where } } -impl RegularKalman +impl RegularKalman where A: StateTransitionMatrix, { @@ -165,7 +175,7 @@ where } } -impl RegularKalman +impl RegularKalman where A: StateTransitionMatrixMut, { @@ -183,7 +193,7 @@ where } } -impl RegularKalman +impl RegularKalman where P: EstimateCovarianceMatrix, { @@ -209,7 +219,9 @@ where } } -impl RegularKalman { +impl + RegularKalman +{ /// Performs the time update / prediction step. /// /// This call assumes that the control covariance and variables are already set in the filter structure. @@ -571,13 +583,13 @@ impl RegularKalman KalmanFilterNumStates - for RegularKalman +impl KalmanFilterNumStates + for RegularKalman { } -impl KalmanFilterStateVector - for RegularKalman +impl KalmanFilterStateVector + for RegularKalman where X: StateVector, { @@ -589,8 +601,8 @@ where } } -impl KalmanFilterStateVectorMut - for RegularKalman +impl KalmanFilterStateVectorMut + for RegularKalman where X: StateVectorMut, { @@ -602,8 +614,8 @@ where } } -impl KalmanFilterStateTransition - for RegularKalman +impl KalmanFilterStateTransition + for RegularKalman where A: StateTransitionMatrix, { @@ -615,8 +627,8 @@ where } } -impl KalmanFilterStateTransitionMut - for RegularKalman +impl KalmanFilterStateTransitionMut + for RegularKalman where A: StateTransitionMatrixMut, { @@ -628,8 +640,8 @@ where } } -impl KalmanFilterEstimateCovariance - for RegularKalman +impl KalmanFilterEstimateCovariance + for RegularKalman where P: EstimateCovarianceMatrix, { @@ -641,8 +653,8 @@ where } } -impl KalmanFilterEstimateCovarianceMut - for RegularKalman +impl KalmanFilterEstimateCovarianceMut + for RegularKalman where P: EstimateCovarianceMatrix, { @@ -654,8 +666,40 @@ where } } -impl KalmanFilterPredict - for RegularKalman +impl RegularKalman +where + P: DirectProcessNoiseCovarianceMatrix, +{ + /// Gets a reference to the direct process noise matrix Q. + /// + /// This matrix represents the process noise covariance. It quantifies the uncertainty + /// introduced by the model dynamics and process variations, providing a measure of + /// how much the true state is expected to deviate from the predicted state due to + /// inherent system noise and external influences. + #[inline(always)] + pub fn direct_process_noise(&self) -> &Q { + &self.Q + } +} + +impl RegularKalman +where + P: DirectProcessNoiseCovarianceMatrixMut, +{ + /// Gets a mutable reference to the direct process noise matrix Q. + /// + /// This matrix represents the process noise covariance. It quantifies the uncertainty + /// introduced by the model dynamics and process variations, providing a measure of + /// how much the true state is expected to deviate from the predicted state due to + /// inherent system noise and external influences. + #[inline(always)] + pub fn direct_process_noise_mut(&mut self) -> &mut Q { + &mut self.Q + } +} + +impl KalmanFilterPredict + for RegularKalman where X: StateVectorMut, A: StateTransitionMatrix, @@ -670,8 +714,8 @@ where } } -impl KalmanFilterUpdate - for RegularKalman +impl KalmanFilterUpdate + for RegularKalman where P: EstimateCovarianceMatrix, X: StateVectorMut, @@ -686,8 +730,8 @@ where } } -impl KalmanFilterApplyControl - for RegularKalman +impl KalmanFilterApplyControl + for RegularKalman where P: EstimateCovarianceMatrix, X: StateVectorMut, diff --git a/crates/minikalman/src/kalman_builder/extended.rs b/crates/minikalman/src/kalman_builder/extended.rs index 0575d49..7569270 100644 --- a/crates/minikalman/src/kalman_builder/extended.rs +++ b/crates/minikalman/src/kalman_builder/extended.rs @@ -30,6 +30,7 @@ pub type KalmanFilterType = ExtendedKalman< SystemMatrixMutBufferOwnedType, StateVectorBufferOwnedType, EstimateCovarianceMatrixBufferOwnedType, + DirectProcessNoiseCovarianceMatrixBufferOwnedType, TemporaryStatePredictionVectorBufferOwnedType, TemporaryStateMatrixBufferOwnedType, >; @@ -64,6 +65,7 @@ impl KalmanFilterBuilder { let state_vector = BufferBuilder::state_vector_x::().new(); let system_matrix = BufferBuilder::system_matrix_A::().new(); let system_covariance = BufferBuilder::estimate_covariance_P::().new(); + let process_noise = BufferBuilder::direct_process_noise_covariance_Q::().new(); // Filter temporaries. let temp_x = BufferBuilder::state_prediction_temp_x::().new(); @@ -73,6 +75,7 @@ impl KalmanFilterBuilder { system_matrix, state_vector, system_covariance, + process_noise, temp_x, temp_p, ) diff --git a/crates/minikalman/src/kalman_builder/regular.rs b/crates/minikalman/src/kalman_builder/regular.rs index d7c0035..ee3d6d4 100644 --- a/crates/minikalman/src/kalman_builder/regular.rs +++ b/crates/minikalman/src/kalman_builder/regular.rs @@ -36,6 +36,7 @@ pub type KalmanFilterType = RegularKalman< SystemMatrixMutBufferOwnedType, StateVectorBufferOwnedType, EstimateCovarianceMatrixBufferOwnedType, + DirectProcessNoiseCovarianceMatrixBufferOwnedType, TemporaryStatePredictionVectorBufferOwnedType, TemporaryStateMatrixBufferOwnedType, >; @@ -71,6 +72,7 @@ impl KalmanFilterBuilder { let state_vector = BufferBuilder::state_vector_x::().new(); let system_matrix = BufferBuilder::system_matrix_A::().new(); let system_covariance = BufferBuilder::estimate_covariance_P::().new(); + let process_noise = BufferBuilder::direct_process_noise_covariance_Q::().new(); // Filter temporaries. let temp_x = BufferBuilder::state_prediction_temp_x::().new(); @@ -80,6 +82,7 @@ impl KalmanFilterBuilder { system_matrix, state_vector, system_covariance, + process_noise, temp_x, temp_p, ) @@ -111,7 +114,7 @@ pub type KalmanFilterControlType T, ControlMatrixBufferOwnedType, ControlVectorBufferOwnedType, - ControlCovarianceMatrixBufferOwnedType, + ControlProcessNoiseCovarianceMatrixBufferOwnedType, TemporaryBQMatrixBufferOwnedType, >; @@ -143,7 +146,8 @@ impl KalmanFilterControlBuilder { // Control buffers. let control_vector = BufferBuilder::control_vector_u::().new(); let control_matrix = BufferBuilder::control_matrix_B::().new(); - let control_covariance = BufferBuilder::process_noise_covariance_Q::().new(); + let control_covariance = + BufferBuilder::control_process_noise_covariance_Q::().new(); // Control temporaries. let temp_bq = BufferBuilder::temp_BQ::().new(); diff --git a/crates/minikalman/src/lib.rs b/crates/minikalman/src/lib.rs index e851dbd..4258c89 100644 --- a/crates/minikalman/src/lib.rs +++ b/crates/minikalman/src/lib.rs @@ -140,9 +140,10 @@ //! impl_buffer_x!(static mut gravity_x, NUM_STATES, f32, 0.0); //! impl_buffer_A!(static mut gravity_A, NUM_STATES, f32, 0.0); //! impl_buffer_P!(static mut gravity_P, NUM_STATES, f32, 0.0); +//! impl_buffer_Q_direct!(static mut gravity_Q, NUM_STATES, f32, 0.0); //! //! // Observation buffers. -//! impl_buffer_Q!(static mut gravity_z, NUM_OBSERVATIONS, f32, 0.0); +//! impl_buffer_z!(static mut gravity_z, NUM_OBSERVATIONS, f32, 0.0); //! impl_buffer_H!(static mut gravity_H, NUM_OBSERVATIONS, NUM_STATES, f32, 0.0); //! impl_buffer_R!(static mut gravity_R, NUM_OBSERVATIONS, f32, 0.0); //! impl_buffer_y!(static mut gravity_y, NUM_OBSERVATIONS, f32, 0.0); @@ -165,6 +166,7 @@ //! StateTransitionMatrixMutBuffer::from(unsafe { gravity_A.as_mut_slice() }), //! StateVectorBuffer::from(unsafe { gravity_x.as_mut_slice() }), //! EstimateCovarianceMatrixBuffer::from(unsafe { gravity_P.as_mut_slice() }), +//! DirectProcessNoiseCovarianceMatrixBuffer::from(unsafe { gravity_Q.as_mut_slice() }), //! PredictedStateEstimateVectorBuffer::from(unsafe { gravity_temp_x.as_mut_slice() }), //! TemporaryStateMatrixBuffer::from(unsafe { gravity_temp_P.as_mut_slice() }), //! ); @@ -256,10 +258,11 @@ pub mod prelude { pub use crate::matrix::*; pub use crate::{ - impl_buffer_A, impl_buffer_B, impl_buffer_H, impl_buffer_K, impl_buffer_P, impl_buffer_Q, - impl_buffer_R, impl_buffer_S, impl_buffer_temp_BQ, impl_buffer_temp_HP, - impl_buffer_temp_KHP, impl_buffer_temp_P, impl_buffer_temp_PHt, impl_buffer_temp_S_inv, - impl_buffer_temp_x, impl_buffer_u, impl_buffer_x, impl_buffer_y, impl_buffer_z, + impl_buffer_A, impl_buffer_B, impl_buffer_H, impl_buffer_K, impl_buffer_P, + impl_buffer_Q_control, impl_buffer_Q_direct, impl_buffer_R, impl_buffer_S, + impl_buffer_temp_BQ, impl_buffer_temp_HP, impl_buffer_temp_KHP, impl_buffer_temp_P, + impl_buffer_temp_PHt, impl_buffer_temp_S_inv, impl_buffer_temp_x, impl_buffer_u, + impl_buffer_x, impl_buffer_y, impl_buffer_z, }; pub use crate::{ diff --git a/crates/minikalman/src/static_macros.rs b/crates/minikalman/src/static_macros.rs index 1544a90..2fcc7d5 100644 --- a/crates/minikalman/src/static_macros.rs +++ b/crates/minikalman/src/static_macros.rs @@ -319,9 +319,9 @@ macro_rules! impl_buffer_B { }; } -/// Creates a static buffer fitting the square process noise covariance matrix Q (`num_controls` × `num_controls`). +/// Creates a static buffer fitting the square control process noise covariance matrix Q (`num_controls` × `num_controls`). /// -/// This will create a [`ProcessNoiseCovarianceMatrixMutBuffer`](crate::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer) +/// This will create a [`ProcessNoiseCovarianceMatrixMutBuffer`](crate::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer) /// backed by a [`MatrixDataArray`](crate::matrix::MatrixDataArray). /// /// ## Arguments @@ -335,7 +335,7 @@ macro_rules! impl_buffer_B { /// ``` /// # use minikalman::prelude::*; /// const NUM_CONTROLS: usize = 2; -/// impl_buffer_Q!(static mut Q, NUM_CONTROLS, f32, 0.0); +/// impl_buffer_Q_control!(static mut Q, NUM_CONTROLS, f32, 0.0); /// /// unsafe { /// assert_eq!(Q.len(), 4); @@ -344,31 +344,31 @@ macro_rules! impl_buffer_B { /// ``` #[macro_export] #[allow(non_snake_case)] -macro_rules! impl_buffer_Q { +macro_rules! impl_buffer_Q_control { (mut $mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, let mut) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, let mut) }; ($mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, let) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, let) }; (let mut $mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, let mut) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, let mut) }; (let $mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, let) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, let) }; (static mut $mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, static mut) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, static mut) }; (static $mat_name:ident, $num_controls:expr, $t:ty, $init:expr) => { - $crate::impl_buffer_Q!($mat_name, $num_controls, $t, $init, static) + $crate::impl_buffer_Q_control!($mat_name, $num_controls, $t, $init, static) }; ($mat_name:ident, $num_controls:expr, $t:ty, $init:expr, $($keywords:tt)+) => { - $($keywords)* $mat_name: $crate::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer< + $($keywords)* $mat_name: $crate::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer< $num_controls, $t, $crate::matrix::MatrixDataArray<$num_controls, $num_controls, { $num_controls * $num_controls }, $t>, - > = $crate::buffers::types::ProcessNoiseCovarianceMatrixMutBuffer::< + > = $crate::buffers::types::ControlProcessNoiseCovarianceMatrixMutBuffer::< $num_controls, $t, $crate::matrix::MatrixDataArray<$num_controls, $num_controls, { $num_controls * $num_controls }, $t>, @@ -378,6 +378,65 @@ macro_rules! impl_buffer_Q { }; } +/// Creates a static buffer fitting the square direct process noise covariance matrix Q (`num_states` × `num_states`). +/// +/// This will create a [`DirectProcessNoiseCovarianceMatrixMutBuffer`](crate::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer) +/// backed by a [`MatrixDataArray`](crate::matrix::MatrixDataArray). +/// +/// ## Arguments +/// * `num_controls` - The number of controls to the system. +/// * `t` - The data type. +/// * `init` - The default value to initialize the buffer with. +/// +/// ## Example +/// You can generate a `static mut` binding: +/// +/// ``` +/// # use minikalman::prelude::*; +/// const NUM_CONTROLS: usize = 2; +/// impl_buffer_Q_direct!(static mut Q, NUM_CONTROLS, f32, 0.0); +/// +/// unsafe { +/// assert_eq!(Q.len(), 4); +/// assert_eq!(Q[0], 0.0_f32); +/// } +/// ``` +#[macro_export] +#[allow(non_snake_case)] +macro_rules! impl_buffer_Q_direct { + (mut $mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, let mut) + }; + ($mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, let) + }; + (let mut $mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, let mut) + }; + (let $mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, let) + }; + (static mut $mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, static mut) + }; + (static $mat_name:ident, $num_states:expr, $t:ty, $init:expr) => { + $crate::impl_buffer_Q_direct!($mat_name, $num_states, $t, $init, static) + }; + ($mat_name:ident, $num_states:expr, $t:ty, $init:expr, $($keywords:tt)+) => { + $($keywords)* $mat_name: $crate::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer< + $num_states, + $t, + $crate::matrix::MatrixDataArray<$num_states, $num_states, { $num_states * $num_states }, $t>, + > = $crate::buffers::types::DirectProcessNoiseCovarianceMatrixMutBuffer::< + $num_states, + $t, + $crate::matrix::MatrixDataArray<$num_states, $num_states, { $num_states * $num_states }, $t>, + >::new($crate::matrix::MatrixDataArray::new_unchecked( + [$init; { $num_states * $num_states }], + )); + }; +} + /// Creates a static buffer fitting the measurement vector z (`num_measurements` × `1`). /// /// This will create a [`ObservationVectorBuffer`](crate::buffers::types::MeasurementVectorBuffer) diff --git a/crates/minikalman/src/test_dummies.rs b/crates/minikalman/src/test_dummies.rs index 9748091..4b009e7 100644 --- a/crates/minikalman/src/test_dummies.rs +++ b/crates/minikalman/src/test_dummies.rs @@ -17,6 +17,7 @@ pub fn make_dummy_filter() -> RegularKalman< Dummy, Dummy, Dummy, + Dummy, Dummy, Dummy, > { @@ -26,6 +27,7 @@ pub fn make_dummy_filter() -> RegularKalman< Dummy::default(), Dummy::default(), Dummy::default(), + Dummy::default(), ) } @@ -35,6 +37,7 @@ pub fn make_dummy_filter_ekf() -> ExtendedKalman< Dummy, Dummy, Dummy, + Dummy, Dummy, Dummy, > { @@ -44,6 +47,7 @@ pub fn make_dummy_filter_ekf() -> ExtendedKalman< Dummy::default(), Dummy::default(), Dummy::default(), + Dummy::default(), ) } @@ -263,7 +267,27 @@ impl ControlMatrixMut ProcessNoiseCovarianceMatrix +impl DirectProcessNoiseCovarianceMatrix + for Dummy +{ + type Target = DummyMatrix; + + fn as_matrix(&self) -> &Self::Target { + &self.0 + } +} + +impl DirectProcessNoiseCovarianceMatrixMut + for Dummy +{ + type TargetMut = DummyMatrix; + + fn as_matrix_mut(&mut self) -> &mut Self::TargetMut { + &mut self.0 + } +} + +impl ControlProcessNoiseCovarianceMatrix for Dummy { type Target = DummyMatrix; @@ -273,7 +297,7 @@ impl ProcessNoiseCovarianceMatrix } } -impl ProcessNoiseCovarianceMatrixMut +impl ControlProcessNoiseCovarianceMatrixMut for Dummy { type TargetMut = DummyMatrix; diff --git a/crates/minikalman/tests/gravity.rs b/crates/minikalman/tests/gravity.rs index b5b91ca..b1a01a9 100644 --- a/crates/minikalman/tests/gravity.rs +++ b/crates/minikalman/tests/gravity.rs @@ -41,6 +41,7 @@ fn test_gravity_estimation() { impl_buffer_x!(mut gravity_x, NUM_STATES, f32, 0.0); impl_buffer_A!(mut gravity_A, NUM_STATES, f32, 0.0); impl_buffer_P!(mut gravity_P, NUM_STATES, f32, 0.0); + impl_buffer_Q_direct!(mut gravity_Q, NUM_STATES, f32, 0.0); // Observation buffers. impl_buffer_z!(mut gravity_z, NUM_OBSERVATIONS, f32, 0.0); @@ -64,6 +65,7 @@ fn test_gravity_estimation() { gravity_A, gravity_x, gravity_P, + gravity_Q, gravity_temp_x, gravity_temp_P, ); diff --git a/crates/minikalman/tests/gravity_q_tuned.rs b/crates/minikalman/tests/gravity_q_tuned.rs index 0fb090b..d6e1423 100644 --- a/crates/minikalman/tests/gravity_q_tuned.rs +++ b/crates/minikalman/tests/gravity_q_tuned.rs @@ -5,6 +5,7 @@ #![forbid(unsafe_code)] +use minikalman::impl_buffer_Q_direct; use minikalman::prelude::*; use minikalman::regular::{RegularKalmanBuilder, RegularObservationBuilder}; @@ -41,6 +42,7 @@ fn test_gravity_estimation_tuned() { impl_buffer_x!(mut gravity_x, NUM_STATES, f64, 0.0); impl_buffer_A!(mut gravity_A, NUM_STATES, f64, 0.0); impl_buffer_P!(mut gravity_P, NUM_STATES, f64, 0.0); + impl_buffer_Q_direct!(mut gravity_Q, NUM_STATES, f64, 0.0); // Observation buffers. impl_buffer_z!(mut gravity_z, NUM_OBSERVATIONS, f64, 0.0); @@ -64,6 +66,7 @@ fn test_gravity_estimation_tuned() { gravity_A, gravity_x, gravity_P, + gravity_Q, gravity_temp_x, gravity_temp_P, ); diff --git a/xbuild-tests/stm32/src/kalman_fixed_example.rs b/xbuild-tests/stm32/src/kalman_fixed_example.rs index 0e5860f..a069c99 100644 --- a/xbuild-tests/stm32/src/kalman_fixed_example.rs +++ b/xbuild-tests/stm32/src/kalman_fixed_example.rs @@ -67,9 +67,10 @@ pub fn predict_gravity() -> I16F16 { impl_buffer_x!(static mut gravity_x, NUM_STATES, I16F16, I16F16::ZERO); impl_buffer_A!(static mut gravity_A, NUM_STATES, I16F16, I16F16::ZERO); impl_buffer_P!(static mut gravity_P, NUM_STATES, I16F16, I16F16::ZERO); + impl_buffer_Q_direct!(static mut gravity_Q, NUM_STATES, I16F16, I16F16::ZERO); // Observation buffers. - impl_buffer_Q!(static mut gravity_z, NUM_OBSERVATIONS, I16F16, I16F16::ZERO); + impl_buffer_Q_control!(static mut gravity_z, NUM_OBSERVATIONS, I16F16, I16F16::ZERO); impl_buffer_H!( static mut gravity_H, NUM_OBSERVATIONS, @@ -121,6 +122,7 @@ pub fn predict_gravity() -> I16F16 { StateTransitionMatrixMutBuffer::from(unsafe { gravity_A.as_mut_slice() }), StateVectorBuffer::from(unsafe { gravity_x.as_mut_slice() }), EstimateCovarianceMatrixBuffer::from(unsafe { gravity_P.as_mut_slice() }), + DirectProcessNoiseCovarianceMatrixBuffer::from(unsafe { gravity_Q.as_mut_slice() }), PredictedStateEstimateVectorBuffer::from(unsafe { gravity_temp_x.as_mut_slice() }), TemporaryStateMatrixBuffer::from(unsafe { gravity_temp_P.as_mut_slice() }), ); diff --git a/xbuild-tests/stm32/src/kalman_float_example.rs b/xbuild-tests/stm32/src/kalman_float_example.rs index f2b2ab2..9a7fd5f 100644 --- a/xbuild-tests/stm32/src/kalman_float_example.rs +++ b/xbuild-tests/stm32/src/kalman_float_example.rs @@ -36,9 +36,10 @@ pub fn predict_gravity() -> f32 { impl_buffer_x!(static mut gravity_x, NUM_STATES, f32, 0.0); impl_buffer_A!(static mut gravity_A, NUM_STATES, f32, 0.0); impl_buffer_P!(static mut gravity_P, NUM_STATES, f32, 0.0); + impl_buffer_Q_direct!(static mut gravity_Q, NUM_STATES, f32, 0.0); // Observation buffers. - impl_buffer_Q!(static mut gravity_z, NUM_OBSERVATIONS, f32, 0.0); + impl_buffer_z!(static mut gravity_z, NUM_OBSERVATIONS, f32, 0.0); impl_buffer_H!(static mut gravity_H, NUM_OBSERVATIONS, NUM_STATES, f32, 0.0); impl_buffer_R!(static mut gravity_R, NUM_OBSERVATIONS, f32, 0.0); impl_buffer_y!(static mut gravity_y, NUM_OBSERVATIONS, f32, 0.0); @@ -61,6 +62,7 @@ pub fn predict_gravity() -> f32 { StateTransitionMatrixMutBuffer::from(unsafe { gravity_A.as_mut_slice() }), StateVectorBuffer::from(unsafe { gravity_x.as_mut_slice() }), EstimateCovarianceMatrixBuffer::from(unsafe { gravity_P.as_mut_slice() }), + DirectProcessNoiseCovarianceMatrixBuffer::from(unsafe { gravity_Q.as_mut_slice() }), PredictedStateEstimateVectorBuffer::from(unsafe { gravity_temp_x.as_mut_slice() }), TemporaryStateMatrixBuffer::from(unsafe { gravity_temp_P.as_mut_slice() }), );