Refactor UKF update for numerical stability and formatting improvements

core/src/earth.rs

@@ -77,43 +77,50 @@ pub const DEGREES_TO_METERS: f64 = 60.0 * 1852.0;
 /// Atmospheric pressure at sea level in Pascals ($P_0$)
 pub const SEA_LEVEL_PRESSURE: f64 = 101325.0;
 /// Standard temperature at sea level in Kelvin ($T_0$)
-pub const SEA_LEVEL_TEMPERATURE: f64 = 288.15; 
+pub const SEA_LEVEL_TEMPERATURE: f64 = 288.15;
 /// Molar mass of dry air in kg/mol ($M$)
-pub const MOLAR_MASS_DRY_AIR: f64 = 0.0289644; 
+pub const MOLAR_MASS_DRY_AIR: f64 = 0.0289644;
 /// Universal gas constant in J/(mol·K) ($R_0$)
 pub const UNIVERSAL_GAS_CONSTANT: f64 = 8.314462618;
 /// Standard lapse rate in K/m ($L$)
 pub const STANDARD_LAPSE_RATE: f64 = 0.0065;
 /// Calculate a barometric altitude from a measured pressure
-/// 
+///
 /// This function calculates the altitude above sea level based on the measured pressure
 /// using the barometric formula. The formula assumes a standard atmosphere and uses the
 /// universal gas constant, standard lapse rate, and molar mass of dry air.
-/// 
+///
 /// # Parameters
 /// - `pressure` - The measured pressure in Pascals
 /// # Returns
 /// The calculated altitude in meters above sea level
 pub fn barometric_altitude(pressure: &f64) -> f64 {
-    let exponent: f64 = - (UNIVERSAL_GAS_CONSTANT * STANDARD_LAPSE_RATE) / (G0 * MOLAR_MASS_DRY_AIR);
-    (SEA_LEVEL_PRESSURE / STANDARD_LAPSE_RATE) * (pressure / SEA_LEVEL_PRESSURE - 1.0) * exponent.exp()
+    let exponent: f64 = -(UNIVERSAL_GAS_CONSTANT * STANDARD_LAPSE_RATE) / (G0 * MOLAR_MASS_DRY_AIR);
+    (SEA_LEVEL_PRESSURE / STANDARD_LAPSE_RATE)
+        * (pressure / SEA_LEVEL_PRESSURE - 1.0)
+        * exponent.exp()
 }
 /// Calculate the relative barometric altitude from a measured pressure
-/// 
+///
 /// This function calculates the relative altitude from a reference pressure using the
-/// barometric formula. The formula assumes a standard atmosphere and uses the universal 
+/// barometric formula. The formula assumes a standard atmosphere and uses the universal
 /// gas constant, standard lapse rate, and molar mass of dry air.
-/// 
+///
 /// # Parameters
 /// - `pressure` - The measured pressure in Pascals
 /// - `initial_pressure` - The reference pressure in Pascals
 /// - `average_temperature` - Optional average temperature in Kelvin (defaults to standard sea level temperature)
-/// 
+///
 /// # Returns
 /// The calculated relative altitude in meters above the reference pressure
-pub fn relative_barometric_altitude(pressure: f64, initial_pressure: f64, average_temperature: Option<f64>) -> f64 {
+pub fn relative_barometric_altitude(
+    pressure: f64,
+    initial_pressure: f64,
+    average_temperature: Option<f64>,
+) -> f64 {
     let average_temperature = average_temperature.unwrap_or(SEA_LEVEL_TEMPERATURE);
-    ((UNIVERSAL_GAS_CONSTANT * average_temperature) / (G0 * MOLAR_MASS_DRY_AIR)) * (initial_pressure / pressure).ln()
+    ((UNIVERSAL_GAS_CONSTANT * average_temperature) / (G0 * MOLAR_MASS_DRY_AIR))
+        * (initial_pressure / pressure).ln()
 }
 /// Calculates the expected barometric pressure at a given altitude
 ///
@@ -138,7 +145,8 @@ pub fn expected_barometric_pressure(altitude: f64, sea_level_pressure: f64) -> f
     // Barometric formula (assuming isothermal atmosphere):
     // P = P0 * exp(- (g0 * M * h) / (R * T0))
     // Here we use the same constants as in barometric_altitude
-    let exponent = - (G0 * MOLAR_MASS_DRY_AIR * altitude) / (UNIVERSAL_GAS_CONSTANT * SEA_LEVEL_TEMPERATURE);
+    let exponent =
+        -(G0 * MOLAR_MASS_DRY_AIR * altitude) / (UNIVERSAL_GAS_CONSTANT * SEA_LEVEL_TEMPERATURE);
     sea_level_pressure * exponent.exp()
 }
 /// Convert a three-element vector to a skew-symmetric matrix

