nyx_space/od/groundpnt/
trk_device.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 anise::almanac::Almanac;
19use anise::ephemerides::EphemerisError;
20use anise::errors::{AlmanacError, AlmanacResult};
21use anise::math::interpolation::InterpolationError;
22use anise::prelude::{Frame, Orbit};
23use hifitime::{Epoch, TimeUnits};
24use indexmap::IndexSet;
25use rand_pcg::Pcg64Mcg;
26use snafu::ResultExt;
27
28use crate::md::prelude::Traj;
29use crate::od::groundpnt::GroundAsset;
30use crate::od::interlink::InterlinkTxSpacecraft;
31use crate::od::msr::MeasurementType;
32use crate::od::prelude::{Measurement, ODError};
33use crate::od::TrackingDevice;
34use crate::od::{ODAlmanacSnafu, ODTrajSnafu};
35use crate::State;
36
37use std::sync::Arc;
38
39impl TrackingDevice<GroundAsset> for InterlinkTxSpacecraft {
40    fn name(&self) -> String {
41        self.traj.name.clone().unwrap_or("unnamed".to_string())
42    }
43
44    fn measurement_types(&self) -> &IndexSet<MeasurementType> {
45        &self.measurement_types
46    }
47
48    fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc<Almanac>) -> AlmanacResult<Orbit> {
49        match self.traj.at(epoch) {
50            Ok(state) => almanac.transform_to(state.orbit, frame, self.ab_corr),
51            Err(_) => Err(AlmanacError::Ephemeris {
52                action: "computing location from Interlink",
53                source: Box::new(EphemerisError::EphemInterpolation {
54                    source: InterpolationError::MissingInterpolationData { epoch },
55                }),
56            }),
57        }
58    }
59
60    fn measure(
61        &mut self,
62        epoch: Epoch,
63        traj: &Traj<GroundAsset>,
64        rng: Option<&mut Pcg64Mcg>,
65        almanac: Arc<Almanac>,
66    ) -> Result<Option<Measurement>, ODError> {
67        match self.integration_time {
68            Some(integration_time) => {
69                // TODO: This should support measurement alignment
70                // If out of traj bounds, return None, else the whole strand is rejected.
71                let rx_0 = match traj.at(epoch - integration_time).context(ODTrajSnafu {
72                    details: format!(
73                        "fetching state {epoch} at start of ground station integration time {integration_time}"
74                    ),
75                }) {
76                    Ok(rx) => rx,
77                    Err(_) => return Ok(None),
78                };
79
80                let rx_1 = match traj.at(epoch).context(ODTrajSnafu {
81                    details: format!(
82                        "fetching state {epoch} at end of ground station integration time"
83                    ),
84                }) {
85                    Ok(rx) => rx,
86                    Err(_) => return Ok(None),
87                };
88
89                // Start of integration time
90                let msr_t0_opt = self.measure_instantaneous(rx_0, None, almanac.clone())?;
91
92                // End of integration time
93                let msr_t1_opt = self.measure_instantaneous(rx_1, None, almanac.clone())?;
94
95                if let Some(msr_t0) = msr_t0_opt {
96                    if let Some(msr_t1) = msr_t1_opt {
97                        // Line of sight in both cases
98
99                        // Noises are computed at the midpoint of the integration time.
100                        let noises = self.noises(epoch - integration_time * 0.5, rng)?;
101
102                        let mut msr = Measurement::new(
103                            "GroundAsset".to_string(),
104                            epoch + noises[0].seconds(),
105                        );
106
107                        for (ii, msr_type) in self.measurement_types.iter().enumerate() {
108                            let msr_value_0 = msr_t0.data[msr_type];
109                            let msr_value_1 = msr_t1.data[msr_type];
110
111                            let msr_value =
112                                (msr_value_1 + msr_value_0) * 0.5 + noises[ii + 1] / 2.0_f64.sqrt();
113                            msr.push(*msr_type, msr_value);
114                        }
115
116                        Ok(Some(msr))
117                    } else {
118                        Ok(None)
119                    }
120                } else {
121                    Ok(None)
122                }
123            }
124            None => self.measure_instantaneous(
125                traj.at(epoch).context(ODTrajSnafu {
126                    details: "fetching state for instantaneous measurement".to_string(),
127                })?,
128                rng,
129                almanac,
130            ),
131        }
132    }
133
134    /// Returns the Range and Doppler from pnt vehicle to ground asset (i.e. the opposed AER range and doppler data.)
135    fn measure_instantaneous(
136        &mut self,
137        rx: GroundAsset,
138        rng: Option<&mut Pcg64Mcg>,
139        almanac: Arc<Almanac>,
140    ) -> Result<Option<Measurement>, ODError> {
141        let pnt_veh = self.traj.at(rx.epoch()).context(ODTrajSnafu {
142            details: format!("fetching state {} for interlink", rx.epoch()),
143        })?;
144
145        let asset_loc = rx.to_location();
146
147        let aer = almanac
148            .azimuth_elevation_range_sez_from_location(pnt_veh.orbit, asset_loc, None, None)
149            .context(ODAlmanacSnafu {
150                action: "transforming receiver to transmitter frame",
151            })?;
152
153        if aer.elevation_above_mask_deg() < 0.0 {
154            Ok(None)
155        } else {
156            let noises = self.noises(rx.epoch, rng)?;
157
158            let mut msr =
159                Measurement::new("GroundAsset".to_string(), rx.epoch + noises[0].seconds());
160
161            // Range is a norm, so we don't flip the sign. But the rate depends on the direction of travel, so we do indeed flip its sign.
162            for (ii, msr_type) in self.measurement_types.iter().enumerate() {
163                let msr_value = match *msr_type {
164                    MeasurementType::Range => aer.range_km,
165                    MeasurementType::Doppler => aer.range_rate_km_s,
166                    // Or return an error for unsupported types
167                    _ => unreachable!("unsupported measurement type for interlink: {:?}", msr_type),
168                } + noises[ii + 1];
169                msr.push(*msr_type, msr_value);
170            }
171
172            Ok(Some(msr))
173        }
174    }
175
176    /// Returns the measurement noise of this ground station.
177    ///
178    /// # Methodology
179    /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise.
180    /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is
181    /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The
182    /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process.
183    fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result<f64, ODError> {
184        let stochastics =
185            self.stochastic_noises
186                .as_ref()
187                .ok_or_else(|| ODError::NoiseNotConfigured {
188                    kind: "stochastic noises for interlink transmitter".to_string(),
189                })?;
190
191        Ok(stochastics
192            .get(&msr_type)
193            .ok_or(ODError::NoiseNotConfigured {
194                kind: format!("{msr_type:?}"),
195            })?
196            .covariance(epoch))
197    }
198
199    fn measurement_bias(&self, msr_type: MeasurementType, _epoch: Epoch) -> Result<f64, ODError> {
200        let stochastics = self.stochastic_noises.as_ref().unwrap();
201
202        if let Some(gm) = stochastics
203            .get(&msr_type)
204            .ok_or(ODError::NoiseNotConfigured {
205                kind: format!("{msr_type:?}"),
206            })?
207            .bias
208        {
209            Ok(gm.constant.unwrap_or(0.0))
210        } else {
211            Ok(0.0)
212        }
213    }
214}
nyx_space/od/groundpnt/trk_device.rs

nyx_space/od/groundpnt/
trk_device.rs
