1use 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)]
31pub 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 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 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 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],
114 orbit_vec[1],
115 orbit_vec[2],
116 orbit_vec[3],
117 orbit_vec[4],
118 orbit_vec[5],
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)].powi(2);
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 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.352808889337306).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 let dcm_ric2inertial = estimate
271 .nominal_state
272 .orbit
273 .dcm_from_ric_to_inertial()
274 .unwrap()
275 .state_dcm();
276
277 let orbit_cov = estimate.covar.fixed_view::<6, 6>(0, 0);
279
280 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 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}