nyx_space/od/groundpnt/
mod.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18use crate::md::trajectory::INTERPOLATION_SAMPLES;
19use crate::md::StateParameter;
20use crate::od::DynamicsError;
21use crate::{cosmic::State, md::prelude::Interpolatable};
22use anise::analysis::prelude::OrbitalElement;
23use anise::errors::PhysicsError;
24use anise::math::interpolation::{hermite_eval, InterpolationError};
25use anise::prelude::Orbit;
26use anise::{astro::Location, prelude::Frame};
27use core::error::Error;
28use core::fmt;
29use core::ops::Add;
30use hifitime::Epoch;
31use hyperdual::{hyperspace_from_vector, OHyperdual};
32use nalgebra::{Const, DimName, Matrix6, OMatrix, OVector, Vector3, Vector6};
33use num_traits::Float;
34pub mod ground_dynamics;
35pub mod sensitivity;
36pub mod solution;
37pub mod trk_device;
38
39/// Represents a ground position/nav/timing receiver, e.g. a customer
40/// Note that we rebuild the Location structure from ANISE but _without_ a terrain mask because the mask is not copyable
41/// and this PNTRx must be copyable to implement State.
42#[derive(Copy, Clone, Debug, PartialEq)]
43pub struct GroundAsset {
44    pub latitude_deg: f64,
45    pub longitude_deg: f64,
46    pub height_km: f64,
47    // Velocity in the SEZ frame, South component, in METERS per second
48    pub latitude_rate_deg_s: f64,
49    // Velocity in the SEZ frame, East component, in METERS per second
50    pub longitude_rate_deg_s: f64,
51    // Velocity in the SEZ frame, Up/+Z component, in METERS per second
52    pub height_rate_km_s: f64,
53    // Epoch
54    pub epoch: Epoch,
55    /// Frame on which this location rests
56    pub frame: Frame,
57    pub stm: Option<OMatrix<f64, Const<6>, Const<6>>>,
58}
59
60impl GroundAsset {
61    pub fn from_fixed(
62        latitude_deg: f64,
63        longitude_deg: f64,
64        height_km: f64,
65        epoch: Epoch,
66        frame: Frame,
67    ) -> Self {
68        Self {
69            latitude_deg,
70            longitude_deg,
71            height_km,
72            epoch,
73            frame,
74            ..Default::default()
75        }
76    }
77
78    pub fn with_velocity_sez_m_s(
79        mut self,
80        vel_s_m_s: f64,
81        vel_e_m_s: f64,
82        vel_z_m_s: f64,
83    ) -> Result<Self, Box<dyn Error>> {
84        let (lat_rate_deg_s, long_rate_deg_s, alt_rate_km_s) = latlongalt_rate(
85            self.orbit(),
86            Vector3::new(vel_s_m_s, vel_e_m_s, vel_z_m_s) * 1e-3,
87        )?;
88
89        self.latitude_rate_deg_s = lat_rate_deg_s;
90        self.longitude_rate_deg_s = long_rate_deg_s;
91        self.height_rate_km_s = alt_rate_km_s;
92
93        Ok(self)
94    }
95
96    pub fn to_location(&self) -> Location {
97        Location {
98            latitude_deg: self.latitude_deg,
99            longitude_deg: self.longitude_deg,
100            height_km: self.height_km,
101            frame: self.frame.into(),
102            ..Default::default()
103        }
104    }
105
106    /// Compute the velocity in m/s in the SEZ frame from the state data stored in latitude deg/s, longitude deg/s, and height in km/s for integration of EOMs
107    pub fn velocity_sez_m_s(&self) -> Result<OVector<f64, Const<3>>, PhysicsError> {
108        // First, convert from SEZ to body frame.
109        let rx = Orbit::try_latlongalt(
110            self.latitude_deg,
111            self.longitude_deg,
112            self.height_km,
113            self.epoch,
114            self.frame,
115        )?;
116
117        velocity_sez_from_latlongalt_rate(
118            rx,
119            self.latitude_rate_deg_s,
120            self.longitude_rate_deg_s,
121            self.height_rate_km_s,
122        )
123        .map(|v| v * 1e3)
124    }
125
126    pub fn geodetic_to_cartesian_jacobian(&self) -> Result<Matrix6<f64>, PhysicsError> {
127        let lat_deg = self.latitude_deg;
128        let lon_deg = self.longitude_deg;
129        let alt_km = self.height_km;
130        let a = self.frame.mean_equatorial_radius_km()?;
131        // If there is no shape data, the previous line returns an error, so we
132        // can safely unwrap here.
133        let b = self.frame.shape.unwrap().polar_radius_km;
134        let e2 = (a * a - b * b) / (a * a);
135
136        // 1. Pack the full geodetic state vector
137        let state_vec = Vector6::new(
138            lat_deg,
139            lon_deg,
140            alt_km,
141            self.latitude_rate_deg_s,
142            self.longitude_rate_deg_s,
143            self.height_rate_km_s,
144        );
145
146        // 2. Initialize hyperspace (1 real + 6 dual components)
147        let hyper_state: Vector6<OHyperdual<f64, Const<7>>> = hyperspace_from_vector(&state_vec);
148
149        // 3. Extract and scale angular variables
150        // Multiplying the hyperdual by π/180 scales the duals perfectly,
151        // yielding partials in per-degree automatically!
152        let deg_to_rad = std::f64::consts::PI / 180.0;
153        let lat = hyper_state[0] * deg_to_rad;
154        let lon = hyper_state[1] * deg_to_rad;
155        let alt = hyper_state[2];
156        let lat_rate = hyper_state[3] * deg_to_rad;
157        let lon_rate = hyper_state[4] * deg_to_rad;
158        let alt_rate = hyper_state[5];
159
160        let sin_lat = lat.sin();
161        let cos_lat = lat.cos();
162        let sin_lon = lon.sin();
163        let cos_lon = lon.cos();
164
165        let one = OHyperdual::<f64, Const<7>>::from_real(1.0);
166        let e2_dual = OHyperdual::<f64, Const<7>>::from_real(e2);
167
168        // Radius of curvature in prime vertical
169        let n = OHyperdual::from_real(a) / (one - e2_dual * sin_lat * sin_lat).sqrt();
170
171        // 4. Cartesian positions
172        let x = (n + alt) * cos_lat * cos_lon;
173        let y = (n + alt) * cos_lat * sin_lon;
174        let z = (n * OHyperdual::from_real(1.0 - e2) + alt) * sin_lat;
175
176        // 5. Cartesian velocities (using your exact J_rp logic mapped to hyperduals)
177        let term1 = n * OHyperdual::from_real(1.0 - e2) / (one - e2_dual * sin_lat * sin_lat);
178
179        let dx_dlat = -(term1 + alt) * sin_lat * cos_lon;
180        let dy_dlat = -(term1 + alt) * sin_lat * sin_lon;
181        let dz_dlat = term1 * cos_lat + alt * cos_lat;
182
183        let dx_dlon = -(n + alt) * cos_lat * sin_lon;
184        let dy_dlon = (n + alt) * cos_lat * cos_lon;
185        let dz_dlon = OHyperdual::from_real(0.0);
186
187        let dx_dalt = cos_lat * cos_lon;
188        let dy_dalt = cos_lat * sin_lon;
189        let dz_dalt = sin_lat;
190
191        let vx = dx_dlat * lat_rate + dx_dlon * lon_rate + dx_dalt * alt_rate;
192        let vy = dy_dlat * lat_rate + dy_dlon * lon_rate + dy_dalt * alt_rate;
193        let vz = dz_dlat * lat_rate + dz_dlon * lon_rate + dz_dalt * alt_rate;
194
195        // 6. Extract the 6x6 Jacobian matrix from the dual parts
196        let mut jacobian = Matrix6::zeros();
197        let cartesian_state = [x, y, z, vx, vy, vz];
198
199        for i in 0..6 {
200            for j in 1..7 {
201                jacobian[(i, j - 1)] = cartesian_state[i][j];
202            }
203        }
204
205        Ok(jacobian)
206    }
207
208    /// Computes the great circle (Haversine) distance between this asset and another in kilometers.
209    ///
210    /// Note: This assumes a perfectly spherical body using the frame's mean equatorial radius.
211    /// It does not account for the ellipsoidal oblateness or the `height_km` of either asset.
212    pub fn great_circle_distance_km(&self, other: &Self) -> Result<f64, PhysicsError> {
213        let lat1 = self.latitude_deg.to_radians();
214        let lon1 = self.longitude_deg.to_radians();
215        let lat2 = other.latitude_deg.to_radians();
216        let lon2 = other.longitude_deg.to_radians();
217
218        let dlat = lat2 - lat1;
219        let dlon = lon2 - lon1;
220
221        let a = (dlat / 2.0).sin().powi(2) + lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
222
223        let c = 2.0 * a.sqrt().atan2((1.0 - a).sqrt());
224
225        // Extract the spherical radius from the frame definition
226        let radius_km = self.frame.mean_equatorial_radius_km()?;
227
228        Ok(radius_km * c)
229    }
230}
231
232impl Default for GroundAsset {
233    fn default() -> Self {
234        Self {
235            frame: Frame::from_ephem_j2000(399),
236            latitude_deg: 0.,
237            longitude_deg: 0.,
238            height_km: 0.,
239            latitude_rate_deg_s: 0.,
240            longitude_rate_deg_s: 0.,
241            height_rate_km_s: 0.0,
242            epoch: Epoch::from_tdb_seconds(0.0),
243            stm: None,
244        }
245    }
246}
247
248impl fmt::Display for GroundAsset {
249    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
250        write!(
251            f,
252            "lat.: {:.3} deg\tlong.: {:.3} deg\talt.: {:.3} km (speed: {:.3} m/s)",
253            self.latitude_deg,
254            self.longitude_deg,
255            self.height_km,
256            match self.velocity_sez_m_s() {
257                Ok(vel_m_s) => vel_m_s.norm(),
258                Err(_) => f64::NAN,
259            }
260        )
261    }
262}
263
264impl fmt::LowerExp for GroundAsset {
265    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
266        write!(
267            f,
268            "lat.: {:.e} deg\tlong.: {:.e} deg\talt.: {:.e} km (speed: {:.e} m/s)",
269            self.latitude_deg,
270            self.longitude_deg,
271            self.height_km,
272            match self.velocity_sez_m_s() {
273                Ok(vel_m_s) => vel_m_s.norm(),
274                Err(_) => f64::NAN,
275            }
276        )
277    }
278}
279
280impl State for GroundAsset {
281    // State is purely lat/long/alt
282    type Size = Const<6>;
283    type VecLength = Const<{ 6 + 36 }>;
284
285    fn to_vector(&self) -> nalgebra::OVector<f64, Self::VecLength> {
286        let mut vector = OVector::<f64, Const<42>>::zeros();
287        vector[0] = self.latitude_deg;
288        vector[1] = self.longitude_deg;
289        vector[2] = self.height_km;
290
291        vector[3] = self.latitude_rate_deg_s;
292        vector[4] = self.longitude_rate_deg_s;
293        vector[5] = self.height_rate_km_s;
294
295        if let Some(stm) = self.stm {
296            let stm_slice = stm.as_slice();
297            for i in 0..36 {
298                vector[6 + i] = stm_slice[i];
299            }
300        }
301
302        vector
303    }
304
305    fn orbit(&self) -> Orbit {
306        Orbit::try_latlongalt(
307            self.latitude_deg,
308            self.longitude_deg,
309            self.height_km,
310            self.epoch,
311            self.frame,
312        )
313        .expect("Ground asset frame does not allow init from lat/long/alt")
314    }
315
316    fn epoch(&self) -> Epoch {
317        self.epoch
318    }
319
320    fn set_epoch(&mut self, epoch: Epoch) {
321        self.epoch = epoch;
322    }
323
324    /// Vector is expected to be organized as such:
325    /// [Latitude, Longitude, Height, ]
326    fn set(&mut self, epoch: Epoch, vector: &OVector<f64, Const<42>>) {
327        let asset_state =
328            OVector::<f64, Self::Size>::from_column_slice(&vector.as_slice()[..Self::Size::dim()]);
329
330        if self.stm.is_some() {
331            let sc_full_stm = OMatrix::<f64, Self::Size, Self::Size>::from_column_slice(
332                &vector.as_slice()[Self::Size::dim()..],
333            );
334
335            self.stm = Some(sc_full_stm);
336        }
337
338        self.latitude_deg = asset_state[0];
339        self.longitude_deg = asset_state[1];
340        self.height_km = asset_state[2];
341
342        self.latitude_rate_deg_s = asset_state[3];
343        self.longitude_rate_deg_s = asset_state[4];
344        self.height_rate_km_s = asset_state[5];
345
346        self.epoch = epoch;
347    }
348
349    fn reset_stm(&mut self) {
350        self.stm = Some(OMatrix::<f64, Const<6>, Const<6>>::identity());
351    }
352
353    fn unset_stm(&mut self) {
354        self.stm = None;
355    }
356
357    fn stm(&self) -> Result<OMatrix<f64, Self::Size, Self::Size>, DynamicsError> {
358        match self.stm {
359            Some(stm) => Ok(stm),
360            None => Err(DynamicsError::StateTransitionMatrixUnset),
361        }
362    }
363
364    fn with_stm(mut self) -> Self {
365        self.stm = Some(OMatrix::<f64, Const<6>, Const<6>>::identity());
366        self
367    }
368
369    fn add(self, other: OVector<f64, Self::Size>) -> Self {
370        self + other
371    }
372
373    fn value(&self, param: StateParameter) -> Result<f64, crate::errors::StateError> {
374        match param {
375            StateParameter::Element(OrbitalElement::Latitude) => Ok(self.latitude_deg),
376            StateParameter::Element(OrbitalElement::Longitude) => Ok(self.longitude_deg),
377            StateParameter::Element(OrbitalElement::Height) => Ok(self.height_km),
378            _ => Err(crate::errors::StateError::Unavailable { param }),
379        }
380    }
381}
382
383impl Interpolatable for GroundAsset {
384    fn interpolate(mut self, epoch: Epoch, states: &[Self]) -> Result<Self, InterpolationError> {
385        // Interpolate the Orbit first
386        // Statically allocated arrays of the maximum number of samples
387        let mut epochs_tdb = [0.0; INTERPOLATION_SAMPLES];
388        let mut xs = [0.0; INTERPOLATION_SAMPLES];
389        let mut ys = [0.0; INTERPOLATION_SAMPLES];
390        let mut zs = [0.0; INTERPOLATION_SAMPLES];
391        let mut vxs = [0.0; INTERPOLATION_SAMPLES];
392        let mut vys = [0.0; INTERPOLATION_SAMPLES];
393        let mut vzs = [0.0; INTERPOLATION_SAMPLES];
394
395        for (cno, state) in states.iter().enumerate() {
396            xs[cno] = state.latitude_deg;
397            ys[cno] = state.longitude_deg;
398            zs[cno] = state.height_km;
399            vxs[cno] = state.latitude_rate_deg_s;
400            vys[cno] = state.longitude_rate_deg_s;
401            vzs[cno] = state.height_rate_km_s;
402            epochs_tdb[cno] = state.epoch.to_et_seconds();
403        }
404
405        // Ensure that if we don't have enough states, we only interpolate using what we have instead of INTERPOLATION_SAMPLES
406        let n = states.len();
407
408        let (latitude_deg, latitude_vel_deg_s) =
409            hermite_eval(&epochs_tdb[..n], &xs[..n], &vxs[..n], epoch.to_et_seconds())?;
410
411        let (longitude_deg, longitude_vel_deg_s) =
412            hermite_eval(&epochs_tdb[..n], &ys[..n], &vys[..n], epoch.to_et_seconds())?;
413
414        let (height_km, height_vel_km_s) =
415            hermite_eval(&epochs_tdb[..n], &zs[..n], &vzs[..n], epoch.to_et_seconds())?;
416
417        self.latitude_deg = latitude_deg;
418        self.longitude_deg = longitude_deg;
419        self.height_km = height_km;
420        self.latitude_rate_deg_s = latitude_vel_deg_s;
421        self.longitude_rate_deg_s = longitude_vel_deg_s;
422        self.height_rate_km_s = height_vel_km_s;
423
424        self.epoch = epoch;
425
426        Ok(self)
427    }
428
429    fn frame(&self) -> Frame {
430        self.frame
431    }
432
433    fn set_frame(&mut self, frame: Frame) {
434        self.frame = frame;
435    }
436
437    fn export_params() -> Vec<StateParameter> {
438        vec![
439            StateParameter::Element(OrbitalElement::Latitude),
440            StateParameter::Element(OrbitalElement::Longitude),
441            StateParameter::Element(OrbitalElement::Height),
442        ]
443    }
444}
445
446impl Add<OVector<f64, Const<6>>> for GroundAsset {
447    type Output = Self;
448
449    /// Adds the provided state deviation to this orbit
450    fn add(mut self, asset_state: OVector<f64, Const<6>>) -> Self {
451        self.latitude_deg += asset_state[0];
452        self.longitude_deg += asset_state[1];
453        self.height_km += asset_state[2];
454
455        self.latitude_rate_deg_s += asset_state[3];
456        self.longitude_rate_deg_s += asset_state[4];
457        self.height_rate_km_s += asset_state[5];
458
459        self
460    }
461}
462
463pub fn latlongalt_rate(
464    orbit: Orbit,
465    velocity_sez_km_s: Vector3<f64>,
466) -> Result<(f64, f64, f64), PhysicsError> {
467    // Get current lat, long, alt
468    let (lat_deg, _long_deg, alt_km) = orbit.latlongalt()?;
469    let lat_rad = lat_deg.to_radians();
470
471    // Extract SEZ velocity components
472    // SEZ frame: x=South, y=East, z=Zenith (up)
473    let v_south = velocity_sez_km_s.x;
474    let v_east = velocity_sez_km_s.y;
475    let v_zenith = velocity_sez_km_s.z;
476
477    // Get ellipsoid parameters
478    let a_km = orbit.frame.mean_equatorial_radius_km()?;
479    // If there is no shape data, the previous line returns an error, so we
480    // can safely unwrap here.
481    let b_km = orbit.frame.shape.unwrap().polar_radius_km;
482    let e2 = (a_km.powi(2) - b_km.powi(2)) / a_km.powi(2);
483
484    // Compute radius of curvature in the meridian (N)
485    let sin_lat = lat_rad.sin();
486    let n = a_km / (1.0 - e2 * sin_lat.powi(2)).sqrt();
487
488    // Compute radius of curvature in the prime vertical (M)
489    let m = a_km * (1.0 - e2) / (1.0 - e2 * sin_lat.powi(2)).powf(1.5);
490
491    // Convert SEZ velocities to geodetic coordinate rates
492    // Altitude rate (positive = up)
493    let alt_rate_km_s = v_zenith;
494
495    // Latitude rate (positive = north, but SEZ x-axis is south)
496    let lat_rate_rad_s = -v_south / (m + alt_km);
497    let lat_rate_deg_s = lat_rate_rad_s.to_degrees();
498
499    // Longitude rate (positive = east)
500    let cos_lat = lat_rad.cos();
501    let long_rate_rad_s = if cos_lat.abs() > 1e-10 {
502        v_east / ((n + alt_km) * cos_lat)
503    } else {
504        0.0 // At poles, longitude rate is undefined
505    };
506    let long_rate_deg_s = long_rate_rad_s.to_degrees();
507
508    Ok((lat_rate_deg_s, long_rate_deg_s, alt_rate_km_s))
509}
510
511/// Convert geodetic coordinate rates (lat rate, long rate, alt rate) to velocity in SEZ frame
512pub fn velocity_sez_from_latlongalt_rate(
513    orbit: Orbit,
514    lat_rate_deg_s: f64,
515    long_rate_deg_s: f64,
516    alt_rate_km_s: f64,
517) -> Result<Vector3<f64>, PhysicsError> {
518    // Get current lat and alt
519    let (lat_deg, _long_deg, alt_km) = orbit.latlongalt()?;
520    let lat_rad = lat_deg.to_radians();
521
522    // Get ellipsoid parameters
523    let a_km = orbit.frame.mean_equatorial_radius_km()?;
524    // If there is no shape data, the previous line returns an error, so we
525    // can safely unwrap here.
526    let b_km = orbit.frame.shape.unwrap().polar_radius_km;
527    let e2 = (a_km.powi(2) - b_km.powi(2)) / a_km.powi(2);
528
529    // Compute radius of curvature in the meridian (M)
530    let sin_lat = lat_rad.sin();
531    let m = a_km * (1.0 - e2) / (1.0 - e2 * sin_lat.powi(2)).powf(1.5);
532
533    // Compute radius of curvature in the prime vertical (N)
534    let n = a_km / (1.0 - e2 * sin_lat.powi(2)).sqrt();
535
536    // Convert rates to radians per second
537    let lat_rate_rad_s = lat_rate_deg_s.to_radians();
538    let long_rate_rad_s = long_rate_deg_s.to_radians();
539
540    // Convert geodetic coordinate rates to SEZ velocities
541    // SEZ frame: x=South, y=East, z=Zenith (up)
542    let v_south = -lat_rate_rad_s * (m + alt_km); // Negative because x is south
543    let v_east = long_rate_rad_s * (n + alt_km) * lat_rad.cos();
544    let v_zenith = alt_rate_km_s;
545
546    Ok(Vector3::new(v_south, v_east, v_zenith))
547}
nyx_space/od/groundpnt/mod.rs

nyx_space/od/groundpnt/
mod.rs
