From 9a46e643eb9dda70202f6ec7d6ea5f7088cc982c Mon Sep 17 00:00:00 2001 From: Markus Mayer Date: Sat, 22 Jun 2024 16:07:02 +0200 Subject: [PATCH 1/2] Add missing process noise trait implementations Signed-off-by: Markus Mayer --- crates/minikalman/src/controls.rs | 5 +- crates/minikalman/src/kalman/extended.rs | 31 +++++++- crates/minikalman/src/kalman/filter_trait.rs | 80 +++++++++++++++----- crates/minikalman/src/kalman/regular.rs | 4 +- crates/minikalman/src/static_macros.rs | 2 +- 5 files changed, 94 insertions(+), 28 deletions(-) diff --git a/crates/minikalman/src/controls.rs b/crates/minikalman/src/controls.rs index 1b68ca6..c2adc69 100644 --- a/crates/minikalman/src/controls.rs +++ b/crates/minikalman/src/controls.rs @@ -288,7 +288,7 @@ where } impl - KalmanFilterProcessNoiseCovariance + KalmanFilterControlProcessNoiseCovariance for Control where Q: ControlProcessNoiseCovarianceMatrix, @@ -301,7 +301,8 @@ where } impl - KalmanFilterControlCovarianceMut for Control + KalmanFilterControlProcessNoiseMut + for Control where Q: ControlProcessNoiseCovarianceMatrixMut, { diff --git a/crates/minikalman/src/kalman/extended.rs b/crates/minikalman/src/kalman/extended.rs index 82326b7..f9363ae 100644 --- a/crates/minikalman/src/kalman/extended.rs +++ b/crates/minikalman/src/kalman/extended.rs @@ -230,7 +230,7 @@ where impl ExtendedKalman where - P: DirectProcessNoiseCovarianceMatrix, + Q: DirectProcessNoiseCovarianceMatrix, { /// Gets a reference to the direct process noise matrix Q. /// @@ -246,7 +246,7 @@ where impl ExtendedKalman where - P: DirectProcessNoiseCovarianceMatrixMut, + Q: DirectProcessNoiseCovarianceMatrixMut, { /// Gets a mutable reference to the direct process noise matrix Q. /// @@ -603,6 +603,33 @@ where } } +impl + KalmanFilterDirectProcessNoiseCovariance + for ExtendedKalman +where + Q: DirectProcessNoiseCovarianceMatrix, +{ + type ProcessNoiseCovarianceMatrix = Q; + + #[inline(always)] + fn direct_process_noise(&self) -> &Self::ProcessNoiseCovarianceMatrix { + self.direct_process_noise() + } +} + +impl KalmanFilterDirectProcessNoiseMut + for ExtendedKalman +where + Q: DirectProcessNoiseCovarianceMatrixMut, +{ + type ProcessNoiseCovarianceMatrixMut = Q; + + #[inline(always)] + fn direct_process_noise_mut(&mut self) -> &mut Self::ProcessNoiseCovarianceMatrixMut { + self.direct_process_noise_mut() + } +} + impl KalmanFilterNonlinearPredict for ExtendedKalman where diff --git a/crates/minikalman/src/kalman/filter_trait.rs b/crates/minikalman/src/kalman/filter_trait.rs index 9337738..6cb3103 100644 --- a/crates/minikalman/src/kalman/filter_trait.rs +++ b/crates/minikalman/src/kalman/filter_trait.rs @@ -30,7 +30,7 @@ pub trait KalmanFilterControl: + KalmanFilterNumControls + KalmanFilterControlVectorMut + KalmanFilterControlTransition - + KalmanFilterControlCovarianceMut + + KalmanFilterControlProcessNoiseMut { } @@ -87,7 +87,7 @@ where + KalmanFilterNumControls + KalmanFilterControlVectorMut + KalmanFilterControlTransition - + KalmanFilterControlCovarianceMut + + KalmanFilterControlProcessNoiseMut + KalmanFilterControlApplyToFilter, { } @@ -294,8 +294,10 @@ pub trait KalmanFilterEstimateCovariance { /// Gets a reference to the estimate covariance matrix P. /// - /// This matrix represents the uncertainty in the state estimate. It quantifies how much the - /// state estimate is expected to vary, providing a measure of confidence in the estimate. + /// This matrix represents the estimate covariance. It quantifies the uncertainty in + /// the state estimate, providing a measure of how much the state estimate is expected + /// to vary. This matrix offers a measure of confidence in the estimate by indicating + /// the degree of variability and uncertainty associated with the predicted state. #[doc(alias = "system_covariance")] fn estimate_covariance(&self) -> &Self::EstimateCovarianceMatrix; } @@ -307,13 +309,41 @@ pub trait KalmanFilterEstimateCovarianceMut: /// Gets a mutable reference to the estimate covariance matrix P. /// - /// This matrix represents the uncertainty in the state estimate. It quantifies how much the - /// state estimate is expected to vary, providing a measure of confidence in the estimate. + /// This matrix represents the estimate covariance. It quantifies the uncertainty in + /// the state estimate, providing a measure of how much the state estimate is expected + /// to vary. This matrix offers a measure of confidence in the estimate by indicating + /// the degree of variability and uncertainty associated with the predicted state. #[doc(alias = "kalman_get_system_covariance")] #[doc(alias = "system_covariance_mut")] fn estimate_covariance_mut(&mut self) -> &mut Self::EstimateCovarianceMatrixMut; } +pub trait KalmanFilterDirectProcessNoiseCovariance { + type ProcessNoiseCovarianceMatrix: DirectProcessNoiseCovarianceMatrix; + + /// Gets a reference to the direct process noise matrix Q. + /// + /// 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. + fn direct_process_noise(&self) -> &Self::ProcessNoiseCovarianceMatrix; +} + +pub trait KalmanFilterDirectProcessNoiseMut: + KalmanFilterDirectProcessNoiseCovariance +{ + type ProcessNoiseCovarianceMatrixMut: DirectProcessNoiseCovarianceMatrixMut; + + /// Gets a mutable reference to the direct process noise matrix Q. + /// + /// 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. + fn direct_process_noise_mut(&mut self) -> &mut Self::ProcessNoiseCovarianceMatrixMut; +} + pub trait KalmanFilterNumControls { /// The number of controls. const NUM_CONTROLS: usize = CONTROLS; @@ -365,8 +395,10 @@ pub trait KalmanFilterControlTransition &Self::ControlTransitionMatrix; } @@ -377,35 +409,41 @@ pub trait KalmanFilterControlTransitionMut &mut Self::ControlTransitionMatrixMut; } #[doc(alias = "KalmanFilterControlCovariance")] -pub trait KalmanFilterProcessNoiseCovariance { +pub trait KalmanFilterControlProcessNoiseCovariance { type ProcessNoiseCovarianceMatrix: ControlProcessNoiseCovarianceMatrix; - /// Gets a reference to the control covariance matrix Q. + /// Gets a reference to the control process noise matrix Q. /// - /// This matrix represents the uncertainty in the state transition process, accounting for the - /// randomness and inaccuracies in the model. It quantifies the expected variability in the - /// state transition. + /// 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 used as B×Q×Bᵀ, where B + /// represents the control input model, and Q is the process noise covariance (this matrix). #[doc(alias = "control_covariance")] fn process_noise_covariance(&self) -> &Self::ProcessNoiseCovarianceMatrix; } -pub trait KalmanFilterControlCovarianceMut: - KalmanFilterProcessNoiseCovariance +pub trait KalmanFilterControlProcessNoiseMut: + KalmanFilterControlProcessNoiseCovariance { type ProcessNoiseCovarianceMatrixMut: ControlProcessNoiseCovarianceMatrixMut; - /// Gets a mutable reference to the control covariance matrix Q. + /// Gets a mutable reference to the control process noise matrix Q. /// - /// This matrix represents the uncertainty in the state transition process, accounting for the - /// randomness and inaccuracies in the model. It quantifies the expected variability in the - /// state transition. + /// 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 used as B×Q×Bᵀ, where B + /// represents the control input model, and Q is the process noise covariance (this matrix). #[doc(alias = "kalman_get_control_covariance")] #[doc(alias = "control_covariance_mut")] fn process_noise_covariance_mut(&mut self) -> &mut Self::ProcessNoiseCovarianceMatrixMut; diff --git a/crates/minikalman/src/kalman/regular.rs b/crates/minikalman/src/kalman/regular.rs index ed4c8e9..5eee6e1 100644 --- a/crates/minikalman/src/kalman/regular.rs +++ b/crates/minikalman/src/kalman/regular.rs @@ -682,7 +682,7 @@ where impl RegularKalman where - P: DirectProcessNoiseCovarianceMatrix, + Q: DirectProcessNoiseCovarianceMatrix, { /// Gets a reference to the direct process noise matrix Q. /// @@ -698,7 +698,7 @@ where impl RegularKalman where - P: DirectProcessNoiseCovarianceMatrixMut, + Q: DirectProcessNoiseCovarianceMatrixMut, { /// Gets a mutable reference to the direct process noise matrix Q. /// diff --git a/crates/minikalman/src/static_macros.rs b/crates/minikalman/src/static_macros.rs index 9381bfd..2d2c065 100644 --- a/crates/minikalman/src/static_macros.rs +++ b/crates/minikalman/src/static_macros.rs @@ -427,7 +427,7 @@ macro_rules! impl_buffer_B { /// 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 +/// in the control process. The matrix is used as B×Q×Bᵀ, where B /// represents the control input model, and Q is the process noise covariance (this matrix). /// /// ## Arguments From 0422f654f8fe9d24d15ab7f737f8f78be6768101 Mon Sep 17 00:00:00 2001 From: Markus Mayer Date: Sat, 22 Jun 2024 16:47:43 +0200 Subject: [PATCH 2/2] Add velocity measurement to radar example Signed-off-by: Markus Mayer --- crates/minikalman/examples/radar_2d.rs | 45 ++++++++++++++++++++------ 1 file changed, 36 insertions(+), 9 deletions(-) diff --git a/crates/minikalman/examples/radar_2d.rs b/crates/minikalman/examples/radar_2d.rs index 87662c4..a1f735a 100644 --- a/crates/minikalman/examples/radar_2d.rs +++ b/crates/minikalman/examples/radar_2d.rs @@ -11,7 +11,7 @@ use minikalman::extended::builder::KalmanFilterBuilder; use minikalman::prelude::*; const NUM_STATES: usize = 4; // position (x, y), velocity (x, y) -const NUM_OBSERVATIONS: usize = 2; // distance and angle to object +const NUM_OBSERVATIONS: usize = 3; // distance, angle to object and object velocity #[allow(non_snake_case)] fn main() { @@ -40,15 +40,15 @@ fn main() { // Set up the initial estimate covariance as an identity matrix. filter.estimate_covariance_mut().make_identity(); - // Set up the process noise covariance matrix as an identity matrix. - measurement - .measurement_noise_covariance_mut() - .make_identity(); + // Set up the process noise covariance matrix. + filter.direct_process_noise_mut().make_scalar(0.1); // Set up the measurement noise covariance. - measurement - .measurement_noise_covariance_mut() - .make_identity(); + measurement.measurement_noise_covariance_mut().apply(|mat| { + mat.set_at(0, 0, 0.5); + mat.set_at(1, 1, 0.1); + mat.set_at(2, 2, 0.2); + }); // Simulate for step in 1..=100 { @@ -81,38 +81,60 @@ fn main() { let mut rng = rand::thread_rng(); let measurement_noise_pos = Normal::new(0.0, 0.5).unwrap(); let measurement_noise_angle = Normal::new(0.0, 0.1).unwrap(); + let measurement_noise_vel = Normal::new(0.0, 0.2).unwrap(); + + let dist_norm = ((time - rx).powi(2) + (time - ry).powi(2)).sqrt(); // Perform a noisy measurement of the (simulated) position. - let z = ((time - rx).powi(2) + (time - ry).powi(2)).sqrt(); + let z = dist_norm; let noise_pos = measurement_noise_pos.sample(&mut rng); // Perform a noisy measurement of the (simulated) angle. let theta = (time - ry).atan2(time - rx); let noise_theta = measurement_noise_angle.sample(&mut rng); + // Perform a noisy measurement of the (simulated) velocity. + let v = ((time - rx) + (time - ry)) / dist_norm; + let noise_v = measurement_noise_vel.sample(&mut rng); + vec.set_row(0, z + noise_pos); vec.set_row(1, theta + noise_theta); + vec.set_row(2, v + noise_v); }); // Update the observation Jacobian. measurement.observation_jacobian_matrix_mut().apply(|mat| { let x = filter.state_vector().get_row(0); let y = filter.state_vector().get_row(1); + let vx = filter.state_vector().get_row(2); + let vy = filter.state_vector().get_row(3); let norm_sq = (x - rx).powi(2) + (y - ry).powi(2); let norm = norm_sq.sqrt(); let dx = x / norm; let dy = y / norm; + // Partial derivatives of position with respect to the state vector. mat.set_at(0, 0, dx); mat.set_at(0, 1, dy); mat.set_at(0, 2, 0.0); mat.set_at(0, 3, 0.0); + // Partial derivatives of angle with respect to the state vector. mat.set_at(1, 0, -(y - ry) / norm_sq); mat.set_at(1, 1, (x - rx) / norm_sq); mat.set_at(1, 2, 0.0); mat.set_at(1, 3, 0.0); + + // Partial derivatives of velocity with respect to the state vector. + mat.set_at(2, 0, (y - ry) * vy / norm_sq.powi(3).sqrt()); + mat.set_at( + 2, + 1, + ((x - rx) * vy + (y - ry) * vx) / norm_sq.powi(3).sqrt(), + ); + mat.set_at(2, 2, dx); + mat.set_at(2, 3, dy); }); // Apply nonlinear correction step. @@ -120,10 +142,15 @@ fn main() { // Transform the state into an observation. let x = state.get_row(0); let y = state.get_row(1); + let vx = state.get_row(2); + let vy = state.get_row(3); + let z = ((x - rx).powi(2) + (y - ry).powi(2)).sqrt(); let theta = (y - ry).atan2(x - rx); + let v = ((x - rx) * vx + (y - ry) * vy) / z; observation.set_row(0, z); observation.set_row(1, theta); + observation.set_row(2, v); }); print_state(time, &filter, Stage::Posterior);