diff --git a/Cargo.lock b/Cargo.lock index d78c302a..28b9cdcf 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -338,6 +338,12 @@ version = "1.0.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + [[package]] name = "libyml" version = "0.0.5" @@ -436,6 +442,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", + "libm", ] [[package]] diff --git a/lib/localisation/Cargo.toml b/lib/localisation/Cargo.toml index e6fdb7a3..f3609e2a 100644 --- a/lib/localisation/Cargo.toml +++ b/lib/localisation/Cargo.toml @@ -4,5 +4,5 @@ version = "0.1.0" edition = "2021" [dependencies] -nalgebra = { version = "0.33.0", default-features = false } +nalgebra = { version = "0.33.0", default-features = false, features = ["libm"] } heapless = "0.8.0" diff --git a/lib/localisation/src/control.rs b/lib/localisation/src/control.rs new file mode 100644 index 00000000..0f450390 --- /dev/null +++ b/lib/localisation/src/control.rs @@ -0,0 +1 @@ +pub mod navigator; diff --git a/lib/localisation/src/control/navigator.rs b/lib/localisation/src/control/navigator.rs index e69de29b..8b137891 100644 --- a/lib/localisation/src/control/navigator.rs +++ b/lib/localisation/src/control/navigator.rs @@ -0,0 +1 @@ + diff --git a/lib/localisation/src/filtering.rs b/lib/localisation/src/filtering.rs new file mode 100644 index 00000000..2111f2df --- /dev/null +++ b/lib/localisation/src/filtering.rs @@ -0,0 +1 @@ +pub mod kalman_filter; diff --git a/lib/localisation/src/filtering/kalman_filter.rs b/lib/localisation/src/filtering/kalman_filter.rs index e69de29b..0adbb58f 100644 --- a/lib/localisation/src/filtering/kalman_filter.rs +++ b/lib/localisation/src/filtering/kalman_filter.rs @@ -0,0 +1,159 @@ +use nalgebra::{Matrix2, Vector1, Vector2, SVD}; + +/// Kalman filter implementation for cart for sensor fusion. +/// Recursively estimates the state from a series of nois measurements. +/// +/// Uses keyence, optical flow and accelerometer data. +/// +/// Control input: Accelerometer data +/// Measurement: Optical flow data, keyence stripe counter +pub struct KalmanFilter { + /// Current state estimate (2x1) + state: Vector2, + /// Current error covariance (2x2) + covariance: Matrix2, + /// State transition matrix (2x2) + transition_matrix: Matrix2, + /// Control matrix (2x1) + control_matrix: Vector2, + /// Observation matrix (1x2) + observation_matrix: Matrix2, + /// Process noise covariance (2x2) + process_noise: Matrix2, + /// Measurement noise covariance (1x1) + measurement_noise: Matrix2, +} + +impl KalmanFilter { + pub fn new( + initial_state: Vector2, + initial_covariance: Matrix2, + transition_matrix: Matrix2, + control_matrix: Vector2, + observation_matrix: Matrix2, + process_noise: Matrix2, + measurement_noise: Matrix2, + ) -> Self { + KalmanFilter { + state: initial_state, + covariance: initial_covariance, + transition_matrix, + observation_matrix, + control_matrix, + process_noise, + measurement_noise, + } + } + + /// Predict step + /// Predicts the next state of the system, using the accelerometer data as control input. + /// Predicts the next state covariance. + pub fn predict(&mut self, control_input: &Vector1) { + // x_k = F * x_k-1 + B * u_k + self.state = self.transition_matrix * self.state + self.control_matrix * control_input; + + // P_k = F * P_k-1 * F^T + Q + self.covariance = + self.transition_matrix * self.covariance * self.transition_matrix.transpose() + + self.process_noise; + } + + /// Update step: Corrects the state estimate based on the measurement. + /// Uses the stripe counter (displacement) and optical flow data (velocity). + pub fn update(&mut self, measurement: &Vector2) { + // y_k = z_k - H * x_k + let innovation = measurement - self.observation_matrix * self.state; + + // S = H * P_k * H^T + R + let innovation_covariance = + self.observation_matrix * self.covariance * self.observation_matrix.transpose() + + self.measurement_noise; + + // Calculate the pseudo-inverse using Singular Value Decomposition + // If the matrix is invertible, the pseudo-inverse is the same as the inverse + let svd = SVD::new(innovation_covariance, true, true); + let innovation_covariance_inv = svd.pseudo_inverse(1e-10).unwrap(); + + // K = P_k * H^T * S^-1 + let kalman_gain = + self.covariance * self.observation_matrix.transpose() * innovation_covariance_inv; + + self.state += kalman_gain * innovation; + + let identity = Matrix2::identity(); + self.covariance = (identity - kalman_gain * self.observation_matrix) * self.covariance; + } + + pub fn get_state(&self) -> Vector2 { + self.state + } +} + +#[cfg(test)] +mod tests { + + use super::*; + use nalgebra::{Matrix2, Vector1, Vector2}; + + #[test] + fn test_kalman_filter() { + // Test simulating simple cart movement + + // Acceleration: 20ms^-2 + // Initial velocity: 0 + // Initial position: 0 + // Measurements taken every 1s + // 20 measurements taken + + // Variance of measurements: + // Distance: 10 + // Velocity: 5 + // Acceleration: 3 + + // Expected displacement: 4000m + // Expected velocity: 400ms^-1 + + let initial_state = Vector2::new(0.0, 0.0); + let initial_covariance = Matrix2::new(1.0, 0.0, 0.0, 1.0); + let transition_matrix = Matrix2::new(1.0, 1.0, 0.0, 1.0); + let control_matrix = Vector2::new(0.5, 1.0); + let process_noise = Matrix2::new(0.25 * 3.0, 0.5 * 3.0, 0.5 * 3.0, 1.0 * 3.0); + let observation_matrix = Matrix2::new(1.0, 0.0, 0.0, 1.0); + let measurement_noise = Matrix2::new(1.0, 0.0, 0.0, 0.0); + + let mut kalman_filter = KalmanFilter::new( + initial_state, + initial_covariance, + transition_matrix, + control_matrix, + observation_matrix, + process_noise, + measurement_noise, + ); + + let acc_measurements = [ + 20.38, 15.02, 19.17, 19.36, 20.6, 16.89, 20.42, 20.33, 21.53, 19.98, 26.08, 17.63, + 21.01, 17.06, 21.83, 23.18, 26.03, 17.67, 22.99, 18.78, + ]; + let dist_measurements = [ + 28.59, 51.7, 87.64, 169.39, 244.96, 363.2, 493.0, 626.97, 817.84, 1021.23, 1192.21, + 1423.34, 1689.46, 1964.72, 2261.09, 2565.47, 2902.0, 3239.25, 3627.35, 4011.47, + ]; + let vel_measurements = [ + 20.06, 42.43, 66.23, 83.03, 104.47, 122.93, 135.85, 157.55, 179.02, 201.75, 216.53, + 240.63, 262.97, 285.75, 307.43, 321.04, 331.48, 354.47, 374.19, 394.29, + ]; + + for i in 0..20 { + let measurement = Vector2::new(dist_measurements[i], vel_measurements[i]); + let control_input = Vector1::new(acc_measurements[i]); + + kalman_filter.predict(&control_input); + kalman_filter.update(&measurement); + } + + let final_state = kalman_filter.get_state(); + assert!(final_state[0] - 4000.0 < 1e-10); + assert!(final_state[1] - 400.0 < 1e-10); + } +} diff --git a/lib/localisation/src/lib.rs b/lib/localisation/src/lib.rs index b0125968..b2d8976f 100644 --- a/lib/localisation/src/lib.rs +++ b/lib/localisation/src/lib.rs @@ -1,3 +1,4 @@ #![no_std] - +pub mod control; +pub mod filtering; pub mod preprocessing; diff --git a/lib/localisation/src/preprocessing.rs b/lib/localisation/src/preprocessing.rs index e1de1ed6..6f4132b3 100644 --- a/lib/localisation/src/preprocessing.rs +++ b/lib/localisation/src/preprocessing.rs @@ -1 +1,3 @@ +pub mod accelerometer; pub mod keyence; +pub mod optical; diff --git a/lib/localisation/src/preprocessing/accelerometer.rs b/lib/localisation/src/preprocessing/accelerometer.rs index 0ae040b6..8b137891 100644 --- a/lib/localisation/src/preprocessing/accelerometer.rs +++ b/lib/localisation/src/preprocessing/accelerometer.rs @@ -1 +1 @@ -//test +