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::cosmic::Frame;
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 SNC3 = SNC<U3>;
28#[allow(clippy::upper_case_acronyms)]
29pub type SNC6 = SNC<U6>;
30
31#[derive(Clone)]
32#[allow(clippy::upper_case_acronyms)]
33pub struct SNC<A: DimName>
34where
35    DefaultAllocator: Allocator<A> + Allocator<A, A>,
36{
37    /// Time at which this SNC starts to become applicable
38    pub start_time: Option<Epoch>,
39    /// Specify the frame of this SNC -- CURRENTLY UNIMPLEMENTED
40    pub frame: Option<Frame>,
41    /// Enables state noise compensation (process noise) only be applied if the time between measurements is less than the disable_time
42    pub disable_time: Duration,
43    // Stores the initial epoch when the SNC is requested, needed for decay. Kalman filter will edit this automatically.
44    pub init_epoch: Option<Epoch>,
45    diag: OVector<f64, A>,
46    decay_diag: Option<Vec<f64>>,
47    // Stores the previous epoch of the SNC request, needed for disable time
48    pub prev_epoch: Option<Epoch>,
49}
50
51impl<A> fmt::Debug for SNC<A>
52where
53    A: DimName,
54    DefaultAllocator: Allocator<A> + Allocator<A, A>,
55{
56    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
57        let mut fmt_cov = Vec::with_capacity(A::dim());
58        if let Some(decay) = &self.decay_diag {
59            for (i, dv) in decay.iter().enumerate() {
60                fmt_cov.push(format!("{:.1e} × exp(- {:.1e} × t)", self.diag[i], dv));
61            }
62        } else {
63            for i in 0..A::dim() {
64                fmt_cov.push(format!("{:.1e}", self.diag[i]));
65            }
66        }
67        write!(
68            f,
69            "SNC: diag({}) {}",
70            fmt_cov.join(", "),
71            if let Some(start) = self.start_time {
72                format!("starting at {start}")
73            } else {
74                "".to_string()
75            }
76        )
77    }
78}
79
80impl<A> fmt::Display for SNC<A>
81where
82    A: DimName,
83    DefaultAllocator: Allocator<A> + Allocator<A, A>,
84{
85    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
86        write!(f, "{self:?}")
87    }
88}
89
90impl<A: DimName> SNC<A>
91where
92    DefaultAllocator: Allocator<A> + Allocator<A, A>,
93{
94    /// Initialize a state noise compensation structure from the diagonal values
95    pub fn from_diagonal(disable_time: Duration, values: &[f64]) -> Self {
96        assert_eq!(
97            values.len(),
98            A::dim(),
99            "Not enough values for the size of the SNC matrix"
100        );
101
102        let mut diag = OVector::zeros();
103        for (i, v) in values.iter().enumerate() {
104            diag[i] = *v;
105        }
106
107        Self {
108            diag,
109            disable_time,
110            start_time: None,
111            frame: None,
112            decay_diag: None,
113            init_epoch: None,
114            prev_epoch: None,
115        }
116    }
117
118    /// Initialize an SNC with a time at which it should start
119    pub fn with_start_time(disable_time: Duration, values: &[f64], start_time: Epoch) -> Self {
120        let mut me = Self::from_diagonal(disable_time, values);
121        me.start_time = Some(start_time);
122        me
123    }
124
125    /// Initialize an exponentially decaying SNC with initial SNC and decay constants.
126    /// Decay constants in seconds since start of the tracking pass.
127    pub fn with_decay(
128        disable_time: Duration,
129        initial_snc: &[f64],
130        decay_constants_s: &[f64],
131    ) -> Self {
132        assert_eq!(
133            decay_constants_s.len(),
134            A::dim(),
135            "Not enough decay constants for the size of the SNC matrix"
136        );
137
138        let mut me = Self::from_diagonal(disable_time, initial_snc);
139        me.decay_diag = Some(decay_constants_s.to_vec());
140        me
141    }
142
143    /// Returns the SNC matrix (_not_ incl. Gamma matrix approximation) at the provided Epoch.
144    /// May be None if:
145    ///  1. Start time of this matrix is _after_ epoch
146    ///  2. Time between epoch and previous epoch (set in the Kalman filter!) is longer than disabling time
147    pub fn to_matrix(&self, epoch: Epoch) -> Option<OMatrix<f64, A, A>> {
148        if let Some(start_time) = self.start_time {
149            if start_time > epoch {
150                // This SNC applies only later
151                debug!("@{} SNC starts at {}", epoch, start_time);
152                return None;
153            }
154        }
155
156        // Check the disable time, and return no SNC if the previous SNC was computed too long ago
157        if let Some(prev_epoch) = self.prev_epoch {
158            if epoch - prev_epoch > self.disable_time {
159                debug!(
160                    "@{} SNC disabled: prior use greater than {}",
161                    epoch, self.disable_time
162                );
163                return None;
164            }
165        }
166        // Build a static matrix
167        let mut snc = OMatrix::<f64, A, A>::zeros();
168        for i in 0..self.diag.nrows() {
169            snc[(i, i)] = self.diag[i];
170        }
171
172        if let Some(decay) = &self.decay_diag {
173            // Let's apply the decay to the diagonals
174            let total_delta_t = (epoch - self.init_epoch.unwrap()).to_seconds();
175            for i in 0..self.diag.nrows() {
176                snc[(i, i)] *= (-decay[i] * total_delta_t).exp();
177            }
178        }
179
180        debug!(
181            "@{} SNC diag {:?}",
182            epoch,
183            snc.diagonal().iter().copied().collect::<Vec<f64>>()
184        );
185
186        Some(snc)
187    }
188}
189
190#[test]
191fn test_snc_init() {
192    use crate::time::Unit;
193    let snc_expo = SNC3::with_decay(
194        2 * Unit::Minute,
195        &[1e-6, 1e-6, 1e-6],
196        &[3600.0, 3600.0, 3600.0],
197    );
198    println!("{}", snc_expo);
199
200    let snc_std = SNC3::with_start_time(
201        2 * Unit::Minute,
202        &[1e-6, 1e-6, 1e-6],
203        Epoch::from_et_seconds(3600.0),
204    );
205    println!("{}", snc_std);
206}
nyx_space/od/snc.rs

nyx_space/od/
snc.rs