core/src/filter.rs

@@ -43,7 +43,8 @@ pub trait MeasurementModel {
 }
 /// GPS position measurement model
 #[derive(Clone, Debug, Default)]
-pub struct GPSPositionMeasurement {  // <-- Check this model for degree/radian consistency
+pub struct GPSPositionMeasurement {
+    // <-- Check this model for degree/radian consistency
     /// latitude in degrees
     pub latitude: f64,
     /// longitude in degrees
@@ -183,9 +184,9 @@ impl MeasurementModel for GPSPositionAndVelocityMeasurement {
     }
 }
 
-/// A relative relative altitude measurement derived from barometric pressure. 
-/// Note that this measurement model is an altitude measurement derived from 
-/// a barometric altimeter and not a direct calculation of altitude from the 
+/// A relative relative altitude measurement derived from barometric pressure.
+/// Note that this measurement model is an altitude measurement derived from
+/// a barometric altimeter and not a direct calculation of altitude from the
 /// barometric pressure.
 #[derive(Clone, Debug, Default)]
 pub struct RelativeAltitudeMeasurement {
@@ -205,7 +206,8 @@ impl MeasurementModel for RelativeAltitudeMeasurement {
         DMatrix::from_diagonal(&DVector::from_vec(vec![5.0])) // 1 mm noise
     }
     fn get_sigma_points(&self, state_sigma_points: &DMatrix<f64>) -> DMatrix<f64> {
-        let mut measurement_sigma_points = DMatrix::<f64>::zeros(self.get_dimension(), state_sigma_points.ncols());
+        let mut measurement_sigma_points =
+            DMatrix::<f64>::zeros(self.get_dimension(), state_sigma_points.ncols());
         for (i, sigma_point) in state_sigma_points.column_iter().enumerate() {
             measurement_sigma_points[(0, i)] = sigma_point[2];
         }
@@ -244,7 +246,7 @@ pub struct StrapdownParams {
 /// Note that, internally, angles are always stored in radians (both for the attitude and the position),
 /// however, the user can choose to convert them to degrees when retrieving the state vector and the UKF
 /// and underlying strapdown state can be constructed from data in degrees by using the boolean `in_degrees`
-/// toggle where applicable. Generally speaking, the design of this crate is such that methods that expect 
+/// toggle where applicable. Generally speaking, the design of this crate is such that methods that expect
 /// a WGS84 coordinate (e.g. latitude or longitude) will expect the value in degrees, whereas trigonometric
 /// functions (e.g. sine, cosine, tangent) will expect the value in radians.
 pub struct UKF {
@@ -370,7 +372,7 @@ impl UKF {
             lambda,
             state_size,
             weights_mean,
-            weights_cov
+            weights_cov,
         }
     }
     /// Predicts the state using the strapdown equations and IMU measurements.
@@ -389,20 +391,20 @@ impl UKF {
         let mut sigma_points = self.get_sigma_points();
         for i in 0..sigma_points.ncols() {
             let mut sigma_point_vec = sigma_points.column(i).clone_owned();
-            let mut state = StrapdownState { 
-                latitude: sigma_point_vec[0], 
-                longitude: sigma_point_vec[1], 
-                altitude: sigma_point_vec[2], 
-                velocity_north: sigma_point_vec[3], 
-                velocity_east:  sigma_point_vec[4], 
-                velocity_down: sigma_point_vec[5], 
+            let mut state = StrapdownState {
+                latitude: sigma_point_vec[0],
+                longitude: sigma_point_vec[1],
+                altitude: sigma_point_vec[2],
+                velocity_north: sigma_point_vec[3],
+                velocity_east: sigma_point_vec[4],
+                velocity_down: sigma_point_vec[5],
                 attitude: Rotation3::from_euler_angles(
-                    sigma_point_vec[6], 
-                    sigma_point_vec[7], 
-                    sigma_point_vec[8]
+                    sigma_point_vec[6],
+                    sigma_point_vec[7],
+                    sigma_point_vec[8],
                 ),
                 coordinate_convention: true,
-             };
+            };
             forward(&mut state, imu_data, dt);
             // Update the sigma point with the new state
             sigma_point_vec[0] = state.latitude;
@@ -478,10 +480,7 @@ impl UKF {
     /// # Arguments
     /// * `measurement` - The measurement vector to update the state with.
     /// * `measurement_sigma_points` - The measurement sigma points to use for the update.
-    pub fn update<M: MeasurementModel>(
-        &mut self,
-        measurement: M,
-    ) {
+    pub fn update<M: MeasurementModel>(&mut self, measurement: M) {
         let measurement_sigma_points = measurement.get_sigma_points(&self.get_sigma_points());
         // Calculate expected measurement
         let mut z_hat = DVector::<f64>::zeros(measurement.get_dimension());
@@ -498,7 +497,8 @@ impl UKF {
         s += measurement.get_noise();
         // Calculate the cross-covariance
         let sigma_points = self.get_sigma_points();
-        let mut cross_covariance = DMatrix::<f64>::zeros(self.state_size, measurement.get_dimension());
+        let mut cross_covariance =
+            DMatrix::<f64>::zeros(self.state_size, measurement.get_dimension());
         for (i, measurement_sigma_point) in measurement_sigma_points.column_iter().enumerate() {
             let measurement_diff = measurement_sigma_point - &z_hat;
             let state_diff = sigma_points.column(i) - &self.mean_state;
@@ -522,10 +522,16 @@ impl UKF {
         self.mean_state[8] = wrap_to_2pi(self.mean_state[8]);
         // Switch to Joseph Form update here
         // P = (I - K * H) P (I - K H)^T + K R K^T
-        let I = DMatrix::<f64>::identity(self.state_size, self.state_size);
-        let P = self.covariance.clone();
+        let i = DMatrix::<f64>::identity(self.state_size, self.state_size);
+        let p = self.covariance.clone();
+        // println!("Updating covariance matrix");
+        // println!("Kalman gain: {:?}", k.shape());
+        // println!("Cross covariance: {:?}", cross_covariance.shape());
+        let m = &i - &k * &cross_covariance.transpose();
+        self.covariance =
+            &m * &p * &m.transpose()
+                + &k * measurement.get_noise() * &k.transpose();
         //self.covariance -= &k * s * &k.transpose();
-        self.covariance = (&I - &k * &cross_covariance) * &P * (&I - &k * &cross_covariance).transpose() + &k * measurement.get_noise() * &k.transpose();
     }
 }
 #[derive(Clone, Debug, Default)]
@@ -667,8 +673,8 @@ impl ParticleFilter {
 /// Tests
 #[cfg(test)]
 mod tests {
-    use crate::earth;
     use super::*;
+    use crate::earth;
     use assert_approx_eq::assert_approx_eq;
     use nalgebra::Vector3;
 
@@ -706,10 +712,7 @@ mod tests {
             BETA,
             KAPPA,
         );
-        assert_eq!(
-            ukf.mean_state.len(),
-            18
-        );
+        assert_eq!(ukf.mean_state.len(), 18);
         let wms = ukf.weights_mean;
         let wcs = ukf.weights_cov;
         assert_eq!(wms.len(), (2 * ukf.state_size) + 1);
@@ -777,12 +780,12 @@ mod tests {
             ukf.mean_state.len() == 15 //+ measurement_bias.len()
         );
         let measurement = GPSPositionMeasurement {
-                latitude: 0.0,
-                longitude: 0.0,
-                altitude: 0.0,
-                horizontal_noise_std: 1e-3,
-                vertical_noise_std: 1e-3,
-            };
+            latitude: 0.0,
+            longitude: 0.0,
+            altitude: 0.0,
+            horizontal_noise_std: 1e-3,
+            vertical_noise_std: 1e-3,
+        };
         ukf.update(measurement);
         // Check that the state has not changed
         assert_approx_eq!(ukf.mean_state[0], 0.0, 1e-3);

core/src/main.rs

@@ -2,29 +2,61 @@ use clap::Parser;
 use csv::ReaderBuilder;
 use std::error::Error;
 use std::path::PathBuf;
-use strapdown::sim::{NavigationResult, TestDataRecord, closed_loop, dead_reckoning, degrade_measurements};
+use strapdown::sim::{
+    NavigationResult, TestDataRecord, closed_loop, dead_reckoning, degrade_measurements,
+};
 
 /// Command line arguments
 #[derive(Parser, Debug)]
 #[clap(author, version, about, long_about = None)]
 struct Args {
     /// Mode of operation, either open-loop or closed-loop
-    #[clap(short, long, value_parser, default_value = "open-loop", help = "Mode of operation: 'open-loop' or 'closed-loop'")]
+    #[clap(
+        short,
+        long,
+        value_parser,
+        default_value = "open-loop",
+        help = "Mode of operation: 'open-loop' or 'closed-loop'"
+    )]
     mode: String,
     /// Input CSV file path
-    #[clap(short, long, value_parser, help = "Path to the input CSV file containing IMU and (optionally) GPS data")]
+    #[clap(
+        short,
+        long,
+        value_parser,
+        help = "Path to the input CSV file containing IMU and (optionally) GPS data"
+    )]
     input: PathBuf,
     /// Output CSV file path
-    #[clap(short, long, value_parser, help = "Path to the output CSV file for navigation results")]
+    #[clap(
+        short,
+        long,
+        value_parser,
+        help = "Path to the output CSV file for navigation results"
+    )]
     output: PathBuf,
     /// Optional: GPS measurement interval in seconds (for simulating intermittent GPS outages)
-    #[clap(long, value_parser, help = "Interval (in seconds) between GPS measurements, used to simulate GPS outages. Default is 0.0 which updates at every iteration.")]
+    #[clap(
+        long,
+        value_parser,
+        help = "Interval (in seconds) between GPS measurements, used to simulate GPS outages. Default is 0.0 which updates at every iteration."
+    )]
     gps_interval: Option<f64>,
     /// Optional: GPS degradation factor
-    #[clap(long, value_parser, default_value = "1.0", help = "Factor by which GPS accuracy is degraded. 1.0 is no degradation. >= 1.0 is a degradation factor. This factor is applied as a scalar multiplier to the recorded GPS accuracy.")]
+    #[clap(
+        long,
+        value_parser,
+        default_value = "1.0",
+        help = "Factor by which GPS accuracy is degraded. 1.0 is no degradation. >= 1.0 is a degradation factor. This factor is applied as a scalar multiplier to the recorded GPS accuracy."
+    )]
     gps_degradation: Option<f64>,
     /// Optional: GPS spoofing offset
-    #[clap(long, value_parser, default_value = "0.0", help = "Offset to apply to GPS coordinates (in meters)")]
+    #[clap(
+        long,
+        value_parser,
+        default_value = "0.0",
+        help = "Offset to apply to GPS coordinates (in meters)"
+    )]
     gps_spoofing_offset: Option<f64>,
 }
 fn main() -> Result<(), Box<dyn Error>> {