Merge pull request #228 from jbrodovsky/copilot/implement-geophysical-jacobians

Implement geophysical measurement Jacobians for EKF updates

Author: jbrodovsky

core/src/kalman.rs

@@ -885,7 +885,11 @@ impl NavigationFilter for ExtendedKalmanFilter {
         };
 
         // Compute measurement Jacobian for 9-state system
-        let h_9state = if measurement
+        // First check if the measurement provides its own Jacobian (for geophysical measurements)
+        let h_9state = if let Some(jacobian) = measurement.get_jacobian(&self.mean_state) {
+            // Measurement provides its own Jacobian (e.g., geophysical anomaly measurements)
+            jacobian
+        } else if measurement
             .as_any()
             .downcast_ref::<GPSPositionMeasurement>()
             .is_some()

core/src/linearize.rs

@@ -632,21 +632,26 @@ pub fn relative_altitude_jacobian(_state: &StrapdownState) -> DMatrix<f64> {
 
 /// Compute measurement Jacobian (H) for gravity anomaly measurement
 ///
-/// **Note**: This is a placeholder function for future use. Currently, the EKF computes
-/// measurement Jacobians internally during the update step. This function provides a
-/// template structure for when custom Jacobian computation is integrated into the EKF.
+/// **Note**: This is a placeholder function that returns zeros. Geophysical measurements
+/// (gravity anomaly and magnetic anomaly) now provide their own Jacobians via the
+/// `MeasurementModel::get_jacobian()` trait method. The EKF update function automatically
+/// detects and uses these measurement-provided Jacobians.
 ///
 /// Gravity anomaly measurements depend on latitude and longitude (position) to query the
-/// geophysical map. The partial derivatives ∂z/∂lat and ∂z/∂lon would be computed
-/// numerically by the measurement model based on map gradients.
+/// geophysical map. The partial derivatives ∂z/∂lat and ∂z/∂lon are computed numerically
+/// by the `GravityMeasurement` struct in the `strapdown-geonav` crate based on map gradients.
 ///
 /// # Arguments
 ///
-/// * `_state` - Current navigation state (reserved for future use)
+/// * `_state` - Current navigation state (unused, this is a placeholder)
 ///
 /// # Returns
 ///
-/// 1×9 measurement Jacobian matrix H template for gravity anomaly (currently zeros)
+/// 1×9 measurement Jacobian matrix H filled with zeros (placeholder)
+///
+/// # See Also
+///
+/// For actual gravity anomaly Jacobian computation, see `geonav::GravityMeasurement::get_jacobian_internal()`.
 ///
 /// # Example
 ///
@@ -660,34 +665,35 @@ pub fn relative_altitude_jacobian(_state: &StrapdownState) -> DMatrix<f64> {
 /// assert_eq!(h.ncols(), 9);
 /// ```
 pub fn gravity_anomaly_jacobian(_state: &StrapdownState) -> DMatrix<f64> {
-    // Gravity anomaly depends on position (lat, lon) through map lookup
-    // The partial derivatives ∂z/∂lat and ∂z/∂lon are computed numerically
-    // by the measurement model based on map gradients
-
-    // These will be filled in by the measurement model with numerical derivatives
-    // from the geophysical map interpolation
-    // h[(0, 0)] = ∂(anomaly)/∂(lat) - computed from map gradient
-    // h[(0, 1)] = ∂(anomaly)/∂(lon) - computed from map gradient
+    // NOTE: This is a placeholder. Real gravity anomaly Jacobians are provided
+    // by the GravityMeasurement struct in the geonav crate via the
+    // MeasurementModel::get_jacobian() trait method. The EKF will automatically
+    // use the measurement-provided Jacobian when available.
     DMatrix::<f64>::zeros(1, 9)
 }
 
 /// Compute measurement Jacobian (H) for magnetic anomaly measurement
 ///
-/// **Note**: This is a placeholder function for future use. Currently, the EKF computes
-/// measurement Jacobians internally during the update step. This function provides a
-/// template structure for when custom Jacobian computation is integrated into the EKF.
+/// **Note**: This is a placeholder function that returns zeros. Geophysical measurements
+/// (gravity anomaly and magnetic anomaly) now provide their own Jacobians via the
+/// `MeasurementModel::get_jacobian()` trait method. The EKF update function automatically
+/// detects and uses these measurement-provided Jacobians.
 ///
 /// Magnetic anomaly measurements depend on latitude and longitude (position) to query the
-/// geophysical map. The partial derivatives ∂z/∂lat and ∂z/∂lon would be computed
-/// numerically by the measurement model based on map gradients.
+/// geophysical map. The partial derivatives ∂z/∂lat and ∂z/∂lon are computed numerically
+/// by the `MagneticAnomalyMeasurement` struct in the `strapdown-geonav` crate based on map gradients.
 ///
 /// # Arguments
 ///
-/// * `_state` - Current navigation state (reserved for future use)
+/// * `_state` - Current navigation state (unused, this is a placeholder)
 ///
 /// # Returns
 ///
-/// 1×9 measurement Jacobian matrix H template for magnetic anomaly (currently zeros)
+/// 1×9 measurement Jacobian matrix H filled with zeros (placeholder)
+///
+/// # See Also
+///
+/// For actual magnetic anomaly Jacobian computation, see `geonav::MagneticAnomalyMeasurement::get_jacobian_internal()`.
 ///
 /// # Example
 ///
@@ -701,14 +707,10 @@ pub fn gravity_anomaly_jacobian(_state: &StrapdownState) -> DMatrix<f64> {
 /// assert_eq!(h.ncols(), 9);
 /// ```
 pub fn magnetic_anomaly_jacobian(_state: &StrapdownState) -> DMatrix<f64> {
-    // Magnetic anomaly depends on position (lat, lon) through map lookup
-    // The partial derivatives ∂z/∂lat and ∂z/∂lon are computed numerically
-    // by the measurement model based on map gradients
-
-    // These will be filled in by the measurement model with numerical derivatives
-    // from the geophysical map interpolation
-    // h[(0, 0)] = ∂(anomaly)/∂(lat) - computed from map gradient
-    // h[(0, 1)] = ∂(anomaly)/∂(lon) - computed from map gradient
+    // NOTE: This is a placeholder. Real magnetic anomaly Jacobians are provided
+    // by the MagneticAnomalyMeasurement struct in the geonav crate via the
+    // MeasurementModel::get_jacobian() trait method. The EKF will automatically
+    // use the measurement-provided Jacobian when available.
     DMatrix::<f64>::zeros(1, 9)
 }
 

core/src/measurements.rs

@@ -72,6 +72,42 @@ pub trait MeasurementModel: Any {
     ///
     /// Expected measurement given the state
     fn get_expected_measurement(&self, state: &DVector<f64>) -> DVector<f64>;
+
+    /// Optionally provide the measurement Jacobian for EKF updates.
+    ///
+    /// This method allows measurements to provide their own linearized measurement
+    /// model (Jacobian matrix H) for Extended Kalman Filter updates. If not provided
+    /// (returns None), the EKF will fall back to using a default Jacobian based on
+    /// the measurement type.
+    ///
+    /// For geophysical measurements (gravity anomaly, magnetic anomaly), this method
+    /// computes numerical gradients from the geophysical map to create the Jacobian.
+    ///
+    /// # Arguments
+    ///
+    /// * `state` - Current state estimate vector [lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw]
+    ///
+    /// # Returns
+    ///
+    /// Optional Jacobian matrix H (measurement_dim × 9) for EKF updates. Returns None
+    /// if the measurement does not provide its own Jacobian.
+    ///
+    /// # Example
+    ///
+    /// ```rust
+    /// use strapdown::measurements::MeasurementModel;
+    /// use nalgebra::{DVector, DMatrix};
+    ///
+    /// // For a measurement that provides its own Jacobian:
+    /// // let h_matrix = measurement.get_jacobian(&state).unwrap();
+    ///
+    /// // For a measurement that doesn't provide a Jacobian:
+    /// // let h_matrix_opt = measurement.get_jacobian(&state);
+    /// // assert!(h_matrix_opt.is_none());
+    /// ```
+    fn get_jacobian(&self, _state: &DVector<f64>) -> Option<DMatrix<f64>> {
+        None // Default implementation returns None
+    }
 }
 
 /// GPS position measurement model

geonav/src/lib.rs

@@ -579,6 +579,10 @@ impl MeasurementModel for GravityMeasurement {
             .unwrap_or(f64::NAN);
         DVector::from_vec(vec![map_value])
     }
+
+    fn get_jacobian(&self, state: &DVector<f64>) -> Option<DMatrix<f64>> {
+        Some(self.get_jacobian_internal(state))
+    }
 }
 
 impl GravityMeasurement {
@@ -594,7 +598,7 @@ impl GravityMeasurement {
     /// # Returns
     ///
     /// 1×9 Jacobian matrix H for gravity anomaly measurement
-    pub fn get_jacobian(&self, state: &DVector<f64>) -> DMatrix<f64> {
+    pub fn get_jacobian_internal(&self, state: &DVector<f64>) -> DMatrix<f64> {
         let mut h = DMatrix::<f64>::zeros(1, 9);
 
         let lat = state[0];
@@ -679,6 +683,10 @@ impl MeasurementModel for MagneticAnomalyMeasurement {
             .unwrap_or(f64::NAN);
         DVector::from_vec(vec![map_value])
     }
+
+    fn get_jacobian(&self, state: &DVector<f64>) -> Option<DMatrix<f64>> {
+        Some(self.get_jacobian_internal(state))
+    }
 }
 
 impl MagneticAnomalyMeasurement {
@@ -694,7 +702,7 @@ impl MagneticAnomalyMeasurement {
     /// # Returns
     ///
     /// 1×9 Jacobian matrix H for magnetic anomaly measurement
-    pub fn get_jacobian(&self, state: &DVector<f64>) -> DMatrix<f64> {
+    pub fn get_jacobian_internal(&self, state: &DVector<f64>) -> DMatrix<f64> {
         let mut h = DMatrix::<f64>::zeros(1, 9);
 
         let lat = state[0];
@@ -1697,7 +1705,7 @@ mod tests {
             0.0, // attitude
         ]);
 
-        let jacobian = measurement.get_jacobian(&state);
+        let jacobian = measurement.get_jacobian_internal(&state);
 
         // Jacobian should be 1x9
         assert_eq!(jacobian.nrows(), 1);
@@ -1737,7 +1745,7 @@ mod tests {
             0.0, // attitude
         ]);
 
-        let jacobian = measurement.get_jacobian(&state);
+        let jacobian = measurement.get_jacobian_internal(&state);
 
         // Jacobian should be 1x9
         assert_eq!(jacobian.nrows(), 1);
@@ -1749,4 +1757,83 @@ mod tests {
             assert_approx_eq!(jacobian[(0, j)], 0.0, 1e-10);
         }
     }
+
+    #[test]
+    fn test_gravity_measurement_trait_jacobian() {
+        // Test that the MeasurementModel trait method works correctly
+        let map = Rc::new(create_test_gravity_map());
+        let measurement: Box<dyn strapdown::measurements::MeasurementModel> =
+            Box::new(GravityMeasurement {
+                map: map.clone(),
+                noise_std: 100.0,
+                gravity_observed: 9.8,
+                latitude: 41.0_f64.to_radians(),
+                altitude: 100.0,
+                north_velocity: 0.0,
+                east_velocity: 0.0,
+            });
+
+        let state = DVector::from_vec(vec![
+            41.0_f64.to_radians(),
+            -73.0_f64.to_radians(),
+            100.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]);
+
+        // Test that get_jacobian returns Some
+        let jacobian_opt = measurement.get_jacobian(&state);
+        assert!(
+            jacobian_opt.is_some(),
+            "Gravity measurement should provide Jacobian via trait"
+        );
+
+        let jacobian = jacobian_opt.unwrap();
+        assert_eq!(jacobian.nrows(), 1);
+        assert_eq!(jacobian.ncols(), 9);
+    }
+
+    #[test]
+    fn test_magnetic_measurement_trait_jacobian() {
+        // Test that the MeasurementModel trait method works correctly
+        let map = Rc::new(create_test_magnetic_map());
+        let measurement: Box<dyn strapdown::measurements::MeasurementModel> =
+            Box::new(MagneticAnomalyMeasurement {
+                map: map.clone(),
+                noise_std: 100.0,
+                mag_obs: 48000.0,
+                latitude: 41.0,
+                longitude: -73.0,
+                altitude: 100.0,
+                year: 2023,
+                day: 216,
+            });
+
+        let state = DVector::from_vec(vec![
+            41.0_f64.to_radians(),
+            -73.0_f64.to_radians(),
+            100.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+        ]);
+
+        // Test that get_jacobian returns Some
+        let jacobian_opt = measurement.get_jacobian(&state);
+        assert!(
+            jacobian_opt.is_some(),
+            "Magnetic measurement should provide Jacobian via trait"
+        );
+
+        let jacobian = jacobian_opt.unwrap();
+        assert_eq!(jacobian.nrows(), 1);
+        assert_eq!(jacobian.ncols(), 9);
+    }
 }