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

nyx_space/od/noise/
gauss_markov.rs
