nyx_space/od/ground_station/
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*/
18
19use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice};
20use crate::md::prelude::{Interpolatable, Traj};
21use crate::od::msr::measurement::Measurement;
22use crate::od::msr::MeasurementType;
23use crate::time::Epoch;
24use crate::Spacecraft;
25use anise::errors::AlmanacResult;
26use anise::frames::Frame;
27use anise::prelude::{Aberration, Almanac, Orbit};
28use hifitime::TimeUnits;
29use indexmap::IndexSet;
30use log::debug;
31use rand_pcg::Pcg64Mcg;
32use snafu::ResultExt;
33use std::sync::Arc;
34
35use super::GroundStation;
36
37impl TrackingDevice<Spacecraft> for GroundStation {
38    fn measurement_types(&self) -> &IndexSet<MeasurementType> {
39        &self.measurement_types
40    }
41
42    /// Perform a measurement from the ground station to the receiver (rx).
43    fn measure(
44        &mut self,
45        epoch: Epoch,
46        traj: &Traj<Spacecraft>,
47        rng: Option<&mut Pcg64Mcg>,
48        almanac: Arc<Almanac>,
49    ) -> Result<Option<Measurement>, ODError> {
50        match self.integration_time {
51            Some(integration_time) => {
52                // TODO: This should support measurement alignment
53                // If out of traj bounds, return None, else the whole strand is rejected.
54                let rx_0 = match traj.at(epoch - integration_time).context(ODTrajSnafu {
55                    details: format!(
56                        "fetching state {epoch} at start of ground station integration time {integration_time}"
57                    ),
58                }) {
59                    Ok(rx) => rx,
60                    Err(_) => return Ok(None),
61                };
62
63                let rx_1 = match traj.at(epoch).context(ODTrajSnafu {
64                    details: format!(
65                        "fetching state {epoch} at end of ground station integration time"
66                    ),
67                }) {
68                    Ok(rx) => rx,
69                    Err(_) => return Ok(None),
70                };
71
72                let obstructing_body =
73                    if self.location.frame.ephemeris_id != rx_0.frame().ephemeris_id {
74                        Some(rx_0.frame())
75                    } else {
76                        None
77                    };
78
79                let ab_corr = if self.light_time_correction {
80                    Aberration::LT
81                } else {
82                    Aberration::NONE
83                };
84
85                let aer_t0 = almanac
86                    .azimuth_elevation_range_sez_from_location(
87                        rx_0.orbit,
88                        self.location.clone(),
89                        obstructing_body,
90                        ab_corr,
91                    )
92                    .context(ODAlmanacSnafu {
93                        action: "computing AER",
94                    })?;
95
96                let aer_t1 = almanac
97                    .azimuth_elevation_range_sez_from_location(
98                        rx_1.orbit,
99                        self.location.clone(),
100                        obstructing_body,
101                        ab_corr,
102                    )
103                    .context(ODAlmanacSnafu {
104                        action: "computing AER",
105                    })?;
106
107                if aer_t0.elevation_above_mask_deg() < 0.0
108                    || aer_t1.elevation_above_mask_deg() < 0.0
109                {
110                    debug!(
111                        "{} {} obstructed by terrain ({:.3} - {:.3} deg) -- no measurement",
112                        self.name,
113                        aer_t0.epoch,
114                        aer_t0.elevation_above_mask_deg(),
115                        aer_t1.elevation_above_mask_deg()
116                    );
117                    return Ok(None);
118                } else if aer_t0.is_obstructed() || aer_t1.is_obstructed() {
119                    debug!(
120                        "{} {} obstruction at t0={}, t1={} -- no measurement",
121                        self.name,
122                        aer_t0.epoch,
123                        aer_t0.is_obstructed(),
124                        aer_t1.is_obstructed()
125                    );
126                    return Ok(None);
127                }
128
129                // Noises are computed at the midpoint of the integration time.
130                let noises = self.noises(epoch - integration_time * 0.5, rng)?;
131
132                let mut msr = Measurement::new(self.name.clone(), epoch + noises[0].seconds());
133
134                for (ii, msr_type) in self.measurement_types.iter().enumerate() {
135                    let msr_value = msr_type.compute_two_way(aer_t0, aer_t1, noises[ii + 1])?;
136                    msr.push(*msr_type, msr_value);
137                }
138
139                Ok(Some(msr))
140            }
141            None => self.measure_instantaneous(
142                traj.at(epoch).context(ODTrajSnafu {
143                    details: "fetching state for instantaneous measurement".to_string(),
144                })?,
145                rng,
146                almanac,
147            ),
148        }
149    }
150
151    fn name(&self) -> String {
152        self.name.clone()
153    }
154
155    fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc<Almanac>) -> AlmanacResult<Orbit> {
156        almanac.transform_to(self.to_orbit(epoch, &almanac).unwrap(), frame, None)
157    }
158
159    fn measure_instantaneous(
160        &mut self,
161        rx: Spacecraft,
162        rng: Option<&mut Pcg64Mcg>,
163        almanac: Arc<Almanac>,
164    ) -> Result<Option<Measurement>, ODError> {
165        let obstructing_body = if self.location.frame.ephemeris_id != rx.frame().ephemeris_id {
166            Some(rx.frame())
167        } else {
168            None
169        };
170
171        let ab_corr = if self.light_time_correction {
172            Aberration::LT
173        } else {
174            Aberration::NONE
175        };
176
177        let aer = almanac
178            .azimuth_elevation_range_sez_from_location(
179                rx.orbit,
180                self.location.clone(),
181                obstructing_body,
182                ab_corr,
183            )
184            .context(ODAlmanacSnafu {
185                action: "computing AER",
186            })?;
187
188        if aer.elevation_above_mask_deg() >= 0.0 && !aer.is_obstructed() {
189            // Only update the noises if the measurement is valid.
190            let noises = self.noises(rx.orbit.epoch, rng)?;
191
192            let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch + noises[0].seconds());
193
194            for (ii, msr_type) in self.measurement_types.iter().enumerate() {
195                let msr_value = msr_type.compute_one_way(aer, noises[ii + 1])?;
196                msr.push(*msr_type, msr_value);
197            }
198
199            Ok(Some(msr))
200        } else {
201            debug!(
202                "{} {} object at {:.3} deg -- no measurement",
203                self.name,
204                rx.orbit.epoch,
205                aer.elevation_above_mask_deg(),
206            );
207            Ok(None)
208        }
209    }
210
211    /// Returns the measurement noise of this ground station.
212    ///
213    /// # Methodology
214    /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise.
215    /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is
216    /// 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
217    /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process.
218    fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result<f64, ODError> {
219        let stochastics = self.stochastic_noises.as_ref().unwrap();
220
221        Ok(stochastics
222            .get(&msr_type)
223            .ok_or(ODError::NoiseNotConfigured {
224                kind: format!("{msr_type:?}"),
225            })?
226            .covariance(epoch))
227    }
228
229    fn measurement_bias(&self, msr_type: MeasurementType, _epoch: Epoch) -> Result<f64, ODError> {
230        let stochastics = self.stochastic_noises.as_ref().unwrap();
231
232        if let Some(gm) = stochastics
233            .get(&msr_type)
234            .ok_or(ODError::NoiseNotConfigured {
235                kind: format!("{msr_type:?}"),
236            })?
237            .bias
238        {
239            Ok(gm.constant.unwrap_or(0.0))
240        } else {
241            Ok(0.0)
242        }
243    }
244}
nyx_space/od/ground_station/trk_device.rs

nyx_space/od/ground_station/
trk_device.rs
