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 typed_builder::TypedBuilder;
25
26use crate::{dynamics::guidance::LocalFrame, Spacecraft};
27
28use super::KfEstimate;
29
30#[derive(Clone, Copy, Debug, TypedBuilder)]
31/// Builds a spacecraft uncertainty in different local frames, dispersing any of the parameters of the spacecraft state.
32///
33/// # Usage
34/// 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()`
35/// 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).
36pub struct SpacecraftUncertainty {
37    pub nominal: Spacecraft,
38    #[builder(default, setter(strip_option))]
39    pub frame: Option<LocalFrame>,
40    #[builder(default = 0.5)]
41    pub x_km: f64,
42    #[builder(default = 0.5)]
43    pub y_km: f64,
44    #[builder(default = 0.5)]
45    pub z_km: f64,
46    #[builder(default = 50e-5)]
47    pub vx_km_s: f64,
48    #[builder(default = 50e-5)]
49    pub vy_km_s: f64,
50    #[builder(default = 50e-5)]
51    pub vz_km_s: f64,
52    #[builder(default)]
53    pub coeff_reflectivity: f64,
54    #[builder(default)]
55    pub coeff_drag: f64,
56    #[builder(default)]
57    pub mass_kg: f64,
58}
59
60impl SpacecraftUncertainty {
61    /// Builds a Kalman filter estimate for a spacecraft state, ready to ingest into an OD Process.
62    ///
63    /// Note: this function will rotate from the provided local frame into the inertial frame with the same central body.
64    pub fn to_estimate(&self) -> PhysicsResult<KfEstimate<Spacecraft>> {
65        if self.x_km < 0.0
66            || self.y_km < 0.0
67            || self.z_km < 0.0
68            || self.vx_km_s < 0.0
69            || self.vy_km_s < 0.0
70            || self.vz_km_s < 0.0
71            || self.coeff_drag < 0.0
72            || self.coeff_reflectivity < 0.0
73            || self.mass_kg < 0.0
74        {
75            return Err(PhysicsError::AppliedMath {
76                source: MathError::DomainError {
77                    value: -0.0,
78                    msg: "uncertainties must be positive ",
79                },
80            });
81        }
82
83        // Build the orbit state vector as provided.
84        let orbit_vec = SVector::<f64, 6>::new(
85            self.x_km,
86            self.y_km,
87            self.z_km,
88            self.vx_km_s,
89            self.vy_km_s,
90            self.vz_km_s,
91        );
92
93        // Rotate into the correct frame.
94        let dcm_local2inertial = match self.frame {
95            None => LocalFrame::Inertial.dcm_to_inertial(self.nominal.orbit)?,
96            Some(frame) => frame.dcm_to_inertial(self.nominal.orbit)?,
97        };
98
99        let mut init_covar =
100            SMatrix::<f64, 9, 9>::from_diagonal(&SVector::<f64, 9>::from_iterator([
101                0.0,
102                0.0,
103                0.0,
104                0.0,
105                0.0,
106                0.0,
107                self.coeff_reflectivity.powi(2),
108                self.coeff_drag.powi(2),
109                self.mass_kg.powi(2),
110            ]));
111
112        let other_cov = SMatrix::<f64, 6, 6>::from_diagonal(&SVector::<f64, 6>::from_iterator([
113            orbit_vec[0].powi(2),
114            orbit_vec[1].powi(2),
115            orbit_vec[2].powi(2),
116            orbit_vec[3].powi(2),
117            orbit_vec[4].powi(2),
118            orbit_vec[5].powi(2),
119        ]));
120
121        let rot_covar =
122            dcm_local2inertial.state_dcm().transpose() * other_cov * dcm_local2inertial.state_dcm();
123
124        for i in 0..6 {
125            for j in 0..6 {
126                init_covar[(i, j)] = rot_covar[(i, j)];
127            }
128        }
129
130        Ok(KfEstimate::from_covar(self.nominal, init_covar))
131    }
132}
133
134impl fmt::Display for SpacecraftUncertainty {
135    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
136        let frame = match self.frame {
137            None => format!("{}", self.nominal.orbit.frame),
138            Some(frame) => match frame {
139                LocalFrame::Inertial => format!("{}", self.nominal.orbit.frame),
140                _ => format!("{frame:?}"),
141            },
142        };
143        writeln!(f, "{}", self.nominal)?;
144        writeln!(
145            f,
146            "{frame}  σ_x = {} km  σ_y = {} km  σ_z = {} km",
147            self.x_km, self.y_km, self.z_km
148        )?;
149        writeln!(
150            f,
151            "{frame}  σ_vx = {} km/s  σ_vy = {} km/s  σ_vz = {} km/s",
152            self.vx_km_s, self.vy_km_s, self.vz_km_s
153        )?;
154        writeln!(
155            f,
156            "σ_cr = {}  σ_cd = {}  σ_mass = {} kg",
157            self.coeff_reflectivity, self.coeff_drag, self.mass_kg
158        )
159    }
160}
161
162#[cfg(test)]
163mod ut_sc_uncertainty {
164
165    use super::{Spacecraft, SpacecraftUncertainty};
166    use crate::dynamics::guidance::LocalFrame;
167    use crate::md::StateParameter;
168    use crate::GMAT_EARTH_GM;
169    use anise::constants::frames::EME2000;
170    use anise::prelude::{Epoch, Orbit};
171
172    use rstest::*;
173    #[fixture]
174    fn spacecraft() -> Spacecraft {
175        let eme2k = EME2000.with_mu_km3_s2(GMAT_EARTH_GM);
176
177        Spacecraft::builder()
178            .orbit(Orbit::keplerian(
179                7000.0,
180                0.01,
181                28.5,
182                15.0,
183                55.0,
184                123.0,
185                Epoch::from_gregorian_utc_hms(2024, 2, 29, 1, 2, 3),
186                eme2k,
187            ))
188            .build()
189    }
190
191    #[rstest]
192    fn test_inertial_frame(spacecraft: Spacecraft) {
193        let uncertainty = SpacecraftUncertainty::builder()
194            .nominal(spacecraft)
195            .x_km(0.5)
196            .y_km(0.5)
197            .z_km(0.5)
198            .vx_km_s(0.5e-3)
199            .vy_km_s(0.5e-3)
200            .vz_km_s(0.5e-3)
201            .build();
202
203        assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON);
204        assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON);
205        assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON);
206        assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON);
207        assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON);
208        assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON);
209
210        println!("{uncertainty}");
211
212        let estimate = uncertainty.to_estimate().unwrap();
213
214        // Ensure that the covariance is a diagonal.
215        for i in 0..6 {
216            for j in 0..6 {
217                if i == j {
218                    if i < 3 {
219                        assert!((estimate.covar[(i, j)] - 0.5_f64.powi(2)).abs() < f64::EPSILON);
220                    } else {
221                        assert!((estimate.covar[(i, j)] - 0.5e-3_f64.powi(2)).abs() < f64::EPSILON);
222                    }
223                } else {
224                    assert_eq!(estimate.covar[(i, j)], 0.0);
225                }
226            }
227        }
228    }
229
230    #[rstest]
231    fn test_ric_frame(spacecraft: Spacecraft) {
232        let uncertainty = SpacecraftUncertainty::builder()
233            .nominal(spacecraft)
234            .frame(LocalFrame::RIC)
235            .x_km(0.5)
236            .y_km(0.5)
237            .z_km(0.5)
238            .vx_km_s(0.5e-3)
239            .vy_km_s(0.5e-3)
240            .vz_km_s(0.5e-3)
241            .build();
242
243        assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON);
244        assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON);
245        assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON);
246        assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON);
247        assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON);
248        assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON);
249
250        println!("{uncertainty}");
251
252        let estimate = uncertainty.to_estimate().unwrap();
253
254        println!("{estimate}");
255
256        let sma_sigma_km = estimate.sigma_for(StateParameter::SMA).unwrap();
257        assert!((sma_sigma_km - 1.3528088887049263).abs() < f64::EPSILON);
258
259        let covar_keplerian = estimate.keplerian_covar();
260        for i in 1..=3 {
261            let val = covar_keplerian[(i, i)].sqrt();
262            assert!(val.abs() < 1e-1);
263        }
264        for i in 4..6 {
265            let val = covar_keplerian[(i, i)].sqrt();
266            assert!(val.abs() < 0.8);
267        }
268
269        // Rotate back into the RIC frame.
270        let dcm_ric2inertial = estimate
271            .nominal_state
272            .orbit
273            .dcm_from_ric_to_inertial()
274            .unwrap()
275            .state_dcm();
276
277        // Build the matrix view of the orbit part of the covariance.
278        let orbit_cov = estimate.covar.fixed_view::<6, 6>(0, 0);
279
280        // Rotate back into the RIC frame
281        let ric_covar = dcm_ric2inertial * orbit_cov * dcm_ric2inertial.transpose();
282
283        println!("{:.9}", ric_covar);
284        for i in 0..6 {
285            for j in 0..6 {
286                if i == j {
287                    // We don't abs the data in the sqrt to ensure that the diag is positive.
288                    if i < 3 {
289                        assert!((ric_covar[(i, j)].sqrt() - 0.5).abs() < 1e-9);
290                    } else {
291                        assert!((ric_covar[(i, j)].sqrt() - 0.5e-3).abs() < 1e-9);
292                    }
293                }
294            }
295        }
296    }
297}
nyx_space/od/estimate/sc_uncertainty.rs

nyx_space/od/estimate/
sc_uncertainty.rs
