nyx_space/od/estimate/
sc_uncertainty.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 core::fmt;
20
21use anise::errors::MathError;
22use anise::{astro::PhysicsResult, errors::PhysicsError};
23use nalgebra::{SMatrix, SVector};
24use rand::SeedableRng;
25use rand_distr::Distribution;
26use rand_pcg::Pcg64Mcg;
27use typed_builder::TypedBuilder;
28
29use crate::mc::MvnSpacecraft;
30use crate::md::StateParameter;
31use crate::{dynamics::guidance::LocalFrame, Spacecraft};
32
33use super::{Estimate, KfEstimate};
34
35#[derive(Clone, Copy, Debug, TypedBuilder)]
36/// Builds a spacecraft uncertainty in different local frames, dispersing any of the parameters of the spacecraft state.
37///
38/// # Usage
39/// Use the `TypeBuilder` trait, e.g `SpacecraftUncertainty::builder().nominal(spacecraft).frame(LocalFrame::RIC).x_km(0.5).y_km(0.5).z_km(0.5).build()`
40/// to build an uncertainty on position in the RIC frame of 500 meters on R, I, and C, and zero on all other parameters (velocity components, Cr, Cd, mass).
41pub struct SpacecraftUncertainty {
42    pub nominal: Spacecraft,
43    #[builder(default, setter(strip_option))]
44    pub frame: Option<LocalFrame>,
45    #[builder(default = 0.5)]
46    pub x_km: f64,
47    #[builder(default = 0.5)]
48    pub y_km: f64,
49    #[builder(default = 0.5)]
50    pub z_km: f64,
51    #[builder(default = 50e-5)]
52    pub vx_km_s: f64,
53    #[builder(default = 50e-5)]
54    pub vy_km_s: f64,
55    #[builder(default = 50e-5)]
56    pub vz_km_s: f64,
57    #[builder(default)]
58    pub coeff_reflectivity: f64,
59    #[builder(default)]
60    pub coeff_drag: f64,
61    #[builder(default)]
62    pub mass_kg: f64,
63}
64
65impl SpacecraftUncertainty {
66    /// Builds a Kalman filter estimate for a spacecraft state, ready to ingest into an OD Process.
67    ///
68    /// Note: this function will rotate from the provided local frame into the inertial frame with the same central body.
69    pub fn to_estimate(&self) -> PhysicsResult<KfEstimate<Spacecraft>> {
70        if self.x_km < 0.0
71            || self.y_km < 0.0
72            || self.z_km < 0.0
73            || self.vx_km_s < 0.0
74            || self.vy_km_s < 0.0
75            || self.vz_km_s < 0.0
76            || self.coeff_drag < 0.0
77            || self.coeff_reflectivity < 0.0
78            || self.mass_kg < 0.0
79        {
80            return Err(PhysicsError::AppliedMath {
81                source: MathError::DomainError {
82                    value: -0.0,
83                    msg: "uncertainties must be positive ",
84                },
85            });
86        }
87
88        // Build the orbit state vector as provided.
89        let orbit_vec = SVector::<f64, 6>::new(
90            self.x_km,
91            self.y_km,
92            self.z_km,
93            self.vx_km_s,
94            self.vy_km_s,
95            self.vz_km_s,
96        );
97
98        // Rotate into the correct frame.
99        let dcm_local2inertial = match self.frame {
100            None => LocalFrame::Inertial.dcm_to_inertial(self.nominal.orbit)?,
101            Some(frame) => frame.dcm_to_inertial(self.nominal.orbit)?,
102        };
103
104        let mut init_covar =
105            SMatrix::<f64, 9, 9>::from_diagonal(&SVector::<f64, 9>::from_iterator([
106                0.0,
107                0.0,
108                0.0,
109                0.0,
110                0.0,
111                0.0,
112                self.coeff_reflectivity.powi(2),
113                self.coeff_drag.powi(2),
114                self.mass_kg.powi(2),
115            ]));
116
117        let other_cov = SMatrix::<f64, 6, 6>::from_diagonal(&SVector::<f64, 6>::from_iterator([
118            orbit_vec[0].powi(2),
119            orbit_vec[1].powi(2),
120            orbit_vec[2].powi(2),
121            orbit_vec[3].powi(2),
122            orbit_vec[4].powi(2),
123            orbit_vec[5].powi(2),
124        ]));
125
126        let rot_covar =
127            dcm_local2inertial.state_dcm().transpose() * other_cov * dcm_local2inertial.state_dcm();
128
129        for i in 0..6 {
130            for j in 0..6 {
131                init_covar[(i, j)] = rot_covar[(i, j)];
132            }
133        }
134
135        Ok(KfEstimate::from_covar(self.nominal, init_covar))
136    }
137
138    /// Returns an estimation whose nominal state is dispersed.
139    /// Known bug in MultiVariate dispersion: https://github.com/nyx-space/nyx/issues/339
140    pub fn to_estimate_randomized(
141        &self,
142        seed: Option<u128>,
143    ) -> PhysicsResult<KfEstimate<Spacecraft>> {
144        let mut estimate = self.to_estimate()?;
145        let mvn =
146            MvnSpacecraft::from_spacecraft_cov(self.nominal, estimate.covar, SVector::zeros())
147                .expect("covar should be PSD!");
148
149        // Setup the RNG
150        let mut rng = match seed {
151            Some(seed) => Pcg64Mcg::new(seed),
152            None => Pcg64Mcg::from_os_rng(),
153        };
154
155        // Generate the states, forcing the borrow as specified in the `sample_iter` docs.
156        let disp_state = mvn.sample(&mut rng);
157
158        estimate.nominal_state = disp_state.state;
159
160        Ok(estimate)
161    }
162}
163
164impl fmt::Display for SpacecraftUncertainty {
165    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
166        let frame = match self.frame {
167            None => format!("{}", self.nominal.orbit.frame),
168            Some(frame) => match frame {
169                LocalFrame::Inertial => format!("{}", self.nominal.orbit.frame),
170                _ => format!("{frame:?}"),
171            },
172        };
173        writeln!(f, "{}", self.nominal)?;
174        writeln!(
175            f,
176            "{frame}  σ_x = {} km  σ_y = {} km  σ_z = {} km",
177            self.x_km, self.y_km, self.z_km
178        )?;
179        writeln!(
180            f,
181            "{frame}  σ_vx = {} km/s  σ_vy = {} km/s  σ_vz = {} km/s",
182            self.vx_km_s, self.vy_km_s, self.vz_km_s
183        )?;
184        writeln!(
185            f,
186            "σ_cr = {}  σ_cd = {}  σ_mass = {} kg",
187            self.coeff_reflectivity, self.coeff_drag, self.mass_kg
188        )
189    }
190}
191
192impl fmt::LowerHex for KfEstimate<Spacecraft> {
193    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
194        let word = if self.predicted {
195            "Prediction"
196        } else {
197            "Estimate"
198        };
199
200        let params = [
201            StateParameter::SMA,
202            StateParameter::Eccentricity,
203            StateParameter::Inclination,
204            StateParameter::RAAN,
205            StateParameter::AoP,
206            StateParameter::TrueAnomaly,
207        ];
208
209        let mut fmt_cov = Vec::with_capacity(params.len());
210        for (i, param) in params.iter().copied().enumerate() {
211            // The sigma_for call will always work because we define which parameters to compute.
212            let this_val = self
213                .sigma_for(param)
214                .unwrap_or_else(|_| panic!("could not compute sigma for {param}"));
215            if i == 1 {
216                // Eccentricity is shown differently
217                fmt_cov.push(format!("{param}: {this_val:.6e}"));
218            } else {
219                fmt_cov.push(format!("{param}: {this_val:.6}"));
220            }
221        }
222        write!(
223            f,
224            "=== {} @ {} -- within 3 sigma: {} ===\nstate {:x}\nsigmas [{}]\n",
225            word,
226            &self.epoch(),
227            self.within_3sigma(),
228            &self.state(),
229            fmt_cov.join(", ")
230        )
231    }
232}
233
234#[cfg(test)]
235mod ut_sc_uncertainty {
236
237    use super::{Spacecraft, SpacecraftUncertainty};
238    use crate::dynamics::guidance::LocalFrame;
239    use crate::md::StateParameter;
240    use crate::GMAT_EARTH_GM;
241    use anise::constants::frames::EME2000;
242    use anise::prelude::{Epoch, Orbit};
243
244    use rstest::*;
245    #[fixture]
246    fn spacecraft() -> Spacecraft {
247        let eme2k = EME2000.with_mu_km3_s2(GMAT_EARTH_GM);
248
249        Spacecraft::builder()
250            .orbit(Orbit::keplerian(
251                7000.0,
252                0.01,
253                28.5,
254                15.0,
255                55.0,
256                123.0,
257                Epoch::from_gregorian_utc_hms(2024, 2, 29, 1, 2, 3),
258                eme2k,
259            ))
260            .build()
261    }
262
263    #[rstest]
264    fn test_inertial_frame(spacecraft: Spacecraft) {
265        let uncertainty = SpacecraftUncertainty::builder()
266            .nominal(spacecraft)
267            .x_km(0.5)
268            .y_km(0.5)
269            .z_km(0.5)
270            .vx_km_s(0.5e-3)
271            .vy_km_s(0.5e-3)
272            .vz_km_s(0.5e-3)
273            .build();
274
275        assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON);
276        assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON);
277        assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON);
278        assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON);
279        assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON);
280        assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON);
281
282        println!("{uncertainty}");
283
284        let estimate = uncertainty.to_estimate().unwrap();
285
286        // Ensure that the covariance is a diagonal.
287        for i in 0..6 {
288            for j in 0..6 {
289                if i == j {
290                    if i < 3 {
291                        assert!((estimate.covar[(i, j)] - 0.5_f64.powi(2)).abs() < f64::EPSILON);
292                    } else {
293                        assert!((estimate.covar[(i, j)] - 0.5e-3_f64.powi(2)).abs() < f64::EPSILON);
294                    }
295                } else {
296                    assert_eq!(estimate.covar[(i, j)], 0.0);
297                }
298            }
299        }
300    }
301
302    #[rstest]
303    fn test_ric_frame(spacecraft: Spacecraft) {
304        let uncertainty = SpacecraftUncertainty::builder()
305            .nominal(spacecraft)
306            .frame(LocalFrame::RIC)
307            .x_km(0.5)
308            .y_km(0.5)
309            .z_km(0.5)
310            .vx_km_s(0.5e-3)
311            .vy_km_s(0.5e-3)
312            .vz_km_s(0.5e-3)
313            .build();
314
315        assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON);
316        assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON);
317        assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON);
318        assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON);
319        assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON);
320        assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON);
321
322        println!("{uncertainty}");
323
324        let estimate = uncertainty.to_estimate().unwrap();
325
326        println!("{estimate}");
327
328        let sma_sigma_km = estimate.sigma_for(StateParameter::SMA).unwrap();
329        assert!((sma_sigma_km - 1.3528088887049263).abs() < f64::EPSILON);
330
331        let covar_keplerian = estimate.keplerian_covar();
332        for i in 1..=3 {
333            let val = covar_keplerian[(i, i)].sqrt();
334            assert!(val.abs() < 1e-1);
335        }
336        for i in 4..6 {
337            let val = covar_keplerian[(i, i)].sqrt();
338            assert!(val.abs() < 0.8);
339        }
340
341        // Rotate back into the RIC frame.
342        let dcm_ric2inertial = estimate
343            .nominal_state
344            .orbit
345            .dcm_from_ric_to_inertial()
346            .unwrap()
347            .state_dcm();
348
349        // Build the matrix view of the orbit part of the covariance.
350        let orbit_cov = estimate.covar.fixed_view::<6, 6>(0, 0);
351
352        // Rotate back into the RIC frame
353        let ric_covar = dcm_ric2inertial * orbit_cov * dcm_ric2inertial.transpose();
354
355        println!("{ric_covar:.9}");
356        for i in 0..6 {
357            for j in 0..6 {
358                if i == j {
359                    // We don't abs the data in the sqrt to ensure that the diag is positive.
360                    if i < 3 {
361                        assert!((ric_covar[(i, j)].sqrt() - 0.5).abs() < 1e-9);
362                    } else {
363                        assert!((ric_covar[(i, j)].sqrt() - 0.5e-3).abs() < 1e-9);
364                    }
365                }
366            }
367        }
368    }
369}
nyx_space/od/estimate/sc_uncertainty.rs

nyx_space/od/estimate/
sc_uncertainty.rs
