sidereon_core/astro/
doppler.rs

1//! Doppler and range-rate computations for satellite-ground links.
2//!
3//! Positive range rate means the satellite is receding from the station.
4//! Positive Doppler ratio means the transmitter is approaching the station.
5//! The satellite ECEF velocity uses `v_ecef = R*v_gcrs - omega x r`, the
6//! standard rotating-frame transport. This is consistent with core's
7//! oracle-validated GNSS range-rate model
8//! `precise_positioning::velocity::predict_range_rate_m_s`. It deliberately
9//! differs from the legacy orbis `shift/4`, which had the transport sign
10//! inverted and was never oracle-gated.
11
12use crate::astro::constants::{models::pz90::OMEGA_E_RAD_S, units::M_PER_KM};
13use crate::astro::frames::transforms::{
14    gcrs_to_itrs_matrix, geodetic_to_itrs, mat3_vec3_mul, FrameTransformError,
15};
16use crate::astro::time::scales::TimeScales;
17use crate::constants::C_M_S;
18
19/// Speed of light in km/s.
20const C_KM_S: f64 = C_M_S / M_PER_KM;
21
22/// Earth rotation rate in rad/s (PZ-90 `OMEGA_E_RAD_S`) used in the standard ECEF transport term.
23const OMEGA_EARTH: f64 = OMEGA_E_RAD_S;
24
25/// Range-rate and Doppler shift result for a carrier frequency.
26#[derive(Debug, Clone, Copy, PartialEq)]
27pub struct DopplerShift {
28    /// Range rate in km/s; positive means receding from the station.
29    pub range_rate_km_s: f64,
30    /// Doppler shift in Hz; positive means a frequency increase.
31    pub doppler_hz: f64,
32    /// Dimensionless Doppler ratio; positive means approaching the station.
33    pub doppler_ratio: f64,
34}
35
36/// Error while computing Doppler shift.
37#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
38pub enum DopplerError {
39    /// A frame transformation failed.
40    #[error("doppler frame transform failed: {0}")]
41    FrameTransform(#[from] FrameTransformError),
42}
43
44/// Compute range rate and dimensionless Doppler ratio from a GCRS state.
45///
46/// Position is in km, velocity is in km/s, station latitude/longitude are in
47/// degrees, and station altitude is in km. Positive range rate means receding;
48/// positive Doppler ratio means approaching.
49pub fn range_rate_and_ratio(
50    gcrs_position_km: [f64; 3],
51    gcrs_velocity_km_s: [f64; 3],
52    station_lat_deg: f64,
53    station_lon_deg: f64,
54    station_alt_km: f64,
55    ts: &TimeScales,
56) -> Result<(f64, f64), DopplerError> {
57    let [sat_x, sat_y, sat_z] = gcrs_position_km;
58    let [sat_vx, sat_vy, sat_vz] = gcrs_velocity_km_s;
59
60    let r_mat = gcrs_to_itrs_matrix(ts)?;
61
62    let pos_gcrs = [sat_x, sat_y, sat_z];
63    let pos_itrs = mat3_vec3_mul(&r_mat, &pos_gcrs)?;
64
65    let vel_gcrs = [sat_vx, sat_vy, sat_vz];
66    let vel_itrs_rot = mat3_vec3_mul(&r_mat, &vel_gcrs)?;
67
68    // Standard rotating-frame transport: v_ecef = R*v_gcrs - omega x r_ecef.
69    // Since omega x r = [-OMEGA*y, OMEGA*x, 0], subtracting it contributes [+OMEGA*y, -OMEGA*x, 0].
70    let transport_x = OMEGA_EARTH * pos_itrs[1];
71    let transport_y = -OMEGA_EARTH * pos_itrs[0];
72    let transport_z = 0.0;
73
74    let vel_itrs = [
75        vel_itrs_rot[0] + transport_x,
76        vel_itrs_rot[1] + transport_y,
77        vel_itrs_rot[2] + transport_z,
78    ];
79
80    let (stn_x, stn_y, stn_z) = geodetic_to_itrs(station_lat_deg, station_lon_deg, station_alt_km)?;
81
82    let range_vec = [
83        pos_itrs[0] - stn_x,
84        pos_itrs[1] - stn_y,
85        pos_itrs[2] - stn_z,
86    ];
87
88    let range_mag =
89        (range_vec[0] * range_vec[0] + range_vec[1] * range_vec[1] + range_vec[2] * range_vec[2])
90            .sqrt();
91
92    let range_unit = [
93        range_vec[0] / range_mag,
94        range_vec[1] / range_mag,
95        range_vec[2] / range_mag,
96    ];
97
98    let range_rate =
99        range_unit[0] * vel_itrs[0] + range_unit[1] * vel_itrs[1] + range_unit[2] * vel_itrs[2];
100
101    let doppler_ratio = -range_rate / C_KM_S;
102
103    Ok((range_rate, doppler_ratio))
104}
105
106/// Compute range rate, Doppler ratio, and carrier Doppler shift.
107///
108/// `frequency_hz` is multiplied by the Doppler ratio exactly as the Orbis
109/// wrapper did.
110pub fn doppler_shift(
111    gcrs_position_km: [f64; 3],
112    gcrs_velocity_km_s: [f64; 3],
113    station_lat_deg: f64,
114    station_lon_deg: f64,
115    station_alt_km: f64,
116    ts: &TimeScales,
117    frequency_hz: f64,
118) -> Result<DopplerShift, DopplerError> {
119    let (range_rate_km_s, doppler_ratio) = range_rate_and_ratio(
120        gcrs_position_km,
121        gcrs_velocity_km_s,
122        station_lat_deg,
123        station_lon_deg,
124        station_alt_km,
125        ts,
126    )?;
127
128    Ok(DopplerShift {
129        range_rate_km_s,
130        doppler_hz: doppler_ratio * frequency_hz,
131        doppler_ratio,
132    })
133}
134
135#[cfg(test)]
136mod tests {
137    #![allow(clippy::excessive_precision)]
138
139    use super::*;
140    use crate::astro::frames::transforms::{gcrs_to_itrs_matrix, geodetic_to_itrs, mat3_vec3_mul};
141
142    const GCRS_POSITION_KM: [f64; 3] = [
143        3700.211211203995390,
144        2015.912218120605530,
145        5309.513078070447591,
146    ];
147    const GCRS_VELOCITY_KM_S: [f64; 3] =
148        [-3.398428894395407, 6.869656830559572, -0.239850181126689];
149    const STATION_LAT_DEG: f64 = 40.0;
150    const STATION_LON_DEG: f64 = -74.0;
151    const STATION_ALT_KM: f64 = 0.0;
152    const FREQUENCY_HZ: f64 = 437.0e6;
153
154    fn fixed_time_scales() -> TimeScales {
155        TimeScales::from_utc(2018, 7, 4, 0, 0, 0.0).expect("valid UTC instant")
156    }
157
158    #[test]
159    fn numerical_derivative_oracle() {
160        let stn =
161            geodetic_to_itrs(STATION_LAT_DEG, STATION_LON_DEG, STATION_ALT_KM).expect("station");
162
163        let range_at = |t_off: f64| {
164            let ts =
165                TimeScales::from_utc(2018, 7, 4, 0, 0, 30.0 + t_off).expect("valid UTC instant");
166            let r = gcrs_to_itrs_matrix(&ts).expect("valid frame transform");
167            let pos_gcrs = [
168                GCRS_POSITION_KM[0] + GCRS_VELOCITY_KM_S[0] * t_off,
169                GCRS_POSITION_KM[1] + GCRS_VELOCITY_KM_S[1] * t_off,
170                GCRS_POSITION_KM[2] + GCRS_VELOCITY_KM_S[2] * t_off,
171            ];
172            let p = mat3_vec3_mul(&r, &pos_gcrs).expect("matrix-vector multiply");
173            let d = [p[0] - stn.0, p[1] - stn.1, p[2] - stn.2];
174            (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt()
175        };
176
177        let h = 0.01;
178        let numerical = (range_at(h) - range_at(-h)) / (2.0 * h);
179
180        let ts = TimeScales::from_utc(2018, 7, 4, 0, 0, 30.0).expect("valid UTC instant");
181        let (analytic_range_rate, _) = range_rate_and_ratio(
182            GCRS_POSITION_KM,
183            GCRS_VELOCITY_KM_S,
184            STATION_LAT_DEG,
185            STATION_LON_DEG,
186            STATION_ALT_KM,
187            &ts,
188        )
189        .expect("valid Doppler computation");
190
191        // The legacy orbis +omega x r sign would fail this by roughly 4.5e-3 km/s.
192        assert!((analytic_range_rate - numerical).abs() < 1.0e-6);
193    }
194
195    #[test]
196    fn consistent_with_oracle_gated_gnss_range_rate() {
197        use crate::precise_positioning::velocity::{
198            predict_range_rate_m_s, ReceiverVelocityState, VelocityObservation,
199        };
200        use crate::{GnssSatelliteId, GnssSystem};
201
202        let ts = TimeScales::from_utc(2018, 7, 4, 0, 0, 30.0).expect("valid UTC instant");
203        let r = gcrs_to_itrs_matrix(&ts).expect("valid frame transform");
204        let pos_ecef = mat3_vec3_mul(&r, &GCRS_POSITION_KM).expect("matrix-vector multiply");
205        let vel_rot = mat3_vec3_mul(&r, &GCRS_VELOCITY_KM_S).expect("matrix-vector multiply");
206        let vel_ecef = [
207            vel_rot[0] + OMEGA_EARTH * pos_ecef[1],
208            vel_rot[1] - OMEGA_EARTH * pos_ecef[0],
209            vel_rot[2],
210        ];
211        let stn =
212            geodetic_to_itrs(STATION_LAT_DEG, STATION_LON_DEG, STATION_ALT_KM).expect("station");
213
214        let obs = VelocityObservation {
215            sat: GnssSatelliteId::new(GnssSystem::Gps, 1).expect("valid satellite id"),
216            satellite_position_m: pos_ecef,
217            satellite_velocity_m_s: vel_ecef,
218            measured_range_rate_m_s: 0.0,
219            sigma_m_s: 1.0,
220            satellite_clock_drift_m_s: 0.0,
221        };
222        let receiver = ReceiverVelocityState {
223            position_m: [stn.0, stn.1, stn.2],
224            velocity_m_s: [0.0; 3],
225            clock_drift_m_s: 0.0,
226        };
227        let pred = predict_range_rate_m_s(&obs, receiver).expect("nonzero line of sight");
228
229        let (rr, _) = range_rate_and_ratio(
230            GCRS_POSITION_KM,
231            GCRS_VELOCITY_KM_S,
232            STATION_LAT_DEG,
233            STATION_LON_DEG,
234            STATION_ALT_KM,
235            &ts,
236        )
237        .expect("valid Doppler computation");
238
239        assert!((rr - pred.range_rate_m_s).abs() < 1.0e-12);
240    }
241
242    #[test]
243    fn frozen_bits_regression() {
244        let ts = fixed_time_scales();
245
246        let (range_rate_km_s, doppler_ratio) = range_rate_and_ratio(
247            GCRS_POSITION_KM,
248            GCRS_VELOCITY_KM_S,
249            STATION_LAT_DEG,
250            STATION_LON_DEG,
251            STATION_ALT_KM,
252            &ts,
253        )
254        .expect("valid Doppler computation");
255        let shift = doppler_shift(
256            GCRS_POSITION_KM,
257            GCRS_VELOCITY_KM_S,
258            STATION_LAT_DEG,
259            STATION_LON_DEG,
260            STATION_ALT_KM,
261            &ts,
262            FREQUENCY_HZ,
263        )
264        .expect("valid Doppler shift");
265
266        // Pinned to the corrected (-omega x r) physics validated by numerical_derivative_oracle.
267        let expected_range_rate_km_s: f64 = 2.11937962917790934e-1;
268        let expected_doppler_ratio: f64 = -7.06948948388124429e-7;
269        let expected_doppler_hz: f64 = -3.08936690445610395e2;
270
271        assert_eq!(
272            range_rate_km_s.to_bits(),
273            expected_range_rate_km_s.to_bits()
274        );
275        assert_eq!(doppler_ratio.to_bits(), expected_doppler_ratio.to_bits());
276        assert_eq!(shift.range_rate_km_s.to_bits(), range_rate_km_s.to_bits());
277        assert_eq!(shift.doppler_ratio.to_bits(), doppler_ratio.to_bits());
278        assert_eq!(shift.doppler_hz.to_bits(), expected_doppler_hz.to_bits());
279    }
280
281    #[test]
282    fn sign_and_physics_sanity() {
283        let ts = fixed_time_scales();
284
285        let (range_rate_km_s, doppler_ratio) = range_rate_and_ratio(
286            GCRS_POSITION_KM,
287            GCRS_VELOCITY_KM_S,
288            STATION_LAT_DEG,
289            STATION_LON_DEG,
290            STATION_ALT_KM,
291            &ts,
292        )
293        .expect("valid Doppler computation");
294        let shift = doppler_shift(
295            GCRS_POSITION_KM,
296            GCRS_VELOCITY_KM_S,
297            STATION_LAT_DEG,
298            STATION_LON_DEG,
299            STATION_ALT_KM,
300            &ts,
301            FREQUENCY_HZ,
302        )
303        .expect("valid Doppler shift");
304
305        assert!(range_rate_km_s.abs() <= 8.0);
306        assert!(shift.doppler_hz.abs() <= 12_000.0);
307        assert_eq!(
308            doppler_ratio.to_bits(),
309            (-range_rate_km_s / C_KM_S).to_bits()
310        );
311        assert_eq!(
312            shift.doppler_hz.to_bits(),
313            (doppler_ratio * FREQUENCY_HZ).to_bits()
314        );
315    }
316}
sidereon_core/astro/doppler.rs

sidereon_core/astro/
doppler.rs
