Add velocity measurement to radar example

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,49 +81,76 @@ 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.
  filter.correct_nonlinear(&mut measurement, |state, observation| {
  // 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);

crates/minikalman/src/controls.rs

@@ -288,7 +288,7 @@ where
 }
 
 impl<const STATES: usize, const CONTROLS: usize, T, B, U, Q, TempBQ>
- KalmanFilterProcessNoiseCovariance<CONTROLS, T>
+ KalmanFilterControlProcessNoiseCovariance<CONTROLS, T>
  for Control<STATES, CONTROLS, T, B, U, Q, TempBQ>
 where
  Q: ControlProcessNoiseCovarianceMatrix<CONTROLS, T>,
@@ -301,7 +301,8 @@ where
 }
 
 impl<const STATES: usize, const CONTROLS: usize, T, B, U, Q, TempBQ>
- KalmanFilterControlCovarianceMut<CONTROLS, T> for Control<STATES, CONTROLS, T, B, U, Q, TempBQ>
+ KalmanFilterControlProcessNoiseMut<CONTROLS, T>
+ for Control<STATES, CONTROLS, T, B, U, Q, TempBQ>
 where
  Q: ControlProcessNoiseCovarianceMatrixMut<CONTROLS, T>,
 {

crates/minikalman/src/kalman/extended.rs

@@ -230,7 +230,7 @@
 
 impl<const STATES: usize, T, A, X, P, Q, PX, TempP> ExtendedKalman<STATES, T, A, X, P, Q, PX, TempP>
 where
- P: DirectProcessNoiseCovarianceMatrix<STATES, T>,
+ Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
 {
  /// Gets a reference to the direct process noise matrix Q.
  ///
@@ -246,7 +246,7 @@
 
 impl<const STATES: usize, T, A, X, P, Q, PX, TempP> ExtendedKalman<STATES, T, A, X, P, Q, PX, TempP>
 where
- P: DirectProcessNoiseCovarianceMatrixMut<STATES, T>,
+ Q: DirectProcessNoiseCovarianceMatrixMut<STATES, T>,
 {
  /// Gets a mutable reference to the direct process noise matrix Q.
  ///
@@ -603,6 +603,33 @@
  }
 }
 
+impl<const STATES: usize, T, A, X, P, Q, PX, TempP>
+ KalmanFilterDirectProcessNoiseCovariance<STATES, T>
+ for ExtendedKalman<STATES, T, A, X, P, Q, PX, TempP>
+where
+ Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
+{
+ type ProcessNoiseCovarianceMatrix = Q;
+
+ #[inline(always)]
+ fn direct_process_noise(&self) -> &Self::ProcessNoiseCovarianceMatrix {
+ self.direct_process_noise()
+ }
+}
+
+impl<const STATES: usize, T, A, X, P, Q, PX, TempP> KalmanFilterDirectProcessNoiseMut<STATES, T>
+ for ExtendedKalman<STATES, T, A, X, P, Q, PX, TempP>
+where
+ Q: DirectProcessNoiseCovarianceMatrixMut<STATES, T>,
+{
+ type ProcessNoiseCovarianceMatrixMut = Q;
+
+ #[inline(always)]
+ fn direct_process_noise_mut(&mut self) -> &mut Self::ProcessNoiseCovarianceMatrixMut {
+ self.direct_process_noise_mut()
+ }
+}
+
 impl<const STATES: usize, T, A, X, P, Q, PX, TempP> KalmanFilterNonlinearPredict<STATES, T>
  for ExtendedKalman<STATES, T, A, X, P, Q, PX, TempP>
 where

crates/minikalman/src/kalman/filter_trait.rs

@@ -30,7 +30,7 @@ pub trait KalmanFilterControl<const STATES: usize, const CONTROLS: usize, T>:
  + KalmanFilterNumControls<CONTROLS>
  + KalmanFilterControlVectorMut<CONTROLS, T>
  + KalmanFilterControlTransition<STATES, CONTROLS, T>
- + KalmanFilterControlCovarianceMut<CONTROLS, T>
+ + KalmanFilterControlProcessNoiseMut<CONTROLS, T>
 {
 }
 
@@ -87,7 +87,7 @@ where
  + KalmanFilterNumControls<CONTROLS>
  + KalmanFilterControlVectorMut<CONTROLS, T>
  + KalmanFilterControlTransition<STATES, CONTROLS, T>
- + KalmanFilterControlCovarianceMut<CONTROLS, T>
+ + KalmanFilterControlProcessNoiseMut<CONTROLS, T>
  + KalmanFilterControlApplyToFilter<STATES, T>,
 {
 }
@@ -294,8 +294,10 @@ pub trait KalmanFilterEstimateCovariance<const STATES: usize, T> {
 
  /// 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<const STATES: usize, T>:
 
  /// 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<const CONTROLS: usize, T> {
+ type ProcessNoiseCovarianceMatrix: DirectProcessNoiseCovarianceMatrix<CONTROLS, T>;
+
+ /// 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<const CONTROLS: usize, T>:
+ KalmanFilterDirectProcessNoiseCovariance<CONTROLS, T>
+{
+ type ProcessNoiseCovarianceMatrixMut: DirectProcessNoiseCovarianceMatrixMut<CONTROLS, T>;
+
+ /// 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<const CONTROLS: usize> {
  /// The number of controls.
  const NUM_CONTROLS: usize = CONTROLS;
@@ -365,8 +395,10 @@ pub trait KalmanFilterControlTransition<const STATES: usize, const CONTROLS: usi
 
  /// Gets a reference to the control transition matrix B.
  ///
- /// This matrix maps the control inputs to the state space, allowing the control vector to
- /// influence the state transition. It quantifies how the control inputs affect the state change.
+ /// This matrix represents the control input model. It defines how the control inputs
+ /// influence the state evolution from one time step to the next. The matrix \( B \)
+ /// is used to incorporate the effect of control inputs into the state transition,
+ /// allowing the model to account for external controls applied to the system.
  fn control_matrix(&self) -> &Self::ControlTransitionMatrix;
 }
 
@@ -377,35 +409,41 @@ pub trait KalmanFilterControlTransitionMut<const STATES: usize, const CONTROLS:
 
  /// Gets a mutable reference to the control transition matrix B.
  ///
- /// This matrix maps the control inputs to the state space, allowing the control vector to
- /// influence the state transition. It quantifies how the control inputs affect the state change.
+ /// This matrix represents the control input model. It defines how the control inputs
+ /// influence the state evolution from one time step to the next. The matrix \( B \)
+ /// is used to incorporate the effect of control inputs into the state transition,
+ /// allowing the model to account for external controls applied to the system.
  #[doc(alias = "kalman_get_control_matrix")]
  fn control_matrix_mut(&mut self) -> &mut Self::ControlTransitionMatrixMut;
 }
 
 #[doc(alias = "KalmanFilterControlCovariance")]
-pub trait KalmanFilterProcessNoiseCovariance<const CONTROLS: usize, T> {
+pub trait KalmanFilterControlProcessNoiseCovariance<const CONTROLS: usize, T> {
  type ProcessNoiseCovarianceMatrix: ControlProcessNoiseCovarianceMatrix<CONTROLS, T>;
 
- /// 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<const CONTROLS: usize, T>:
- KalmanFilterProcessNoiseCovariance<CONTROLS, T>
+pub trait KalmanFilterControlProcessNoiseMut<const CONTROLS: usize, T>:
+ KalmanFilterControlProcessNoiseCovariance<CONTROLS, T>
 {
  type ProcessNoiseCovarianceMatrixMut: ControlProcessNoiseCovarianceMatrixMut<CONTROLS, T>;
 
- /// 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;

crates/minikalman/src/kalman/regular.rs

@@ -682,7 +682,7 @@ where
 
 impl<const STATES: usize, T, A, X, P, Q, PX, TempP> RegularKalman<STATES, T, A, X, P, Q, PX, TempP>
 where
- P: DirectProcessNoiseCovarianceMatrix<STATES, T>,
+ Q: DirectProcessNoiseCovarianceMatrix<STATES, T>,
 {
  /// Gets a reference to the direct process noise matrix Q.
  ///
@@ -698,7 +698,7 @@ where
 
 impl<const STATES: usize, T, A, X, P, Q, PX, TempP> RegularKalman<STATES, T, A, X, P, Q, PX, TempP>
 where
- P: DirectProcessNoiseCovarianceMatrixMut<STATES, T>,
+ Q: DirectProcessNoiseCovarianceMatrixMut<STATES, T>,
 {
  /// Gets a mutable reference to the direct process noise matrix Q.
  ///

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