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

nyx_space/od/noise/
gauss_markov.rs
