Fixed range rate computation & add GMAT validation

ChristopherRabotin · ChristopherRabotin · commit 3aefe89f8e82 · 2025-02-06T23:26:32.000-07:00
diff --git a/anise/src/almanac/aer.rs b/anise/src/almanac/aer.rs
@@ -16,7 +16,6 @@ use crate::{
     frames::Frame,
     math::angles::{between_0_360, between_pm_180},
     prelude::Orbit,
-    time::uuid_from_epoch,
 };
 
 use super::Almanac;
@@ -81,10 +80,9 @@ impl Almanac {
         }
 
         // Compute the SEZ DCM
-        let from = uuid_from_epoch(tx.frame.orientation_id, rx.epoch);
         // SEZ DCM is topo to fixed
         let sez_dcm = tx
-            .dcm_from_topocentric_to_body_fixed(from)
+            .dcm_from_topocentric_to_body_fixed(-1)
             .context(EphemerisPhysicsSnafu { action: "" })
             .context(EphemerisSnafu {
                 action: "computing SEZ DCM for AER",
@@ -96,7 +94,7 @@ impl Almanac {
                 action: "transforming transmitter to SEZ",
             })?;
 
-        // Convert the receiver into the transmitter frame.
+        // Convert the receiver into the body fixed transmitter frame.
         let rx_in_tx_frame = self.transform_to(rx, tx.frame, ab_corr)?;
         // Convert into SEZ frame
         let rx_sez = (sez_dcm.transpose() * rx_in_tx_frame)
@@ -105,12 +103,16 @@ impl Almanac {
                 action: "transforming received to SEZ",
             })?;
 
-        // Compute the range ρ.
+        // Convert transmitter and receiver into the transmitter frame
+        let rx_in_tx_frame = self.transform_to(rx, tx.frame, ab_corr)?;
+
+        // Compute the range ρ in the SEZ frame for az/el
         let rho_sez = rx_sez.radius_km - tx_sez.radius_km;
+        // And in the inertial frame for range and range-rate
+        let rho_tx_frame = rx_in_tx_frame.radius_km - tx.radius_km;
 
         // Compute the range-rate \dot ρ
-        let range_rate_km_s =
-            rho_sez.dot(&(rx_sez.velocity_km_s - tx_sez.velocity_km_s)) / rho_sez.norm();
+        let range_rate_km_s = rho_tx_frame.dot(&rx_in_tx_frame.velocity_km_s) / rho_tx_frame.norm();
 
         // Finally, compute the elevation (math is the same as declination)
         // Source: Vallado, section 4.4.3
