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        let orbit = almanac
77            .transform_to(rx.orbit, self.frame, None)
78            .map_err(|e| ODError::MeasurementSimError {
79                details: format!("Failed to transform to frame {:?}: {e}", self.frame),
80            })?;
81
82        for msr_type in self.measurement_types.iter().copied() {
83            let val = match msr_type {
84                MeasurementType::X => orbit.radius_km.x,
85                MeasurementType::Y => orbit.radius_km.y,
86                MeasurementType::Z => orbit.radius_km.z,
87                _ => {
88                    return Err(ODError::MeasurementSimError {
89                        details: format!("{msr_type:?} not supported by PositionDevice"),
90                    });
91                }
92            };
93
94            msr.push(
95                msr_type,
96                val + noises.get(&msr_type).unwrap_or(&0.0)
97                    + self.measurement_bias(msr_type, rx.orbit.epoch)?,
98            );
99        }
100
101        Ok(Some(msr))
102    }
103
104    fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result<f64, ODError> {
105        if let Some(stochastics) = &self.stochastic_noises {
106            Ok(stochastics
107                .get(&msr_type)
108                .ok_or(ODError::NoiseNotConfigured {
109                    kind: format!("{msr_type:?}"),
110                })?
111                .covariance(epoch))
112        } else {
113            Ok(0.0)
114        }
115    }
116
117    fn measurement_bias(&self, msr_type: MeasurementType, _epoch: Epoch) -> Result<f64, ODError> {
118        if let Some(stochastics) = &self.stochastic_noises {
119            if let Some(gm) = stochastics
120                .get(&msr_type)
121                .ok_or(ODError::NoiseNotConfigured {
122                    kind: format!("{msr_type:?}"),
123                })?
124                .bias
125            {
126                Ok(gm.constant.unwrap_or(0.0))
127            } else {
128                Ok(0.0)
129            }
130        } else {
131            Ok(0.0)
132        }
133    }
134}
nyx_space/od/position/trk_device.rs

nyx_space/od/position/
trk_device.rs
