Skip to content

Commit

Permalink
Add direct process noise covariance matrix
Browse files Browse the repository at this point in the history 
Signed-off-by: Markus Mayer <[email protected]>
  • Loading branch information
@sunsided
sunsided committed Jun 22, 2024
1 parent eb855d5 commit b67ddd9
Show file tree
Hide file tree
Showing 21 changed files with 978 additions and 183 deletions.
4 changes: 4 additions & 0 deletions crates/minikalman/benches/gravity.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ fn criterion_benchmark(c: &mut Criterion) {
let mut gravity_x = BufferBuilder::state_vector_x::<NUM_STATES>().new();
let mut gravity_A = BufferBuilder::system_matrix_A::<NUM_STATES>().new();
let mut gravity_P = BufferBuilder::estimate_covariance_P::<NUM_STATES>().new();
let mut gravity_Q = BufferBuilder::direct_process_noise_covariance_Q::<NUM_STATES>().new();

// Control buffers.
// let mut gravity_u = BufferBuilder::control_vector_u::<NUM_CONTROLS>().new();
Expand Down Expand Up @@ -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()),
);
Expand Down Expand Up @@ -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()),
);
Expand Down Expand Up @@ -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()),
);
Expand Down
4 changes: 4 additions & 0 deletions crates/minikalman/benches/gravity_fixed.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ fn criterion_benchmark(c: &mut Criterion) {
let mut gravity_x = BufferBuilder::state_vector_x::<NUM_STATES>().new();
let mut gravity_A = BufferBuilder::system_matrix_A::<NUM_STATES>().new();
let mut gravity_P = BufferBuilder::estimate_covariance_P::<NUM_STATES>().new();
let mut gravity_Q = BufferBuilder::direct_process_noise_covariance_Q::<NUM_STATES>().new();

// Observation buffers.
let mut gravity_z = BufferBuilder::measurement_vector_z::<NUM_OBSERVATIONS>().new();
Expand All @@ -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()),
);
Expand Down Expand Up @@ -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()),
);
Expand Down Expand Up @@ -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()),
);
Expand Down
2 changes: 1 addition & 1 deletion crates/minikalman/examples/gravity.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ fn initialize_control_matrix(filter: &mut impl ControlMatrixMut<NUM_STATES, NUM_

/// Initializes the control covariance.
fn initialize_control_covariance_matrix(
filter: &mut impl ProcessNoiseCovarianceMatrixMut<NUM_CONTROLS, f32>,
filter: &mut impl ControlProcessNoiseCovarianceMatrixMut<NUM_CONTROLS, f32>,
) {
filter.as_matrix_mut().apply(|mat| {
mat[0] = 1.0; // :)
Expand Down
140 changes: 118 additions & 22 deletions crates/minikalman/src/buffers/builder.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,13 @@ impl BufferBuilder {
EstimateCovarianceMatrixBufferBuilder
}

#[allow(non_snake_case)]
#[doc(alias = "process_noise_covariance_Q")]
pub fn direct_process_noise_covariance_Q<const STATES: usize>(
) -> DirectProcessNoiseCovarianceMatrixBufferBuilder<STATES> {
DirectProcessNoiseCovarianceMatrixBufferBuilder
}

pub fn control_vector_u<const CONTROLS: usize>() -> ControlVectorBufferBuilder<CONTROLS> {
ControlVectorBufferBuilder
}
Expand All @@ -36,9 +43,9 @@ impl BufferBuilder {

#[allow(non_snake_case)]
#[doc(alias = "control_covariance_Q")]
pub fn process_noise_covariance_Q<const CONTROLS: usize>(
) -> ProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS> {
ProcessNoiseCovarianceMatrixBufferBuilder
pub fn control_process_noise_covariance_Q<const CONTROLS: usize>(
) -> ControlProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS> {
ControlProcessNoiseCovarianceMatrixBufferBuilder
}

#[doc(alias = "observation_vector_z")]
Expand Down Expand Up @@ -128,15 +135,18 @@ pub struct StateTransitionMatrixBufferBuilder<const STATES: usize>;
#[doc(alias = "SystemCovarianceMatrixBufferBuilder")]
pub struct EstimateCovarianceMatrixBufferBuilder<const STATES: usize>;

/// A builder for direct process noise covariance matrices (`num_states` × `num_states`).
pub struct DirectProcessNoiseCovarianceMatrixBufferBuilder<const STATES: usize>;

/// A builder for control vectors (`num_controls` × `1`).
pub struct ControlVectorBufferBuilder<const CONTROLS: usize>;

/// A builder for control transition matrices (`num_states` × `num_controls`).
pub struct ControlTransitionMatrixBufferBuilder<const STATES: usize, const CONTROLS: usize>;

/// 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<const CONTROLS: usize>;
pub struct ControlProcessNoiseCovarianceMatrixBufferBuilder<const CONTROLS: usize>;

/// A builder for measurement vectors (`num_measurements` × `1`).
pub struct ObservationVectorBufferBuilder<const OBSERVATIONS: usize>;
Expand Down Expand Up @@ -455,46 +465,124 @@ impl<const STATES: usize, const CONTROLS: usize>
}
}

/// 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<const CONTROLS: usize, T> =
ProcessNoiseCovarianceMatrixMutBuffer<CONTROLS, T, MatrixDataBoxed<CONTROLS, CONTROLS, T>>;
pub type DirectProcessNoiseCovarianceMatrixBufferOwnedType<const STATES: usize, T> =
DirectProcessNoiseCovarianceMatrixMutBuffer<STATES, T, MatrixDataBoxed<STATES, STATES, T>>;

impl<const CONTROLS: usize> ProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS> {
/// Builds a new [`ProcessNoiseCovarianceMatrixMutBuffer`] that owns its data.
impl<const CONTROLS: usize> DirectProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS> {
/// 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<T>(
&self,
) -> DirectProcessNoiseCovarianceMatrixMutBuffer<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>
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<T>(
&self,
init: T,
) -> DirectProcessNoiseCovarianceMatrixMutBuffer<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>
where
T: Copy,
{
DirectProcessNoiseCovarianceMatrixMutBuffer::<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>::new(MatrixData::new_boxed::<CONTROLS, CONTROLS, T, _>(
alloc::vec![init; CONTROLS * CONTROLS],
))
}
}

/// The type of owned control process noise matrix buffers.
#[cfg_attr(docsrs, doc(cfg(feature = "alloc")))]
pub type ControlProcessNoiseCovarianceMatrixBufferOwnedType<const CONTROLS: usize, T> =
ControlProcessNoiseCovarianceMatrixMutBuffer<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>;

impl<const CONTROLS: usize> ControlProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS> {
/// 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)]
#[cfg_attr(docsrs, doc(cfg(feature = "alloc")))]
#[cfg(feature = "alloc")]
pub fn new<T>(
&self,
) -> ProcessNoiseCovarianceMatrixMutBuffer<CONTROLS, T, MatrixDataBoxed<CONTROLS, CONTROLS, T>>
) -> ControlProcessNoiseCovarianceMatrixMutBuffer<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>
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)]
Expand All @@ -503,13 +591,21 @@ impl<const CONTROLS: usize> ProcessNoiseCovarianceMatrixBufferBuilder<CONTROLS>
pub fn new_with<T>(
&self,
init: T,
) -> ProcessNoiseCovarianceMatrixMutBuffer<CONTROLS, T, MatrixDataBoxed<CONTROLS, CONTROLS, T>>
) -> ControlProcessNoiseCovarianceMatrixMutBuffer<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>
where
T: Copy,
{
ProcessNoiseCovarianceMatrixMutBuffer::<CONTROLS, T, MatrixDataBoxed<CONTROLS, CONTROLS, T>>::new(
MatrixData::new_boxed::<CONTROLS, CONTROLS, T, _>(alloc::vec![init; CONTROLS * CONTROLS]),
)
ControlProcessNoiseCovarianceMatrixMutBuffer::<
CONTROLS,
T,
MatrixDataBoxed<CONTROLS, CONTROLS, T>,
>::new(MatrixData::new_boxed::<CONTROLS, CONTROLS, T, _>(
alloc::vec![init; CONTROLS * CONTROLS],
))
}
}

Expand Down
6 changes: 4 additions & 2 deletions crates/minikalman/src/buffers/types.rs
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,14 @@
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;
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;
Expand All @@ -19,15 +20,16 @@ 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::*;
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::*;
Expand Down
Loading

0 comments on commit b67ddd9

Please sign in to comment.