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 rand::Rng;
23use rand_distr::Normal;
24use serde_derive::{Deserialize, Serialize};
25use std::fmt;
26use std::ops::{Mul, MulAssign};
27
28use super::Stochastics;
29
30/// 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.).
31///
32/// The process is defined by the following stochastic differential equation:
33///
34/// \dot{b(t)} = -1/τ * b(t) + w(t)
35///
36/// Programmatically, it's calculated by sampling from b(t) ~ 𝓝(0, p_b(t)), where
37///
38/// p_b(t) = exp((-2 / τ) * (t - t_0)) * p_b(t_0) + s(t - t_0)
39///
40/// s(t - t_0) = ((q * τ) / 2) * (1 - exp((-2 / τ) * (t - t_0)))
41#[derive(Copy, Clone, Debug, Serialize, Deserialize, PartialEq)]
42pub struct GaussMarkov {
43    /// 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.)
44    pub tau: Duration,
45    pub process_noise: f64,
46    /// An optional constant offset on top of the noise, defaults to zero.
47    pub constant: Option<f64>,
48    /// Epoch of the previous realization, used to compute the time delta for the process noise.
49    #[serde(skip)]
50    pub prev_epoch: Option<Epoch>,
51    /// Sample of previous realization
52    #[serde(skip)]
53    pub init_sample: Option<f64>,
54}
55
56impl fmt::Display for GaussMarkov {
57    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> fmt::Result {
58        write!(
59            f,
60            "First order Gauss-Markov process with τ = {}, σ = {}",
61            self.tau, self.process_noise
62        )
63    }
64}
65
66impl GaussMarkov {
67    /// Create a new first order Gauss-Markov process.
68    /// # Arguments
69    /// * `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.
70    /// * `process_noise` - process noise of the system.
71    pub fn new(tau: Duration, process_noise: f64) -> Result<Self, ConfigError> {
72        if tau <= Duration::ZERO {
73            return Err(ConfigError::InvalidConfig {
74                msg: format!("tau must be positive but got {tau}"),
75            });
76        }
77
78        Ok(Self {
79            tau,
80            process_noise,
81            constant: None,
82            init_sample: None,
83            prev_epoch: None,
84        })
85    }
86
87    /// Zero noise Gauss-Markov process.
88    pub const ZERO: Self = Self {
89        tau: Duration::MAX,
90        process_noise: 0.0,
91        constant: None,
92        init_sample: None,
93        prev_epoch: None,
94    };
95
96    /// Default Gauss Markov noise of the Deep Space Network, as per DESCANSO Chapter 3, Table 3-3.
97    /// Used the range value of 60 cm over a 60 second average.
98    pub fn default_range_km() -> Self {
99        Self {
100            tau: 1.minutes(),
101            process_noise: 60.0e-5,
102            constant: None,
103            init_sample: None,
104            prev_epoch: None,
105        }
106    }
107
108    /// Default Gauss Markov noise of the Deep Space Network, as per DESCANSO Chapter 3, Table 3-3.
109    /// Used the Doppler value of 0.03 mm/s over a 60 second average.
110    pub fn default_doppler_km_s() -> Self {
111        Self {
112            tau: 1.minutes(),
113            process_noise: 0.03e-6,
114            constant: None,
115            init_sample: None,
116            prev_epoch: None,
117        }
118    }
119}
120
121impl Stochastics for GaussMarkov {
122    fn covariance(&self, _epoch: Epoch) -> f64 {
123        self.process_noise.powi(2)
124    }
125
126    /// Return the next bias sample.
127    fn sample<R: Rng>(&mut self, epoch: Epoch, rng: &mut R) -> f64 {
128        // Compute the delta time in seconds between the previous epoch and the sample epoch.
129        let dt_s = (match self.prev_epoch {
130            None => Duration::ZERO,
131            Some(prev_epoch) => epoch - prev_epoch,
132        })
133        .to_seconds();
134        self.prev_epoch = Some(epoch);
135
136        // If there is no bias, generate one using the standard deviation of the bias
137        if self.init_sample.is_none() {
138            self.init_sample = Some(rng.sample(Normal::new(0.0, self.process_noise).unwrap()));
139        }
140
141        let decay = (-dt_s / self.tau.to_seconds()).exp();
142        let anti_decay = 1.0 - decay;
143
144        // The steady state contribution. This is the bias that the process will converge to as t approaches infinity.
145        let steady_noise = 0.5 * self.process_noise * self.tau.to_seconds() * anti_decay;
146        let ss_sample = rng.sample(Normal::new(0.0, steady_noise).unwrap());
147
148        self.init_sample.unwrap() * decay + ss_sample + self.constant.unwrap_or(0.0)
149    }
150}
151
152impl Mul<f64> for GaussMarkov {
153    type Output = Self;
154
155    /// Scale the Gauss Markov process by a constant, maintaining the same time constant.
156    fn mul(mut self, rhs: f64) -> Self::Output {
157        self.process_noise *= rhs;
158        self.constant = None;
159        self.init_sample = None;
160        self.prev_epoch = None;
161        self
162    }
163}
164
165impl MulAssign<f64> for GaussMarkov {
166    fn mul_assign(&mut self, rhs: f64) {
167        *self = *self * rhs;
168    }
169}
170
171impl ConfigRepr for GaussMarkov {}
172
173#[cfg(test)]
174mod ut_gm {
175
176    use hifitime::{Duration, Epoch, TimeUnits};
177    use rand_pcg::Pcg64Mcg;
178    use rstats::{triangmat::Vecops, Stats};
179
180    use crate::{
181        io::ConfigRepr,
182        od::noise::{GaussMarkov, Stochastics},
183    };
184
185    #[test]
186    fn fogm_test() {
187        let mut gm = GaussMarkov::new(24.hours(), 0.1).unwrap();
188
189        let mut biases = Vec::with_capacity(1000);
190        let epoch = Epoch::now().unwrap();
191
192        let mut rng = Pcg64Mcg::new(0);
193        for seconds in 0..1000 {
194            biases.push(gm.sample(epoch + seconds.seconds(), &mut rng));
195        }
196
197        // Result was inspected visually with the test_gauss_markov.py Python script
198        // I'm not sure how to correctly test this and open to ideas.
199        let min_max = biases.minmax();
200
201        assert_eq!(biases.amean().unwrap(), 0.09373233290645445);
202        assert_eq!(min_max.max, 0.24067114622652647);
203        assert_eq!(min_max.min, -0.045552031890295525);
204    }
205
206    #[test]
207    fn zero_noise_test() {
208        use rstats::{triangmat::Vecops, Stats};
209
210        let mut gm = GaussMarkov::ZERO;
211
212        let mut biases = Vec::with_capacity(1000);
213        let epoch = Epoch::now().unwrap();
214
215        let mut rng = Pcg64Mcg::new(0);
216        for seconds in 0..1000 {
217            biases.push(gm.sample(epoch + seconds.seconds(), &mut rng));
218        }
219
220        let min_max = biases.minmax();
221
222        assert_eq!(biases.amean().unwrap(), 0.0);
223        assert_eq!(min_max.min, 0.0);
224        assert_eq!(min_max.max, 0.0);
225    }
226
227    #[test]
228    fn serde_test() {
229        use serde_yml;
230        use std::env;
231        use std::path::PathBuf;
232
233        // Note that we set the initial bias to zero because it is not serialized.
234        let gm = GaussMarkov::new(Duration::MAX, 0.1).unwrap();
235        let serialized = serde_yml::to_string(&gm).unwrap();
236        println!("{serialized}");
237        let gm_deser: GaussMarkov = serde_yml::from_str(&serialized).unwrap();
238        assert_eq!(gm_deser, gm);
239
240        let test_data: PathBuf = [
241            env::var("CARGO_MANIFEST_DIR").unwrap(),
242            "data".to_string(),
243            "tests".to_string(),
244            "config".to_string(),
245            "high-prec-network.yaml".to_string(),
246        ]
247        .iter()
248        .collect();
249
250        let models = <GaussMarkov as ConfigRepr>::load_named(test_data).unwrap();
251        assert_eq!(models.len(), 2);
252        assert_eq!(
253            models["range_noise_model"].tau,
254            12.hours() + 159.milliseconds()
255        );
256        assert_eq!(models["range_noise_model"].process_noise, 5.0e-3);
257
258        assert_eq!(models["doppler_noise_model"].tau, 11.hours() + 59.minutes());
259        assert_eq!(models["doppler_noise_model"].process_noise, 50.0e-6);
260    }
261}
nyx_space/od/noise/gauss_markov.rs

nyx_space/od/noise/
gauss_markov.rs
