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