nyx_space/od/position/
trk_device.rs

1use crate::md::prelude::Traj;
2use crate::od::msr::measurement::Measurement;
3use crate::od::msr::MeasurementType;
4use crate::od::NoiseNotConfiguredSnafu;
5use crate::od::{ODError, ODTrajSnafu, TrackingDevice};
6use crate::time::Epoch;
7use crate::Spacecraft;
8use anise::errors::AlmanacResult;
9use anise::frames::Frame;
10use anise::prelude::{Almanac, Orbit};
11use indexmap::{IndexMap, IndexSet};
12use rand_pcg::Pcg64Mcg;
13use snafu::{ensure, ResultExt};
14use std::sync::Arc;
15
16use super::PositionDevice;
17
18impl TrackingDevice<Spacecraft> for PositionDevice {
19    fn measurement_types(&self) -> &IndexSet<MeasurementType> {
20        &self.measurement_types
21    }
22
23    fn measure(
24        &mut self,
25        epoch: Epoch,
26        traj: &Traj<Spacecraft>,
27        rng: Option<&mut Pcg64Mcg>,
28        almanac: Arc<Almanac>,
29    ) -> Result<Option<Measurement>, ODError> {
30        let rx = traj.at(epoch).context(ODTrajSnafu {
31            details: "fetching state for instantaneous measurement".to_string(),
32        })?;
33        self.measure_instantaneous(rx, rng, almanac)
34    }
35
36    fn name(&self) -> String {
37        self.name.clone()
38    }
39
40    fn location(
41        &self,
42        _epoch: Epoch,
43        _frame: Frame,
44        _almanac: Arc<Almanac>,
45    ) -> AlmanacResult<Orbit> {
46        // XyzDevice does not have a location
47        unimplemented!("XyzDevice does not have a location")
48    }
49
50    fn measure_instantaneous(
51        &mut self,
52        rx: Spacecraft,
53        rng: Option<&mut Pcg64Mcg>,
54        _almanac: Arc<Almanac>,
55    ) -> Result<Option<Measurement>, ODError> {
56        let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch);
57        let mut noises = IndexMap::with_capacity(self.measurement_types.len());
58
59        if let Some(rng) = rng {
60            ensure!(
61                self.stochastic_noises.is_some(),
62                NoiseNotConfiguredSnafu {
63                    kind: "ground station stochastics".to_string(),
64                }
65            );
66
67            let stochastics = self.stochastic_noises.as_mut().unwrap();
68
69            for msr_type in &self.measurement_types {
70                noises.insert(
71                    *msr_type,
72                    stochastics
73                        .get_mut(msr_type)
74                        .ok_or(ODError::NoiseNotConfigured {
75                            kind: format!("{msr_type:?}"),
76                        })?
77                        .sample(rx.orbit.epoch, rng),
78                );
79            }
80        }
81
82        for (ii, msr_type) in self.measurement_types.iter().copied().enumerate() {
83            msr.push(
84                msr_type,
85                rx.orbit.radius_km[ii]
86                    + noises.get(&msr_type).unwrap_or(&0.0)
87                    + self.measurement_bias(msr_type, rx.orbit.epoch)?,
88            );
89        }
90
91        Ok(Some(msr))
92    }
93
94    fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result<f64, ODError> {
95        if let Some(stochastics) = &self.stochastic_noises {
96            Ok(stochastics
97                .get(&msr_type)
98                .ok_or(ODError::NoiseNotConfigured {
99                    kind: format!("{msr_type:?}"),
100                })?
101                .covariance(epoch))
102        } else {
103            Ok(0.0)
104        }
105    }
106
107    fn measurement_bias(&self, msr_type: MeasurementType, _epoch: Epoch) -> Result<f64, ODError> {
108        if let Some(stochastics) = &self.stochastic_noises {
109            if let Some(gm) = stochastics
110                .get(&msr_type)
111                .ok_or(ODError::NoiseNotConfigured {
112                    kind: format!("{msr_type:?}"),
113                })?
114                .bias
115            {
116                Ok(gm.constant.unwrap_or(0.0))
117            } else {
118                Ok(0.0)
119            }
120        } else {
121            Ok(0.0)
122        }
123    }
124}
nyx_space/od/position/trk_device.rs

nyx_space/od/position/
trk_device.rs
