nyx_space/od/noise/
gauss_markov.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*/
18
19use crate::io::{ConfigError, ConfigRepr};
20use hifitime::{Duration, Epoch, TimeUnits};
21
22use der::{Decode, Encode, Reader};
23use rand::{Rng, RngExt};
24use rand_distr::Normal;
25use serde::{Deserialize, Serialize};
26use std::fmt;
27use std::ops::{Mul, MulAssign};
28
29#[cfg(feature = "python")]
30use pyo3::prelude::*;
31
32use super::Stochastics;
33
34/// A first order Gauss-Markov process for modeling biases as described in section 5.2.4 of the NASA Best Practices for Navigation Filters (D'Souza et al.).
35///
36/// The process is defined by the following stochastic differential equation:
37///
38/// \dot{b(t)} = -1/τ * b(t) + w(t)
39///
40/// Programmatically, it's calculated by sampling from b(t) ~ 𝓝(0, p_b(t)), where
41///
42/// p_b(t) = exp((-2 / τ) * (t - t_0)) * p_b(t_0) + s(t - t_0)
43///
44/// s(t - t_0) = ((q * τ) / 2) * (1 - exp((-2 / τ) * (t - t_0)))
45///
46/// ## JPL DESCANSO Deep Space Network (DSN) Defaults
47///
48/// - Range: 60 cm process noise over a 60 second average (tau, half life)
49/// - Doppler: 0.03 mm/s process noise over a 60 second average (tau, half life)
50#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)]
51#[cfg_attr(feature = "python", pyclass(from_py_object, get_all, set_all))]
52pub struct GaussMarkov {
53    /// The time constant, tau gives the correlation time, or the time over which the intensity of the time correlation will fade to 1/e of its prior value. (This is sometimes incorrectly referred to as the "half-life" of the process.)
54    pub tau: Duration,
55    pub process_noise: f64,
56    /// An optional constant offset on top of the noise, defaults to zero.
57    pub constant: Option<f64>,
58    /// Epoch of the previous realization, used to compute the time delta for the process noise.
59    #[serde(skip)]
60    pub prev_epoch: Option<Epoch>,
61    /// Sample of previous realization
62    #[serde(skip)]
63    pub init_sample: Option<f64>,
64}
65
66impl fmt::Display for GaussMarkov {
67    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> fmt::Result {
68        write!(
69            f,
70            "First order Gauss-Markov process with τ = {}, σ = {}",
71            self.tau, self.process_noise
72        )
73    }
74}
75
76impl GaussMarkov {
77    /// Create a new first order Gauss-Markov process.
78    /// # Arguments
79    /// * `tau` - The time constant, tau gives the correlation time, or the time over which the intensity of the time correlation will fade to 1/e of its prior value.
80    /// * `process_noise` - process noise of the system.
81    pub fn new(tau: Duration, process_noise: f64) -> Result<Self, ConfigError> {
82        if tau <= Duration::ZERO {
83            return Err(ConfigError::InvalidConfig {
84                msg: format!("tau must be positive but got {tau}"),
85            });
86        }
87
88        Ok(Self {
89            tau,
90            process_noise,
91            constant: None,
92            init_sample: None,
93            prev_epoch: None,
94        })
95    }
96
97    /// Zero noise Gauss-Markov process.
98    pub const ZERO: Self = Self {
99        tau: Duration::MAX,
100        process_noise: 0.0,
101        constant: None,
102        init_sample: None,
103        prev_epoch: None,
104    };
105
106    /// Default Gauss Markov noise of the Deep Space Network, as per DESCANSO Chapter 3, Table 3-3.
107    /// Used the range value of 60 cm over a 60 second average.
108    pub fn default_range_km() -> Self {
109        Self {
110            tau: 1.minutes(),
111            process_noise: 60.0e-5,
112            constant: None,
113            init_sample: None,
114            prev_epoch: None,
115        }
116    }
117
118    /// Default Gauss Markov noise of the Deep Space Network, as per DESCANSO Chapter 3, Table 3-3.
119    /// Used the Doppler value of 0.03 mm/s over a 60 second average.
120    pub fn default_doppler_km_s() -> Self {
121        Self {
122            tau: 1.minutes(),
123            process_noise: 0.03e-6,
124            constant: None,
125            init_sample: None,
126            prev_epoch: None,
127        }
128    }
129}
130
131impl Stochastics for GaussMarkov {
132    fn covariance(&self, _epoch: Epoch) -> f64 {
133        self.process_noise.powi(2)
134    }
135
136    /// Return the next bias sample.
137    fn sample<R: Rng>(&mut self, epoch: Epoch, rng: &mut R) -> f64 {
138        // Compute the delta time in seconds between the previous epoch and the sample epoch.
139        let dt_s = (match self.prev_epoch {
140            None => Duration::ZERO,
141            Some(prev_epoch) => epoch - prev_epoch,
142        })
143        .to_seconds();
144        self.prev_epoch = Some(epoch);
145
146        // If there is no bias, generate one using the standard deviation of the bias
147        if self.init_sample.is_none() {
148            self.init_sample = Some(rng.sample(Normal::new(0.0, self.process_noise).unwrap()));
149        }
150
151        let decay = (-dt_s / self.tau.to_seconds()).exp();
152        let anti_decay = 1.0 - decay;
153
154        // The steady state contribution. This is the bias that the process will converge to as t approaches infinity.
155        let steady_noise = 0.5 * self.process_noise * self.tau.to_seconds() * anti_decay;
156        let ss_sample = rng.sample(Normal::new(0.0, steady_noise).unwrap());
157
158        self.init_sample.unwrap() * decay + ss_sample + self.constant.unwrap_or(0.0)
159    }
160}
161
162impl Mul<f64> for GaussMarkov {
163    type Output = Self;
164
165    /// Scale the Gauss Markov process by a constant, maintaining the same time constant.
166    fn mul(mut self, rhs: f64) -> Self::Output {
167        self.process_noise *= rhs;
168        self.constant = None;
169        self.init_sample = None;
170        self.prev_epoch = None;
171        self
172    }
173}
174
175impl MulAssign<f64> for GaussMarkov {
176    fn mul_assign(&mut self, rhs: f64) {
177        *self = *self * rhs;
178    }
179}
180
181impl Encode for GaussMarkov {
182    fn encoded_len(&self) -> der::Result<der::Length> {
183        self.tau.total_nanoseconds().encoded_len()?
184            + self.process_noise.encoded_len()?
185            + if let Some(constant) = self.constant {
186                (true.encoded_len()? + constant.encoded_len()?)?
187            } else {
188                false.encoded_len()?
189            }
190    }
191
192    fn encode(&self, encoder: &mut impl der::Writer) -> der::Result<()> {
193        self.tau.total_nanoseconds().encode(encoder)?;
194        self.process_noise.encode(encoder)?;
195        if let Some(constant) = self.constant {
196            true.encode(encoder)?;
197            constant.encode(encoder)
198        } else {
199            false.encode(encoder)
200        }
201    }
202}
203
204impl<'a> Decode<'a> for GaussMarkov {
205    fn decode<R: Reader<'a>>(decoder: &mut R) -> der::Result<Self> {
206        let tau = Duration::from_total_nanoseconds(decoder.decode::<i128>()?);
207        let process_noise = decoder.decode()?;
208        let constant = if decoder.decode::<bool>()? {
209            Some(decoder.decode()?)
210        } else {
211            None
212        };
213
214        Ok(Self {
215            tau,
216            process_noise,
217            constant,
218            prev_epoch: None,
219            init_sample: None,
220        })
221    }
222}
223
224impl ConfigRepr for GaussMarkov {}
225
226#[cfg(feature = "python")]
227#[cfg_attr(feature = "python", pymethods)]
228impl GaussMarkov {
229    #[new]
230    fn py_new(tau: Duration, process_noise: f64) -> Result<Self, ConfigError> {
231        Self::new(tau, process_noise)
232    }
233
234    fn __str__(&self) -> String {
235        format!("{self}")
236    }
237
238    fn __repr__(&self) -> String {
239        format!("{self} @ {self:p}")
240    }
241}
242
243#[cfg(test)]
244mod ut_gm {
245
246    use hifitime::{Duration, Epoch, TimeUnits};
247    use rand_pcg::Pcg64Mcg;
248    use rstats::{Stats, triangmat::Vecops};
249
250    use crate::{
251        io::ConfigRepr,
252        od::noise::{GaussMarkov, Stochastics},
253    };
254
255    #[test]
256    fn fogm_test() {
257        let mut gm = GaussMarkov::new(24.hours(), 0.1).unwrap();
258
259        let mut biases = Vec::with_capacity(1000);
260        let epoch = Epoch::now().unwrap();
261
262        let mut rng = Pcg64Mcg::new(0);
263        for seconds in 0..1000 {
264            biases.push(gm.sample(epoch + seconds.seconds(), &mut rng));
265        }
266
267        // Result was inspected visually with the test_gauss_markov.py Python script
268        // I'm not sure how to correctly test this and open to ideas.
269        let min_max = biases.minmax();
270
271        assert_eq!(biases.amean().unwrap(), 0.09373233290645445);
272        assert_eq!(min_max.max, 0.24067114622652647);
273        assert_eq!(min_max.min, -0.045552031890295525);
274    }
275
276    #[test]
277    fn zero_noise_test() {
278        use rstats::{Stats, triangmat::Vecops};
279
280        let mut gm = GaussMarkov::ZERO;
281
282        let mut biases = Vec::with_capacity(1000);
283        let epoch = Epoch::now().unwrap();
284
285        let mut rng = Pcg64Mcg::new(0);
286        for seconds in 0..1000 {
287            biases.push(gm.sample(epoch + seconds.seconds(), &mut rng));
288        }
289
290        let min_max = biases.minmax();
291
292        assert_eq!(biases.amean().unwrap(), 0.0);
293        assert_eq!(min_max.min, 0.0);
294        assert_eq!(min_max.max, 0.0);
295    }
296
297    #[test]
298    fn serde_test() {
299        use serde_yml;
300        use std::env;
301        use std::path::PathBuf;
302
303        // Note that we set the initial bias to zero because it is not serialized.
304        let gm = GaussMarkov::new(Duration::MAX, 0.1).unwrap();
305        let serialized = serde_yml::to_string(&gm).unwrap();
306        println!("{serialized}");
307        let gm_deser: GaussMarkov = serde_yml::from_str(&serialized).unwrap();
308        assert_eq!(gm_deser, gm);
309
310        let test_data: PathBuf = [
311            env!("CARGO_MANIFEST_DIR"),
312            "../data",
313            "03_tests",
314            "config",
315            "high-prec-network.yaml",
316        ]
317        .iter()
318        .collect();
319
320        let models = <GaussMarkov as ConfigRepr>::load_named(test_data).unwrap();
321        assert_eq!(models.len(), 2);
322        assert_eq!(
323            models["range_noise_model"].tau,
324            12.hours() + 159.milliseconds()
325        );
326        assert_eq!(models["range_noise_model"].process_noise, 5.0e-3);
327
328        assert_eq!(models["doppler_noise_model"].tau, 11.hours() + 59.minutes());
329        assert_eq!(models["doppler_noise_model"].process_noise, 50.0e-6);
330    }
331}
nyx_space/od/noise/gauss_markov.rs

nyx_space/od/noise/
gauss_markov.rs
