Merge pull request #239 from jbrodovsky/jbrodovsky/issue238

Fix EKF altitude bounds for magnetic field calculations

Author: jbrodovsky

core/src/sim.rs

@@ -2066,37 +2066,47 @@ pub fn print_ukf(ukf: &UnscentedKalmanFilter, record: &TestDataRecord) {
         ukf.get_certainty()[(14, 14)]
     );
 }
+
+/// Configuration parameters for UKF initialization.
+///
+/// This struct groups together optional parameters for initializing an Unscented Kalman Filter,
+/// reducing the number of function arguments and making it easier to specify custom configurations.
+#[derive(Debug, Clone, Default)]
+pub struct UkfConfig {
+    /// Optional vector of f64 representing the initial attitude covariance (default is a small value).
+    pub attitude_covariance: Option<Vec<f64>>,
+    /// Optional vector of f64 representing the initial IMU biases (default is a small value).
+    pub imu_biases: Option<Vec<f64>>,
+    /// Optional vector of f64 representing the IMU biases covariance.
+    pub imu_biases_covariance: Option<Vec<f64>>,
+    /// Optional vector of f64 for any additional states (not used in the canonical UKF, but can be useful for custom implementations).
+    pub other_states: Option<Vec<f64>>,
+    /// Optional vector of f64 for other states covariance.
+    pub other_states_covariance: Option<Vec<f64>>,
+    /// Optional process noise diagonal vector.
+    pub process_noise_diagonal: Option<Vec<f64>>,
+    /// Optional UKF alpha parameter (sigma-point spread).
+    pub ukf_alpha: Option<f64>,
+    /// Optional UKF beta parameter (prior distribution).
+    pub ukf_beta: Option<f64>,
+    /// Optional UKF kappa parameter (secondary spread control).
+    pub ukf_kappa: Option<f64>,
+}
+
 /// Helper function to initialize a UKF for closed-loop mode.
 ///
-/// This function sets up the Unscented Kalman Filter (UKF) with initial pose, attitude covariance, and IMU biases based on
-/// the provided `TestDataRecord`. It initializes the UKF with position, velocity, attitude, and covariance matrices.
-/// Optional parameters for attitude covariance and IMU biases can be provided to customize the filter's initial state.
+/// This function sets up the Unscented Kalman Filter (UKF) with initial pose and configuration parameters.
+/// It initializes the UKF with position, velocity, attitude, and covariance matrices.
 ///
 /// # Arguments
 ///
 /// * `initial_pose` - A `TestDataRecord` containing the initial pose information.
-/// * `attitude_covariance` - Optional vector of f64 representing the initial attitude covariance (default is a small value).
-/// * `imu_biases` - Optional vector of f64 representing the initial IMU biases (default is a small value).
-/// * `other_states` - Optional vector of f64 for any additional states (not used in the canonical UKF, but can be useful for custom implementations).
-/// * `ukf_alpha` - Optional UKF alpha parameter (sigma-point spread).
-/// * `ukf_beta` - Optional UKF beta parameter (prior distribution).
-/// * `ukf_kappa` - Optional UKF kappa parameter (secondary spread control).
+/// * `config` - A `UkfConfig` struct containing optional configuration parameters.
 ///
 /// # Returns
 ///
