nyx_space/od/
snc.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::dynamics::guidance::LocalFrame;
20use crate::linalg::allocator::Allocator;
21use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector, U3, U6};
22use crate::time::{Duration, Epoch};
23
24use std::fmt;
25
26#[allow(clippy::upper_case_acronyms)]
27pub type ProcessNoise3D = ProcessNoise<U3>;
28
29#[allow(clippy::upper_case_acronyms)]
30pub type ProcessNoise6D = ProcessNoise<U6>;
31
32#[deprecated = "SNC has been renamed to ProcessNoise"]
33#[allow(clippy::upper_case_acronyms)]
34pub type SNC<A> = ProcessNoise<A>;
35#[deprecated = "SNC has been renamed to ProcessNoise"]
36#[allow(clippy::upper_case_acronyms)]
37pub type SNC3 = ProcessNoise<U3>;
38#[deprecated = "SNC has been renamed to ProcessNoise"]
39#[allow(clippy::upper_case_acronyms)]
40pub type SNC6 = ProcessNoise<U6>;
41
42#[derive(Clone)]
43#[allow(clippy::upper_case_acronyms)]
44pub struct ProcessNoise<A: DimName>
45where
46    DefaultAllocator: Allocator<A> + Allocator<A, A>,
47{
48    /// Time at which this SNC starts to become applicable
49    pub start_time: Option<Epoch>,
50    /// Specify the local frame of this SNC
51    pub local_frame: Option<LocalFrame>,
52    /// Enables state noise compensation (process noise) only be applied if the time between measurements is less than the disable_time
53    pub disable_time: Duration,
54    // Stores the initial epoch when the SNC is requested, needed for decay. Kalman filter will edit this automatically.
55    pub init_epoch: Option<Epoch>,
56    diag: OVector<f64, A>,
57    decay_diag: Option<Vec<f64>>,
58    // Stores the previous epoch of the SNC request, needed for disable time
59    pub prev_epoch: Option<Epoch>,
60}
61
62impl<A> fmt::Debug for ProcessNoise<A>
63where
64    A: DimName,
65    DefaultAllocator: Allocator<A> + Allocator<A, A>,
66{
67    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
68        let mut fmt_cov = Vec::with_capacity(A::dim());
69        if let Some(decay) = &self.decay_diag {
70            for (i, dv) in decay.iter().enumerate() {
71                fmt_cov.push(format!(
72                    "{:.1e} × exp(- {:.1e} × t)",
73                    self.diag[i] * 1e6,
74                    dv
75                ));
76            }
77        } else {
78            for i in 0..A::dim() {
79                fmt_cov.push(format!("{:.1e}", self.diag[i] * 1e6));
80            }
81        }
82        write!(
83            f,
84            "SNC: diag({}) mm/s^2 {} {}",
85            fmt_cov.join(", "),
86            if let Some(lf) = self.local_frame {
87                format!("in {lf:?}")
88            } else {
89                "".to_string()
90            },
91            if let Some(start) = self.start_time {
92                format!("starting at {start}")
93            } else {
94                "".to_string()
95            }
96        )
97    }
98}
99
100impl<A> fmt::Display for ProcessNoise<A>
101where
102    A: DimName,
103    DefaultAllocator: Allocator<A> + Allocator<A, A>,
104{
105    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
106        write!(f, "{self:?}")
107    }
108}
109
110impl<A: DimName> ProcessNoise<A>
111where
112    DefaultAllocator: Allocator<A> + Allocator<A, A>,
113{
114    /// Initialize a state noise compensation structure from the diagonal values
115    pub fn from_diagonal(disable_time: Duration, values: &[f64]) -> Self {
116        assert_eq!(
117            values.len(),
118            A::dim(),
119            "Not enough values for the size of the SNC matrix"
120        );
121
122        let mut diag = OVector::zeros();
123        for (i, v) in values.iter().enumerate() {
124            diag[i] = *v;
125        }
126
127        Self {
128            diag,
129            disable_time,
130            start_time: None,
131            local_frame: None,
132            decay_diag: None,
133            init_epoch: None,
134            prev_epoch: None,
135        }
136    }
137
138    /// Initialize an SNC with a time at which it should start
139    pub fn with_start_time(disable_time: Duration, values: &[f64], start_time: Epoch) -> Self {
140        let mut me = Self::from_diagonal(disable_time, values);
141        me.start_time = Some(start_time);
142        me
143    }
144
145    /// Initialize an exponentially decaying SNC with initial SNC and decay constants.
146    /// Decay constants in seconds since start of the tracking pass.
147    pub fn with_decay(
148        disable_time: Duration,
149        initial_snc: &[f64],
150        decay_constants_s: &[f64],
151    ) -> Self {
152        assert_eq!(
153            decay_constants_s.len(),
154            A::dim(),
155            "Not enough decay constants for the size of the SNC matrix"
156        );
157
158        let mut me = Self::from_diagonal(disable_time, initial_snc);
159        me.decay_diag = Some(decay_constants_s.to_vec());
160        me
161    }
162
163    /// Returns the SNC matrix (_not_ incl. Gamma matrix approximation) at the provided Epoch.
164    /// May be None if:
165    ///  1. Start time of this matrix is _after_ epoch
166    ///  2. Time between epoch and previous epoch (set in the Kalman filter!) is longer than disabling time
167    pub fn to_matrix(&self, epoch: Epoch) -> Option<OMatrix<f64, A, A>> {
168        if let Some(start_time) = self.start_time {
169            if start_time > epoch {
170                // This SNC applies only later
171                debug!("@{} SNC starts at {}", epoch, start_time);
172                return None;
173            }
174        }
175
176        // Check the disable time, and return no SNC if the previous SNC was computed too long ago
177        if let Some(prev_epoch) = self.prev_epoch {
178            if epoch - prev_epoch > self.disable_time {
179                debug!(
180                    "@{} SNC disabled: prior use greater than {}",
181                    epoch, self.disable_time
182                );
183                return None;
184            }
185        }
186        // Build a static matrix
187        let mut snc = OMatrix::<f64, A, A>::zeros();
188        for i in 0..self.diag.nrows() {
189            snc[(i, i)] = self.diag[i];
190        }
191
192        if let Some(decay) = &self.decay_diag {
193            // Let's apply the decay to the diagonals
194            let total_delta_t = (epoch - self.init_epoch.unwrap()).to_seconds();
195            for i in 0..self.diag.nrows() {
196                snc[(i, i)] *= (-decay[i] * total_delta_t).exp();
197            }
198        }
199
200        debug!(
201            "@{} SNC diag {:?}",
202            epoch,
203            snc.diagonal().iter().copied().collect::<Vec<f64>>()
204        );
205
206        Some(snc)
207    }
208}
209
210impl ProcessNoise3D {
211    /// Initialize the process noise from velocity errors over time
212    pub fn from_velocity_km_s(
213        velocity_noise: &[f64; 3],
214        noise_duration: Duration,
215        disable_time: Duration,
216        local_frame: Option<LocalFrame>,
217    ) -> Self {
218        let mut diag = OVector::<f64, U3>::zeros();
219        for (i, v) in velocity_noise.iter().enumerate() {
220            diag[i] = *v / noise_duration.to_seconds();
221        }
222
223        Self {
224            diag,
225            disable_time,
226            start_time: None,
227            local_frame,
228            decay_diag: None,
229            init_epoch: None,
230            prev_epoch: None,
231        }
232    }
233}
234
235#[test]
236fn test_snc_init() {
237    use crate::time::Unit;
238    let snc_expo = ProcessNoise3D::with_decay(
239        2 * Unit::Minute,
240        &[1e-6, 1e-6, 1e-6],
241        &[3600.0, 3600.0, 3600.0],
242    );
243    println!("{}", snc_expo);
244
245    let snc_std = ProcessNoise3D::with_start_time(
246        2 * Unit::Minute,
247        &[1e-6, 1e-6, 1e-6],
248        Epoch::from_et_seconds(3600.0),
249    );
250    println!("{}", snc_std);
251
252    let snc_vel = ProcessNoise3D::from_velocity_km_s(
253        &[1e-2, 1e-2, 1e-2],
254        Unit::Hour * 2,
255        Unit::Minute * 2,
256        Some(LocalFrame::RIC),
257    );
258
259    let diag_val = 1e-2 / (Unit::Hour * 2).to_seconds();
260    assert_eq!(
261        snc_vel
262            .to_matrix(Epoch::from_et_seconds(3600.0))
263            .unwrap()
264            .diagonal(),
265        OVector::<f64, U3>::new(diag_val, diag_val, diag_val)
266    );
267    assert_eq!(snc_vel.local_frame, Some(LocalFrame::RIC));
268}
nyx_space/od/snc.rs

nyx_space/od/
snc.rs