@@ -127,7 +129,7 @@ impl Almanac {
             epoch: tx.epoch,
             azimuth_deg,
             elevation_deg,
-            range_km: rho_sez.norm(),
+            range_km: rho_tx_frame.norm(),
             range_rate_km_s,
             obstructed_by,
             light_time: (rho_sez.norm() / SPEED_OF_LIGHT_KM_S).seconds(),
diff --git a/anise/src/astro/orbit.rs b/anise/src/astro/orbit.rs
@@ -320,20 +320,52 @@ impl Orbit {
 #[allow(clippy::too_many_arguments)]
 #[cfg_attr(feature = "python", pymethods)]
 impl Orbit {
+    /// Builds the rotation matrix that rotates from the topocentric frame (SEZ) into the body fixed frame of this state.
+    ///
+    /// # Frame warnings
+    /// + If the state is NOT in a body fixed frame (i.e. ITRF93), then this computation is INVALID.
+    /// + (Usually) no time derivative can be computed: the orbit is expected to be a body fixed frame where the `at_epoch` function will fail. Exceptions for Moon body fixed frames.
+    ///
+    /// # UNUSED Arguments
+    /// + `from`: ID of this new frame. Only used to set the "from" frame of the DCM. -- No longer used since 0.5.3
+    ///
+    /// # Source
+    /// From the GMAT MathSpec, page 30 section 2.6.9 and from `Calculate_RFT` in `TopocentricAxes.cpp`, this returns the
+    /// rotation matrix from the topocentric frame (SEZ) to body fixed frame.
+    /// In the GMAT MathSpec notation, R_{IF} is the DCM from body fixed to inertial. Similarly, R{FT} is from topocentric
+    /// to body fixed.
+    pub fn dcm_from_topocentric_to_body_fixed(&self, _from: NaifId) -> PhysicsResult<DCM> {
+        let rot_mat_dt = if let Ok(pre) = self.at_epoch(self.epoch - Unit::Second * 1) {
+            if let Ok(post) = self.at_epoch(self.epoch + Unit::Second * 1) {
+                let dcm_pre = pre.dcm3x3_from_topocentric_to_body_fixed()?;
+                let dcm_post = post.dcm3x3_from_topocentric_to_body_fixed()?;
+                Some(0.5 * dcm_post.rot_mat - 0.5 * dcm_pre.rot_mat)
+            } else {
+                None
+            }
+        } else {
+            None
+        };
+
+        Ok(DCM {
+            rot_mat: self.dcm3x3_from_topocentric_to_body_fixed()?.rot_mat,
+            rot_mat_dt,
+            from: uuid_from_epoch(self.frame.orientation_id, self.epoch),
+            to: self.frame.orientation_id,
+        })
+    }
+
     /// Builds the rotation matrix that rotates from the topocentric frame (SEZ) into the body fixed frame of this state.
     ///
     /// # Frame warning
     /// If the state is NOT in a body fixed frame (i.e. ITRF93), then this computation is INVALID.
     ///
-    /// # Arguments
-    /// + `from`: ID of this new frame, must be unique if it'll be added to the Almanac. Only used to set the "from" frame of the DCM.
-    ///
     /// # Source
     /// From the GMAT MathSpec, page 30 section 2.6.9 and from `Calculate_RFT` in `TopocentricAxes.cpp`, this returns the
     /// rotation matrix from the topocentric frame (SEZ) to body fixed frame.
     /// In the GMAT MathSpec notation, R_{IF} is the DCM from body fixed to inertial. Similarly, R{FT} is from topocentric
     /// to body fixed.
-    pub fn dcm_from_topocentric_to_body_fixed(&self, from: NaifId) -> PhysicsResult<DCM> {
+    pub fn dcm3x3_from_topocentric_to_body_fixed(&self) -> PhysicsResult<DCM> {
         if (self.radius_km.x.powi(2) + self.radius_km.y.powi(2)).sqrt() < 1e-3 {
             warn!("SEZ frame ill-defined when close to the poles");
         }
@@ -357,7 +389,7 @@ impl Orbit {
         Ok(DCM {
             rot_mat,
             rot_mat_dt: None,
-            from,
+            from: uuid_from_epoch(self.frame.orientation_id, self.epoch),
             to: self.frame.orientation_id,
         })
     }
diff --git a/anise/src/frames/frame.rs b/anise/src/frames/frame.rs
@@ -91,6 +91,15 @@ impl Frame {
         self.shape = Some(shape);
         self
     }
+
+    /// Returns a copy of this frame with the graviational parameter and the shape information from this frame.
+    /// Use this to prevent astrodynamical computations.
+    ///
+    /// :rtype: None
+    pub fn stripped(mut self) -> Self {
+        self.strip();
+        self
+    }
 }
 
 #[cfg(feature = "python")]
diff --git a/anise/tests/astro/aer.rs b/anise/tests/astro/aer.rs
@@ -0,0 +1,214 @@
+use anise::{
+    constants::{
+        frames::{EME2000, IAU_EARTH_FRAME},
+        usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S,
+    },
+    prelude::{Almanac, Orbit},
+};
+use core::str::FromStr;
+use hifitime::Epoch;
+
+// Define location of DSN DSS-65 in Madrid, Spain
+const DSS65_LATITUDE_DEG: f64 = 40.427_222;
+const DSS65_LONGITUDE_DEG: f64 = 4.250_556;
+const DSS65_HEIGHT_KM: f64 = 0.834_939;
+
+/// Validation test for azimuth, elevation, range, and range-rate computation.
+/// Importantly, we only test for range-rate here; I forget the set up used for DSS65 in GMAT, and could not export the az/el data.
+#[test]
+fn validate_aer_vs_gmat_cislunar1() {
+    let almanac = Almanac::default()
+        .load("../data/earth_latest_high_prec.bpc")
+        .unwrap()
+        .load("../data/pck11.pca")
+        .unwrap()
+        .load("../data/de430.bsp")
+        .unwrap();
+
+    let eme2k = almanac.frame_from_uid(EME2000).unwrap();
+
+    let states = &[
+        Orbit::new(
+            58643.769540,
+            -61696.435624,
+            -36178.745722,
+            2.148654,
+            -1.202489,
+            -0.714016,
+            Epoch::from_str("2023-11-16T13:35:30.231999909 UTC").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            66932.786920,
+            -66232.188134,
+            -38873.611383,
+            2.040555,
+            -1.092316,
+            -0.649376,
+            Epoch::from_str("2023-11-16T14:41:30.231999930 UTC").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            74004.678872,
+            -69951.400821,
+            -41085.748329,
+            1.956606,
+            -1.011239,
+            -0.601766,
+            Epoch::from_str("2023-11-16T15:40:30.231999839 UTC").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            80796.572756,
+            -73405.951304,
+            -43142.418173,
+            1.882015,
+            -0.942232,
+            -0.561216,
+            Epoch::from_str("2023-11-16T16:39:30.232000062 UTC").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            91643.444941,
+            -78707.219860,
+            -46302.227968,
+            1.773135,
+            -0.846264,
+            -0.504775,
+            Epoch::from_str("2023-11-16T18:18:30.231999937 UTC").unwrap(),
+            eme2k,
+        ),
+    ];
+
+    let observations = &[
+        (91457.558, 2.199),
+        (99965.056, 2.105),
+        (107322.912, 2.056),
+        (114551.675, 2.031),
+        (126573.919, 2.021),
+    ];
+
+    for (rx, (range_km, range_rate_km_s)) in
+        states.iter().copied().zip(observations.iter().copied())
+    {
+        // Rebuild the ground stations
+        let tx = Orbit::try_latlongalt(
+            DSS65_LATITUDE_DEG,
+            DSS65_LONGITUDE_DEG,
+            DSS65_HEIGHT_KM,
+            MEAN_EARTH_ANGULAR_VELOCITY_DEG_S,
+            rx.epoch,
+            almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(),
+        )
+        .unwrap();
+
+        let aer = almanac
+            .azimuth_elevation_range_sez(rx, tx, None, None)
+            .unwrap();
+
+        dbg!(aer.range_km - range_km);
+        assert!(
+            (aer.range_rate_km_s - range_rate_km_s).abs() < 1e-3,
+            "more than 1 m/s error!"
+        );
+    }
+}
+
+#[test]
+fn validate_aer_vs_gmat_cislunar2() {
+    let almanac = Almanac::default()
+        .load("../data/earth_latest_high_prec.bpc")
+        .unwrap()
+        .load("../data/pck11.pca")
+        .unwrap()
+        .load("../data/de430.bsp")
+        .unwrap();
+
+    let eme2k = almanac.frame_from_uid(EME2000).unwrap();
+
+    let states = &[
+        Orbit::new(
+            102114.297454,
+            13933.746232,
+            -671.117990,
+            2.193540,
+            0.906982,
+            0.333105,
+            Epoch::from_str("2022-11-29T14:39:28.000000216 TAI").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            110278.148176,
+            17379.224108,
+            608.602854,
+            2.062036,
+            0.887598,
+            0.333149,
+            Epoch::from_str("2022-11-29T15:43:28.000000160 TAI").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            117388.586896,
+            20490.340765,
+            1786.240391,
+            1.957486,
+            0.870185,
+            0.332038,
+            Epoch::from_str("2022-11-29T16:42:28.000000384 TAI").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            124151.820782,
+            23540.835319,
+            2958.593254,
+            1.865399,
+            0.853363,
+            0.330212,
+            Epoch::from_str("2022-11-29T17:41:28.000000293 TAI").unwrap(),
+            eme2k,
+        ),
+        Orbit::new(
+            131247.969145,
+            26834.012939,
+            4241.579371,
+            1.775455,
+            0.835578,
+            0.327653,
+            Epoch::from_str("2022-11-29T18:46:28.000000433 TAI").unwrap(),
+            eme2k,
+        ),
+    ];
+
+    let observations = &[
+        (102060.177, 1.957),
+        (109389.490, 1.868),
+        (115907.202, 1.820),
+        (122305.708, 1.799),
+        (129320.821, 1.802),
+    ];
+
+    for (rx, (range_km, range_rate_km_s)) in
+        states.iter().copied().zip(observations.iter().copied())
+    {
+        // Rebuild the ground stations
+        let tx = Orbit::try_latlongalt(
+            DSS65_LATITUDE_DEG,
+            DSS65_LONGITUDE_DEG,
+            DSS65_HEIGHT_KM,
+            MEAN_EARTH_ANGULAR_VELOCITY_DEG_S,
+            rx.epoch,
+            almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(),
+        )
+        .unwrap();
+
+        let aer = almanac
+            .azimuth_elevation_range_sez(rx, tx, None, None)
+            .unwrap();
+
+        dbg!(aer.range_km - range_km);
+        assert!(
+            (aer.range_rate_km_s - range_rate_km_s).abs() < 1e-3,
+            "more than 1 m/s error!"
+        );
+    }
+}
diff --git a/anise/tests/astro/mod.rs b/anise/tests/astro/mod.rs
@@ -1 +1,2 @@
+mod aer;
 mod orbit;
diff --git a/anise/tests/orientations/validation.rs b/anise/tests/orientations/validation.rs
@@ -666,7 +666,7 @@ fn validate_bpc_to_iau_rotations() {
                 dcm.rot_mat - rot_mat
             );
 
-            // Check the derivative with a slightly tighet constraint
+            // Check the derivative with a slightly tighter constraint
             assert!(
                 (dcm.rot_mat_dt.unwrap() - spice_dcm.rot_mat_dt.unwrap()).norm()
                     < DCM_EPSILON * 0.1,