-/// * `UKF` - An instance of the Unscented Kalman Filter initialized with the provided parameters.
-pub fn initialize_ukf(
-    initial_pose: TestDataRecord,
-    attitude_covariance: Option<Vec<f64>>,
-    imu_biases: Option<Vec<f64>>,
-    imu_biases_covariance: Option<Vec<f64>>,
-    other_states: Option<Vec<f64>>,
-    other_states_covariance: Option<Vec<f64>>,
-    process_noise_diagonal: Option<Vec<f64>>,
-    ukf_alpha: Option<f64>,
-    ukf_beta: Option<f64>,
-    ukf_kappa: Option<f64>,
-) -> UnscentedKalmanFilter {
+/// * `UnscentedKalmanFilter` - An instance of the Unscented Kalman Filter initialized with the provided parameters.
+pub fn initialize_ukf(initial_pose: TestDataRecord, config: UkfConfig) -> UnscentedKalmanFilter {
     let initial_state = InitialState {
         latitude: initial_pose.latitude,
         longitude: initial_pose.longitude,
@@ -2122,7 +2132,7 @@ pub fn initialize_ukf(
         in_degrees: true,
         is_enu: true,
     };
-    let process_noise_diagonal = match process_noise_diagonal {
+    let process_noise_diagonal = match config.process_noise_diagonal {
         Some(pn) => pn,
         None => DEFAULT_PROCESS_NOISE.to_vec(),
     };
@@ -2138,14 +2148,14 @@ pub fn initialize_ukf(
         initial_pose.speed_accuracy.powf(2.0),
     ];
     // extend the covariance diagonal if attitude covariance is provided
-    match attitude_covariance {
+    match config.attitude_covariance {
         Some(att_cov) => covariance_diagonal.extend(att_cov),
         None => covariance_diagonal.extend(vec![1e-9; 3]), // Default values if not provided
     }
     // extend the covariance diagonal if imu biases are provided
-    let imu_biases = match imu_biases {
+    let imu_biases = match config.imu_biases {
         Some(imu_biases) => {
-            covariance_diagonal.extend(match imu_biases_covariance {
+            covariance_diagonal.extend(match config.imu_biases_covariance {
                 Some(imu_cov) => imu_cov,
                 None => vec![1e-3; 6], // Default covariance if not provided
             });
@@ -2157,9 +2167,9 @@ pub fn initialize_ukf(
         }
     };
     // extend the covariance diagonal if other states are provided
-    let other_states = match other_states {
+    let other_states = match config.other_states {
         Some(other_states) => {
-            covariance_diagonal.extend(match other_states_covariance {
+            covariance_diagonal.extend(match config.other_states_covariance {
                 Some(other_cov) => other_cov,
                 None => vec![1e-3; other_states.len()], // Default covariance if not provided
             });
@@ -2193,9 +2203,9 @@ pub fn initialize_ukf(
         other_states,
         covariance_diagonal,
         process_noise,
-        ukf_alpha.unwrap_or(1e-3),
-        ukf_beta.unwrap_or(2.0),
-        ukf_kappa.unwrap_or(0.0),
+        config.ukf_alpha.unwrap_or(1e-3),
+        config.ukf_beta.unwrap_or(2.0),
+        config.ukf_kappa.unwrap_or(0.0),
     )
 }
 
@@ -3718,18 +3728,7 @@ mod tests {
             });
 
         // Initialize UKF
-        let mut ukf = initialize_ukf(
-            rec.clone(),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-        );
+        let mut ukf = initialize_ukf(rec.clone(), UkfConfig::default());
 
         // Create a minimal EventStream with one IMU event
         let imu_data = IMUData {
@@ -3784,30 +3783,15 @@ mod tests {
             grav_y: 0.0,
             grav_x: 0.0,
         };
-        let ukf = initialize_ukf(
-            rec.clone(),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-        );
+        let ukf = initialize_ukf(rec.clone(), UkfConfig::default());
         assert!(!ukf.get_estimate().is_empty());
         let ukf2 = initialize_ukf(
             rec,
-            Some(vec![0.1, 0.2, 0.3]),
-            Some(vec![0.4, 0.5, 0.6, 0.7, 0.8, 0.9]),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
+            UkfConfig {
+                attitude_covariance: Some(vec![0.1, 0.2, 0.3]),
+                imu_biases: Some(vec![0.4, 0.5, 0.6, 0.7, 0.8, 0.9]),
+                ..Default::default()
+            },
         );
         assert!(!ukf2.get_estimate().is_empty());
     }
@@ -3997,18 +3981,7 @@ mod tests {
             yaw: 0.3,
             ..Default::default()
         };
-        let ukf = initialize_ukf(
-            rec.clone(),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-        );
+        let ukf = initialize_ukf(rec.clone(), UkfConfig::default());
         let timestamp = Utc::now();
         let nav_result = NavigationResult::from((&timestamp, &ukf));
 
@@ -4062,18 +4035,7 @@ mod tests {
             yaw: 0.3,
             ..Default::default()
         };
-        let ukf = initialize_ukf(
-            rec.clone(),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-        );
+        let ukf = initialize_ukf(rec.clone(), UkfConfig::default());
         // Just ensure it doesn't panic
         print_ukf(&ukf, &rec);
     }
@@ -4094,7 +4056,7 @@ mod tests {
             yaw: f64::NAN,
             ..Default::default()
         };
-        let ukf = initialize_ukf(rec, None, None, None, None, None, None, None, None, None);
+        let ukf = initialize_ukf(rec, UkfConfig::default());
         let estimate = ukf.get_estimate();
         // Should default NaN angles to 0.0
         assert!(estimate[6].abs() < 1e-6); // roll
@@ -4121,15 +4083,12 @@ mod tests {
         };
         let ukf = initialize_ukf(
             rec,
-            Some(vec![1e-4, 2e-4, 3e-4]),
-            Some(vec![0.01, 0.02, 0.03, 0.001, 0.002, 0.003]),
-            Some(vec![1e-5; 6]),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
+            UkfConfig {
+                attitude_covariance: Some(vec![1e-4, 2e-4, 3e-4]),
+                imu_biases: Some(vec![0.01, 0.02, 0.03, 0.001, 0.002, 0.003]),
+                imu_biases_covariance: Some(vec![1e-5; 6]),
+                ..Default::default()
+            },
         );
         let estimate = ukf.get_estimate();
         assert_eq!(estimate.len(), 15);
@@ -4155,15 +4114,10 @@ mod tests {
         let custom_noise = vec![1e-5; 15];
         let ukf = initialize_ukf(
             rec,
-            None,
-            None,
-            None,
-            None,
-            None,
-            Some(custom_noise),
-            None,
-            None,
-            None,
+            UkfConfig {
+                process_noise_diagonal: Some(custom_noise),
+                ..Default::default()
+            },
         );
         assert!(!ukf.get_estimate().is_empty());
     }
@@ -4651,18 +4605,7 @@ mod tests {
             ..Default::default()
         };
 
-        let mut ukf = initialize_ukf(
-            rec.clone(),
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-            None,
-        );
+        let mut ukf = initialize_ukf(rec.clone(), UkfConfig::default());
 
         let stream = EventStream {
             start_time: rec.time,

geonav/src/lib.rs

@@ -52,6 +52,11 @@ use strapdown::{IMUData, NavigationFilter, StrapdownState};
 /// Conversion factor from radians to degrees (180/π)
 const RAD_TO_DEG: f64 = 180.0 / std::f64::consts::PI;
 
+/// World Magnetic Model valid altitude range (meters)
+/// The WMM is typically valid from -1km below sea level to ~850km above
+const WMM_MIN_ALTITUDE_M: f64 = -1000.0;
+const WMM_MAX_ALTITUDE_M: f64 = 850000.0;
+
 //================= Map Information ========================================================================
 /// Resolution values for bathymetric or terrain relief maps
 #[derive(Clone, Copy, Debug, PartialEq, Eq)]
@@ -671,8 +676,11 @@ pub struct MagneticAnomalyMeasurement {
 }
 impl GeophysicalAnomalyMeasurementModel for MagneticAnomalyMeasurement {
     fn get_anomaly(&self) -> f64 {
+        // Clamp altitude to valid WMM range to prevent errors
+        let alt_clamped = self.altitude.clamp(WMM_MIN_ALTITUDE_M, WMM_MAX_ALTITUDE_M);
+
         let magnetic_field = GeomagneticField::new(
-            Length::new::<meter>(self.altitude as f32),
+            Length::new::<meter>(alt_clamped as f32),
             Angle::new::<degree>(self.latitude as f32),
             Angle::new::<degree>(self.longitude as f32),
             Date::from_ordinal_date(self.year, self.day).unwrap(),
@@ -700,16 +708,27 @@ impl MeasurementModel for MagneticAnomalyMeasurement {
         // Return the observed magnetic anomaly as the measurement vector.
         // Use provided state if available (for per-particle updates), otherwise fallback to stored state.
         let anomaly = if let Some((lat_deg, lon_deg, alt)) = self.extract_state_inputs(state) {
+            // Clamp altitude to valid WMM range to prevent errors
+            let alt_clamped = alt.clamp(WMM_MIN_ALTITUDE_M, WMM_MAX_ALTITUDE_M);
+
+            if (alt - alt_clamped).abs() > 1.0 {
+                log::warn!(
+                    "Altitude {} m out of WMM bounds, clamped to {} m",
+                    alt,
+                    alt_clamped
+                );
+            }
+
             let magnetic_field = GeomagneticField::new(
-                Length::new::<meter>(alt as f32),
+                Length::new::<meter>(alt_clamped as f32),
                 Angle::new::<degree>(lat_deg as f32),
                 Angle::new::<degree>(lon_deg as f32),
                 Date::from_ordinal_date(self.year, self.day).unwrap(),
             )
             .unwrap_or_else(|e| {
                 panic!(
-                    "Failed to create GeomagneticField at lat={}, lon={}, alt={}: {:?}",
-                    lat_deg, lon_deg, alt, e
+                    "Failed to create GeomagneticField at lat={}, lon={}, alt={} (clamped: {}): {:?}",
+                    lat_deg, lon_deg, alt, alt_clamped, e
                 )
             });
             self.mag_obs - magnetic_field.f().value as f64
@@ -898,7 +917,7 @@ pub fn build_event_stream(
                     r1.horizontal_accuracy.max(1e-3)
                 };
                 let vert_std = if r1.vertical_accuracy.is_nan() {
-                    1000.0
+                    20.0
                 } else {
                     r1.vertical_accuracy.max(1e-3)
                 };