nyx_space/od/interlink/
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::astro::Aberration;
20use anise::errors::AlmanacResult;
21use anise::prelude::{Frame, Orbit};
22use hifitime::{Duration, Epoch, TimeUnits};
23use indexmap::{IndexMap, IndexSet};
24use rand_pcg::Pcg64Mcg;
25use serde::{Deserialize, Serialize};
26use snafu::{ensure, ResultExt};
27
28use crate::io::ConfigRepr;
29use crate::md::prelude::Traj;
30use crate::md::Trajectory;
31use crate::od::msr::MeasurementType;
32use crate::od::noise::StochasticNoise;
33use crate::od::prelude::{Measurement, NoiseNotConfiguredSnafu, ODError};
34use crate::od::TrackingDevice;
35use crate::od::{ODAlmanacSnafu, ODTrajSnafu};
36use crate::Spacecraft;
37use crate::State;
38
39use std::sync::Arc;
40
41// Defines a (transmitter) spacecraft capable of inter-satellite links.
42// NOTE: There is _no_ `InterlinkRxSpacecraft`, instead you must independently build their trajectories and provide them to the InterlinkArcSim.
43#[derive(Clone, Debug)]
44pub struct InterlinkTxSpacecraft {
45    /// Trajectory of the transmitter spacercaft
46    pub traj: Trajectory,
47    /// Measurement types supported by the link
48    pub measurement_types: IndexSet<MeasurementType>,
49    /// Integration time used to generate the measurement.
50    pub integration_time: Option<Duration>,
51    pub timestamp_noise_s: Option<StochasticNoise>,
52    pub stochastic_noises: Option<IndexMap<MeasurementType, StochasticNoise>>,
53    /// Aberration correction used in the interlink
54    pub ab_corr: Option<Aberration>,
55}
56
57impl InterlinkTxSpacecraft {
58    /// Returns the noises for all measurement types configured for this ground station at the provided epoch, timestamp noise is the first entry.
59    pub fn noises(
60        &mut self,
61        epoch: Epoch,
62        rng: Option<&mut Pcg64Mcg>,
63    ) -> Result<Vec<f64>, ODError> {
64        let mut noises = vec![0.0; self.measurement_types.len() + 1];
65
66        if let Some(rng) = rng {
67            ensure!(
68                self.stochastic_noises.is_some(),
69                NoiseNotConfiguredSnafu {
70                    kind: "ground station stochastics".to_string(),
71                }
72            );
73            // Add the timestamp noise first
74
75            if let Some(mut timestamp_noise) = self.timestamp_noise_s {
76                noises[0] = timestamp_noise.sample(epoch, rng);
77            }
78
79            let stochastics = self.stochastic_noises.as_mut().unwrap();
80
81            for (ii, msr_type) in self.measurement_types.iter().enumerate() {
82                noises[ii + 1] = stochastics
83                    .get_mut(msr_type)
84                    .ok_or(ODError::NoiseNotConfigured {
85                        kind: format!("{msr_type:?}"),
86                    })?
87                    .sample(epoch, rng);
88            }
89        }
90
91        Ok(noises)
92    }
93}
94
95impl TrackingDevice<Spacecraft> for InterlinkTxSpacecraft {
96    fn name(&self) -> String {
97        self.traj.name.clone().unwrap_or("unnamed".to_string())
98    }
99
100    fn measurement_types(&self) -> &IndexSet<MeasurementType> {
101        &self.measurement_types
102    }
103
104    fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc<Almanac>) -> AlmanacResult<Orbit> {
105        almanac.transform_to(self.traj.at(epoch).unwrap().orbit, frame, self.ab_corr)
106    }
107
108    fn measure(
109        &mut self,
110        epoch: Epoch,
111        traj: &Traj<Spacecraft>,
112        rng: Option<&mut Pcg64Mcg>,
113        almanac: Arc<Almanac>,
114    ) -> Result<Option<Measurement>, ODError> {
115        match self.integration_time {
116            Some(integration_time) => {
117                // TODO: This should support measurement alignment
118                // If out of traj bounds, return None, else the whole strand is rejected.
119                let rx_0 = match traj.at(epoch - integration_time).context(ODTrajSnafu {
120                    details: format!(
121                        "fetching state {epoch} at start of ground station integration time {integration_time}"
122                    ),
123                }) {
124                    Ok(rx) => rx,
125                    Err(_) => return Ok(None),
126                };
127
128                let rx_1 = match traj.at(epoch).context(ODTrajSnafu {
129                    details: format!(
130                        "fetching state {epoch} at end of ground station integration time"
131                    ),
132                }) {
133                    Ok(rx) => rx,
134                    Err(_) => return Ok(None),
135                };
136
137                // Start of integration time
138                let msr_t0_opt = self.measure_instantaneous(rx_0, None, almanac.clone())?;
139
140                // End of integration time
141                let msr_t1_opt = self.measure_instantaneous(rx_1, None, almanac.clone())?;
142
143                if let Some(msr_t0) = msr_t0_opt {
144                    if let Some(msr_t1) = msr_t1_opt {
145                        // Line of sight in both cases
146
147                        // Noises are computed at the midpoint of the integration time.
148                        let noises = self.noises(epoch - integration_time * 0.5, rng)?;
149
150                        let mut msr = Measurement::new(
151                            <InterlinkTxSpacecraft as TrackingDevice<Spacecraft>>::name(self),
152                            epoch + noises[0].seconds(),
153                        );
154
155                        for (ii, msr_type) in self.measurement_types.iter().enumerate() {
156                            let msr_value_0 = msr_t0.data[msr_type];
157                            let msr_value_1 = msr_t1.data[msr_type];
158
159                            let msr_value =
160                                (msr_value_1 + msr_value_0) * 0.5 + noises[ii + 1] / 2.0_f64.sqrt();
161                            msr.push(*msr_type, msr_value);
162                        }
163
164                        Ok(Some(msr))
165                    } else {
166                        Ok(None)
167                    }
168                } else {
169                    Ok(None)
170                }
171            }
172            None => self.measure_instantaneous(
173                traj.at(epoch).context(ODTrajSnafu {
174                    details: "fetching state for instantaneous measurement".to_string(),
175                })?,
176                rng,
177                almanac,
178            ),
179        }
180    }
181
182    fn measure_instantaneous(
183        &mut self,
184        rx: Spacecraft,
185        rng: Option<&mut Pcg64Mcg>,
186        almanac: Arc<Almanac>,
187    ) -> Result<Option<Measurement>, ODError> {
188        let observer = self.traj.at(rx.epoch()).context(ODTrajSnafu {
189            details: format!("fetching state {} for interlink", rx.epoch()),
190        })?;
191
192        let is_obstructed = almanac
193            .line_of_sight_obstructed(observer.orbit, rx.orbit, observer.orbit.frame, self.ab_corr)
194            .context(ODAlmanacSnafu {
195                action: "computing line of sight",
196            })?;
197
198        if is_obstructed {
199            Ok(None)
200        } else {
201            // Convert the receiver into the body fixed transmitter frame.
202            let rx_in_tx_frame = almanac
203                .transform_to(rx.orbit, observer.orbit.frame, self.ab_corr)
204                .context(ODAlmanacSnafu {
205                    action: "transforming receiver to transmitter frame",
206                })?;
207
208            let rho_tx_frame = rx_in_tx_frame.radius_km - observer.orbit.radius_km;
209
210            // Compute the range-rate \dot ρ. Note that rx_in_tx_frame is already the relative velocity of rx wrt tx!
211            let range_rate_km_s =
212                rho_tx_frame.dot(&rx_in_tx_frame.velocity_km_s) / rho_tx_frame.norm();
213
214            let noises = self.noises(observer.epoch(), rng)?;
215
216            let mut msr = Measurement::new(
217                <InterlinkTxSpacecraft as TrackingDevice<Spacecraft>>::name(self),
218                rx.orbit.epoch + noises[0].seconds(),
219            );
220
221            for (ii, msr_type) in self.measurement_types.iter().enumerate() {
222                let msr_value = match *msr_type {
223                    MeasurementType::Range => rho_tx_frame.norm(),
224                    MeasurementType::Doppler => range_rate_km_s,
225                    // Or return an error for unsupported types
226                    _ => unreachable!("unsupported measurement type for interlink: {:?}", msr_type),
227                } + noises[ii + 1];
228                msr.push(*msr_type, msr_value);
229            }
230
231            Ok(Some(msr))
232        }
233    }
234
235    /// Returns the measurement noise of this ground station.
236    ///
237    /// # Methodology
238    /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise.
239    /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is
240    /// 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
241    /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process.
242    fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result<f64, ODError> {
243        let stochastics = self.stochastic_noises.as_ref().unwrap();
244
245        Ok(stochastics
246            .get(&msr_type)
247            .ok_or(ODError::NoiseNotConfigured {
248                kind: format!("{msr_type:?}"),
249            })?
250            .covariance(epoch))
251    }
252
253    fn measurement_bias(&self, msr_type: MeasurementType, _epoch: Epoch) -> Result<f64, ODError> {
254        let stochastics = self.stochastic_noises.as_ref().unwrap();
255
256        if let Some(gm) = stochastics
257            .get(&msr_type)
258            .ok_or(ODError::NoiseNotConfigured {
259                kind: format!("{msr_type:?}"),
260            })?
261            .bias
262        {
263            Ok(gm.constant.unwrap_or(0.0))
264        } else {
265            Ok(0.0)
266        }
267    }
268}
269
270impl Serialize for InterlinkTxSpacecraft {
271    fn serialize<S>(&self, _serializer: S) -> Result<S::Ok, S::Error>
272    where
273        S: serde::Serializer,
274    {
275        unimplemented!("interlink spacecraft cannot be serialized")
276    }
277}
278
279impl<'de> Deserialize<'de> for InterlinkTxSpacecraft {
280    fn deserialize<D>(_deserializer: D) -> Result<Self, D::Error>
281    where
282        D: serde::Deserializer<'de>,
283    {
284        unimplemented!("interlink spacecraft cannot be deserialized")
285    }
286}
287
288impl ConfigRepr for InterlinkTxSpacecraft {}
nyx_space/od/interlink/trk_device.rs

nyx_space/od/interlink/
trk_device.rs
