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

nyx_space/od/ground_station/
trk_device.rs
