nyx_space/od/msr/trackingdata/
mod.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 super::{measurement::Measurement, MeasurementType};
19use core::fmt;
20use hifitime::prelude::{Duration, Epoch};
21use hifitime::Unit;
22use indexmap::{IndexMap, IndexSet};
23use std::collections::btree_map::Entry;
24use std::collections::BTreeMap;
25use std::ops::Bound::{self, Excluded, Included, Unbounded};
26use std::ops::{Add, AddAssign, RangeBounds};
27
28mod io_ccsds_tdm;
29mod io_parquet;
30
31/// Tracking data storing all of measurements as a B-Tree.
32/// It inherently does NOT support multiple concurrent measurements from several trackers.
33///
34/// # Measurement Moduli, e.g. range modulus
35///
36/// In the case of ranging, and possibly other data types, a code is used to measure the range to the spacecraft. The length of this code
37/// determines the ambiguity resolution, as per equation 9 in section 2.2.2.2 of the JPL DESCANSO, document 214, _Pseudo-Noise and Regenerative Ranging_.
38/// For example, using the JPL Range Code and a frequency range clock of 1 MHz, the range ambiguity is 75,660 km. In other words,
39/// as soon as the spacecraft is at a range of 75,660 + 1 km the JPL Range Code will report the vehicle to be at a range of 1 km.
40/// This is simply because the range code overlaps with itself, effectively loosing track of its own reference:
41/// it's due to the phase shift of the signal "lapping" the original signal length.
42///
43/// ```text
44///             (Spacecraft)
45///             ^
46///             |    Actual Distance = 75,661 km
47///             |
48/// 0 km                                         75,660 km (Wrap-Around)
49/// |-----------------------------------------------|
50///   When the "code length" is exceeded,
51///   measurements wrap back to 0.
52///
53/// So effectively:
54///     Observed code range = Actual range (mod 75,660 km)
55///     75,661 km → 1 km
56///
57/// ```
58///
59/// Nyx can only resolve the range ambiguity if the tracking data specifies a modulus for this specific measurement type.
60/// For example, in the case of the JPL Range Code and a 1 MHz range clock, the ambiguity interval is 75,660 km.
61///
62/// The measurement used in the Orbit Determination Process then becomes the following, where `//` represents the [Euclidian division](https://doc.rust-lang.org/std/primitive.f64.html#method.div_euclid).
63///
64/// ```text
65/// k = computed_obs // ambiguity_interval
66/// real_obs = measured_obs + k * modulus
67/// ```
68///
69/// Reference: JPL DESCANSO, document 214, _Pseudo-Noise and Regenerative Ranging_.
70///
71#[derive(Clone, Default)]
72pub struct TrackingDataArc {
73    /// All measurements in this data arc
74    pub measurements: BTreeMap<Epoch, Measurement>, // BUG: Consider a map of tracking to epoch!
75    /// Source file if loaded from a file or saved to a file.
76    pub source: Option<String>,
77    /// Optionally provide a map of modulos (e.g. the RANGE_MODULO of CCSDS TDM).
78    pub moduli: Option<IndexMap<MeasurementType, f64>>,
79    /// Reject all of the measurements, useful for debugging passes.
80    pub force_reject: bool,
81}
82
83impl TrackingDataArc {
84    /// Set (or overwrites) the modulus of the provided measurement type.
85    pub fn set_moduli(&mut self, msr_type: MeasurementType, modulus: f64) {
86        if modulus.is_nan() || modulus.abs() < f64::EPSILON {
87            warn!("cannot set modulus for {msr_type:?} to {modulus}");
88            return;
89        }
90        if self.moduli.is_none() {
91            self.moduli = Some(IndexMap::new());
92        }
93
94        self.moduli.as_mut().unwrap().insert(msr_type, modulus);
95    }
96
97    /// Applies the moduli to each measurement, if defined.
98    pub fn apply_moduli(&mut self) {
99        if let Some(moduli) = &self.moduli {
100            for msr in self.measurements.values_mut() {
101                for (msr_type, modulus) in moduli {
102                    if let Some(msr_value) = msr.data.get_mut(msr_type) {
103                        *msr_value %= *modulus;
104                    }
105                }
106            }
107        }
108    }
109
110    /// Returns the unique list of aliases in this tracking data arc
111    pub fn unique_aliases(&self) -> IndexSet<String> {
112        self.unique().0
113    }
114
115    /// Returns the unique measurement types in this tracking data arc
116    pub fn unique_types(&self) -> IndexSet<MeasurementType> {
117        self.unique().1
118    }
119
120    /// Returns the unique trackers and unique measurement types in this data arc
121    pub fn unique(&self) -> (IndexSet<String>, IndexSet<MeasurementType>) {
122        let mut aliases = IndexSet::new();
123        let mut types = IndexSet::new();
124        for msr in self.measurements.values() {
125            aliases.insert(msr.tracker.clone());
126            for k in msr.data.keys() {
127                types.insert(*k);
128            }
129        }
130        (aliases, types)
131    }
132
133    /// Returns the start epoch of this tracking arc
134    pub fn start_epoch(&self) -> Option<Epoch> {
135        self.measurements.first_key_value().map(|(k, _)| *k)
136    }
137
138    /// Returns the end epoch of this tracking arc
139    pub fn end_epoch(&self) -> Option<Epoch> {
140        self.measurements.last_key_value().map(|(k, _)| *k)
141    }
142
143    /// Returns the duration this tracking arc
144    pub fn duration(&self) -> Option<Duration> {
145        match self.start_epoch() {
146            Some(start) => self.end_epoch().map(|end| end - start),
147            None => None,
148        }
149    }
150
151    /// Returns the number of measurements in this data arc
152    pub fn len(&self) -> usize {
153        self.measurements.len()
154    }
155
156    /// Returns whether this arc has no measurements.
157    pub fn is_empty(&self) -> bool {
158        self.measurements.is_empty()
159    }
160
161    /// Returns the minimum duration between two subsequent measurements, flooring at 0.1 seconds.
162    pub fn min_duration_sep(&self) -> Option<Duration> {
163        if self.is_empty() {
164            None
165        } else {
166            let mut min_sep = Duration::MAX;
167            let mut prev_epoch = self.start_epoch().unwrap();
168            for (epoch, _) in self.measurements.iter().skip(1) {
169                let this_sep = *epoch - prev_epoch;
170                min_sep = min_sep.min(this_sep);
171                prev_epoch = *epoch;
172            }
173            Some(min_sep.max(Unit::Second * 0.1))
174        }
175    }
176
177    /// Returns a new tracking arc that only contains measurements that fall within the given epoch range.
178    pub fn filter_by_epoch<R: RangeBounds<Epoch>>(mut self, bound: R) -> Self {
179        self.measurements = self
180            .measurements
181            .range(bound)
182            .map(|(epoch, msr)| (*epoch, msr.clone()))
183            .collect::<BTreeMap<Epoch, Measurement>>();
184        self
185    }
186
187    /// Returns a new tracking arc that only contains measurements that fall within the given offset from the first epoch
188    pub fn filter_by_offset<R: RangeBounds<Duration>>(self, bound: R) -> Self {
189        if self.is_empty() {
190            return self;
191        }
192        // Rebuild an epoch bound.
193        let start = match bound.start_bound() {
194            Unbounded => self.start_epoch().unwrap(),
195            Included(offset) | Excluded(offset) => self.start_epoch().unwrap() + *offset,
196        };
197
198        let end = match bound.end_bound() {
199            Unbounded => self.end_epoch().unwrap(),
200            Included(offset) | Excluded(offset) => self.end_epoch().unwrap() - *offset,
201        };
202
203        self.filter_by_epoch(start..end)
204    }
205
206    /// Returns a new tracking arc that only contains measurements from the desired tracker.
207    pub fn filter_by_tracker(mut self, tracker: String) -> Self {
208        self.measurements = self
209            .measurements
210            .iter()
211            .filter_map(|(epoch, msr)| {
212                if msr.tracker == tracker {
213                    Some((*epoch, msr.clone()))
214                } else {
215                    None
216                }
217            })
218            .collect::<BTreeMap<Epoch, Measurement>>();
219        self
220    }
221
222    /// Returns a new tracking arc that contains measurements from all trackers except the one provided
223    pub fn exclude_tracker(mut self, excluded_tracker: String) -> Self {
224        info!("Excluding tracker {excluded_tracker}");
225
226        self.measurements = self
227            .measurements
228            .iter()
229            .filter_map(|(epoch, msr)| {
230                if msr.tracker != excluded_tracker {
231                    Some((*epoch, msr.clone()))
232                } else {
233                    None
234                }
235            })
236            .collect::<BTreeMap<Epoch, Measurement>>();
237        self
238    }
239
240    /// Returns a new tracking arc that excludes measurements within the given epoch range.
241    pub fn exclude_by_epoch<R: RangeBounds<Epoch>>(mut self, bound: R) -> Self {
242        info!(
243            "Excluding measurements from {:?} to {:?}",
244            bound.start_bound(),
245            bound.end_bound()
246        );
247
248        let mut new_measurements = BTreeMap::new();
249
250        // Include entries before the excluded range
251        new_measurements.extend(
252            self.measurements
253                .range((Bound::Unbounded, bound.start_bound()))
254                .map(|(epoch, msr)| (*epoch, msr.clone())),
255        );
256
257        // Include entries after the excluded range
258        new_measurements.extend(
259            self.measurements
260                .range((bound.end_bound(), Bound::Unbounded))
261                .map(|(epoch, msr)| (*epoch, msr.clone())),
262        );
263
264        self.measurements = new_measurements;
265        self
266    }
267
268    /// Downsamples the tracking data to a lower frequency using a simple moving average low-pass filter followed by decimation,
269    /// returning new `TrackingDataArc` with downsampled measurements.
270    ///
271    /// It provides a computationally efficient approach to reduce the sampling rate while mitigating aliasing effects.
272    ///
273    /// # Algorithm
274    ///
275    /// 1. A simple moving average filter is applied as a low-pass filter.
276    /// 2. Decimation is performed by selecting every Nth sample after filtering.
277    ///
278    /// # Advantages
279    ///
280    /// - Computationally efficient, suitable for large datasets common in spaceflight applications.
281    /// - Provides basic anti-aliasing, crucial for preserving signal integrity in orbit determination and tracking.
282    /// - Maintains phase information, important for accurate timing in spacecraft state estimation.
283    ///
284    /// # Limitations
285    ///
286    /// - The frequency response is not as sharp as more sophisticated filters (e.g., FIR, IIR).
287    /// - May not provide optimal stopband attenuation for high-precision applications.
288    ///
289    /// ## Considerations for Spaceflight Applications
290    ///
291    /// - Suitable for initial data reduction in ground station tracking pipelines.
292    /// - Adequate for many orbit determination and tracking tasks where computational speed is prioritized.
293    /// - For high-precision applications (e.g., interplanetary navigation), consider using more advanced filtering techniques.
294    ///
295    pub fn downsample(self, target_step: Duration) -> Self {
296        if self.is_empty() {
297            return self;
298        }
299        let current_step = self.min_duration_sep().unwrap();
300
301        if current_step >= target_step {
302            warn!("cannot downsample tracking data from {current_step} to {target_step} (that would be upsampling)");
303            return self;
304        }
305
306        let current_hz = 1.0 / current_step.to_seconds();
307        let target_hz = 1.0 / target_step.to_seconds();
308
309        // Simple moving average as low-pass filter
310        let window_size = (current_hz / target_hz).round() as usize;
311
312        info!("downsampling tracking data from {current_step} ({current_hz:.6} Hz) to {target_step} ({target_hz:.6} Hz) (N = {window_size})");
313
314        let mut result = TrackingDataArc {
315            source: self.source.clone(),
316            ..Default::default()
317        };
318
319        let measurements: Vec<_> = self.measurements.iter().collect();
320
321        for (i, (epoch, _)) in measurements.iter().enumerate().step_by(window_size) {
322            let start = if i >= window_size / 2 {
323                i - window_size / 2
324            } else {
325                0
326            };
327            let end = (i + window_size / 2 + 1).min(measurements.len());
328            let window = &measurements[start..end];
329
330            let mut filtered_measurement = Measurement {
331                tracker: window[0].1.tracker.clone(),
332                epoch: **epoch,
333                data: IndexMap::new(),
334            };
335
336            // Apply moving average filter for each measurement type
337            for mtype in self.unique_types() {
338                let sum: f64 = window.iter().filter_map(|(_, m)| m.data.get(&mtype)).sum();
339                let count = window
340                    .iter()
341                    .filter(|(_, m)| m.data.contains_key(&mtype))
342                    .count();
343
344                if count > 0 {
345                    filtered_measurement.data.insert(mtype, sum / count as f64);
346                }
347            }
348
349            result.measurements.insert(**epoch, filtered_measurement);
350        }
351        result
352    }
353}
354
355impl fmt::Display for TrackingDataArc {
356    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
357        if self.is_empty() {
358            write!(f, "Empty tracking arc")
359        } else {
360            let start = self.start_epoch().unwrap();
361            let end = self.end_epoch().unwrap();
362            let src = match &self.source {
363                Some(src) => format!(" (source: {src})"),
364                None => String::new(),
365            };
366            write!(
367                f,
368                "Tracking arc with {} measurements of type {:?} over {} (from {start} to {end}) with trackers {:?}{src}",
369                self.len(),
370                self.unique_types(),
371                end - start,
372                self.unique_aliases()
373            )
374        }
375    }
376}
377
378impl fmt::Debug for TrackingDataArc {
379    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
380        write!(f, "{self} @ {self:p}")
381    }
382}
383
384impl PartialEq for TrackingDataArc {
385    fn eq(&self, other: &Self) -> bool {
386        self.measurements == other.measurements
387    }
388}
389
390impl Add for TrackingDataArc {
391    type Output = Self;
392
393    fn add(mut self, rhs: Self) -> Self::Output {
394        self.force_reject = false;
395        for (epoch, msr) in rhs.measurements {
396            if let Entry::Vacant(e) = self.measurements.entry(epoch) {
397                e.insert(msr);
398            } else {
399                error!("merging tracking data with overlapping epoch is not supported");
400            }
401        }
402
403        self
404    }
405}
406
407impl AddAssign for TrackingDataArc {
408    fn add_assign(&mut self, rhs: Self) {
409        *self = self.clone() + rhs;
410    }
411}