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

nyx_space/od/estimate/
sc_uncertainty.rs
