nyx_space/od/position/
trk_device.rs

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

nyx_space/od/position/
trk_device.rs
