Merge pull request #350 from nyx-space/94-support-estimation-of-spacecraft-parameters-cr-cd

Support estimation of spacecraft coefficient of reflectivity and drag

Author: ChristopherRabotin

Diff for: src/cosmic/orbitdual.rs

@@ -303,7 +303,7 @@ impl OrbitDual {
         .cross(&self.hvec());
         let aop = (n.dot(&self.evec()?) / (norm(&n) * self.ecc()?.dual)).acos();
         if aop.is_nan() {
-            error!("AoP is NaN");
+            warn!("AoP is NaN");
             Ok(OrbitPartial {
                 dual: OHyperdual::from(0.0),
                 param: StateParameter::AoP,

Diff for: src/cosmic/spacecraft.rs

@@ -497,7 +497,7 @@ impl State for Spacecraft {
         self.orbit.epoch = epoch;
         self.orbit.radius_km = radius_km;
         self.orbit.velocity_km_s = vel_km_s;
-        self.srp.cr = sc_state[6];
+        self.srp.cr = sc_state[6].clamp(0.0, 2.0);
         self.drag.cd = sc_state[7];
         self.fuel_mass_kg = sc_state[8];
     }
@@ -788,7 +788,7 @@ impl Add<OVector<f64, Const<9>>> for Spacecraft {
 
         self.orbit.radius_km += radius_km;
         self.orbit.velocity_km_s += vel_km_s;
-        self.srp.cr += other[6];
+        self.srp.cr = (self.srp.cr + other[6]).clamp(0.0, 2.0);
         self.drag.cd += other[7];
         self.fuel_mass_kg += other[8];
 

Diff for: src/dynamics/drag.rs

@@ -24,7 +24,7 @@ use super::{
     DynamicsAlmanacSnafu, DynamicsAstroSnafu, DynamicsError, DynamicsPlanetarySnafu, ForceModel,
 };
 use crate::cosmic::{AstroError, AstroPhysicsSnafu, Frame, Spacecraft};
-use crate::linalg::{Matrix3, Vector3};
+use crate::linalg::{Matrix4x3, Vector3};
 use std::fmt;
 use std::sync::Arc;
 
@@ -47,6 +47,8 @@ pub struct ConstantDrag {
     pub rho: f64,
     /// Geoid causing the drag
     pub drag_frame: Frame,
+    /// Set to true to estimate the coefficient of drag
+    pub estimate: bool,
 }
 
 impl fmt::Display for ConstantDrag {
@@ -60,6 +62,14 @@ impl fmt::Display for ConstantDrag {
 }
 
 impl ForceModel for ConstantDrag {
+    fn estimation_index(&self) -> Option<usize> {
+        if self.estimate {
+            Some(7)
+        } else {
+            None
+        }
+    }
+
     fn eom(&self, ctx: &Spacecraft, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError> {
         let osc = almanac
             .transform_to(ctx.orbit, self.drag_frame, None)
@@ -76,7 +86,7 @@ impl ForceModel for ConstantDrag {
         &self,
         _osc_ctx: &Spacecraft,
         _almanac: Arc<Almanac>,
-    ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
+    ) -> Result<(Vector3<f64>, Matrix4x3<f64>), DynamicsError> {
         Err(DynamicsError::DynamicsAstro {
             source: AstroError::PartialsUndefined,
         })
@@ -90,6 +100,8 @@ pub struct Drag {
     pub density: AtmDensity,
     /// Frame to compute the drag in
     pub drag_frame: Frame,
+    /// Set to true to estimate the coefficient of drag
+    pub estimate: bool,
 }
 
 impl Drag {
@@ -106,6 +118,7 @@ impl Drag {
                     action: "planetary data from third body not loaded",
                 }
             })?,
+            estimate: false,
         }))
     }
 
@@ -120,6 +133,7 @@ impl Drag {
                     action: "planetary data from third body not loaded",
                 }
             })?,
+            estimate: false,
         }))
     }
 }
@@ -135,6 +149,14 @@ impl fmt::Display for Drag {
 }
 
 impl ForceModel for Drag {
+    fn estimation_index(&self) -> Option<usize> {
+        if self.estimate {
+            Some(7)
+        } else {
+            None
+        }
+    }
+
     fn eom(&self, ctx: &Spacecraft, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError> {
         let integration_frame = ctx.orbit.frame;
 
@@ -227,7 +249,7 @@ impl ForceModel for Drag {
         &self,
         _osc_ctx: &Spacecraft,
         _almanac: Arc<Almanac>,
-    ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
+    ) -> Result<(Vector3<f64>, Matrix4x3<f64>), DynamicsError> {
         Err(DynamicsError::DynamicsAstro {
             source: AstroError::PartialsUndefined,
         })

Diff for: src/dynamics/mod.rs

@@ -18,7 +18,7 @@
 
 use crate::cosmic::{AstroError, Orbit};
 use crate::linalg::allocator::Allocator;
-use crate::linalg::{DefaultAllocator, DimName, Matrix3, OMatrix, OVector, Vector3};
+use crate::linalg::{DefaultAllocator, DimName, Matrix3, Matrix4x3, OMatrix, OVector, Vector3};
 use crate::State;
 use anise::almanac::planetary::PlanetaryDataError;
 use anise::almanac::Almanac;
@@ -138,16 +138,20 @@ where
 ///
 /// Examples include Solar Radiation Pressure, drag, etc., i.e. forces which do not need to save the current state, only act on it.
 pub trait ForceModel: Send + Sync + fmt::Display {
+    /// If a parameter of this force model is stored in the spacecraft state, then this function should return the index where this parameter is being affected
+    fn estimation_index(&self) -> Option<usize>;
+
     /// Defines the equations of motion for this force model from the provided osculating state.
     fn eom(&self, ctx: &Spacecraft, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError>;
 
     /// Force models must implement their partials, although those will only be called if the propagation requires the
     /// computation of the STM. The `osc_ctx` is the osculating context, i.e. it changes for each sub-step of the integrator.
+    /// The last row corresponds to the partials of the parameter of this force model wrt the position, i.e. this only applies to conservative forces.
     fn dual_eom(
         &self,
         osc_ctx: &Spacecraft,
         almanac: Arc<Almanac>,
-    ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError>;
+    ) -> Result<(Vector3<f64>, Matrix4x3<f64>), DynamicsError>;
 }
 
 /// The `AccelModel` trait handles immutable dynamics which return an acceleration. Those can be added directly to Orbital Dynamics for example.

Diff for: src/dynamics/solarpressure.rs

@@ -19,7 +19,7 @@
 use super::{DynamicsAlmanacSnafu, DynamicsError, DynamicsPlanetarySnafu, ForceModel};
 use crate::cosmic::eclipse::EclipseLocator;
 use crate::cosmic::{Frame, Spacecraft, AU, SPEED_OF_LIGHT_M_S};
-use crate::linalg::{Const, Matrix3, Vector3};
+use crate::linalg::{Const, Matrix4x3, Vector3};
 use anise::almanac::Almanac;
 use anise::constants::frames::SUN_J2000;
 use hyperdual::{hyperspace_from_vector, linalg::norm, Float, OHyperdual};
@@ -38,6 +38,8 @@ pub struct SolarPressure {
     /// solar flux at 1 AU, in W/m^2
     pub phi: f64,
     pub e_loc: EclipseLocator,
+    /// Set to true to estimate the coefficient of reflectivity
+    pub estimate: bool,
 }
 
 impl SolarPressure {
@@ -66,6 +68,7 @@ impl SolarPressure {
         Ok(Self {
             phi: SOLAR_FLUX_W_m2,
             e_loc,
+            estimate: true,
         })
     }
 
@@ -74,6 +77,16 @@ impl SolarPressure {
         Ok(Arc::new(Self::default_raw(vec![shadow_body], almanac)?))
     }
 
+    /// Accounts for the shadowing of only one body and will set the solar flux at 1 AU to: Phi = 1367.0
+    pub fn default_no_estimation(
+        shadow_body: Frame,
+        almanac: Arc<Almanac>,
+    ) -> Result<Arc<Self>, DynamicsError> {
+        let mut srp = Self::default_raw(vec![shadow_body], almanac)?;
+        srp.estimate = false;
+        Ok(Arc::new(srp))
+    }
+
     /// Must provide the flux in W/m^2
     pub fn with_flux(
         flux_w_m2: f64,
@@ -95,6 +108,14 @@ impl SolarPressure {
 }
 
 impl ForceModel for SolarPressure {
+    fn estimation_index(&self) -> Option<usize> {
+        if self.estimate {
+            Some(6)
+        } else {
+            None
+        }
+    }
+
     fn eom(&self, ctx: &Spacecraft, almanac: Arc<Almanac>) -> Result<Vector3<f64>, DynamicsError> {
         let osc = ctx.orbit;
         // Compute the position of the Sun as seen from the spacecraft
@@ -128,7 +149,7 @@ impl ForceModel for SolarPressure {
         &self,
         ctx: &Spacecraft,
         almanac: Arc<Almanac>,
-    ) -> Result<(Vector3<f64>, Matrix3<f64>), DynamicsError> {
+    ) -> Result<(Vector3<f64>, Matrix4x3<f64>), DynamicsError> {
         let osc = ctx.orbit;
 
         // Compute the position of the Sun as seen from the spacecraft
@@ -145,7 +166,7 @@ impl ForceModel for SolarPressure {
         // Compute the shadowing factor.
         let k: f64 = self
             .e_loc
-            .compute(osc, almanac)
+            .compute(osc, almanac.clone())
             .context(DynamicsAlmanacSnafu {
                 action: "solar radiation pressure computation",
             })?
@@ -169,7 +190,7 @@ impl ForceModel for SolarPressure {
 
         // Extract result into Vector6 and Matrix6
         let mut dx = Vector3::zeros();
-        let mut grad = Matrix3::zeros();
+        let mut grad = Matrix4x3::zeros();
         for i in 0..3 {
             dx[i] += dual_force[i].real();
             // NOTE: Although the hyperdual state is of size 7, we're only setting the values up to 3 (Matrix3)
@@ -178,6 +199,12 @@ impl ForceModel for SolarPressure {
             }
         }
 
+        // Compute the partial wrt to Cr.
+        let wrt_cr = self.eom(ctx, almanac)? / ctx.srp.cr;
+        for j in 0..3 {
+            grad[(3, j)] = wrt_cr[j];
+        }
+
         Ok((dx, grad))
     }
 }

Diff for: src/dynamics/spacecraft.rs

@@ -281,8 +281,8 @@ impl Dynamics for SpacecraftDynamics {
                     d_x[i] = *val;
                 }
 
-                for (i, val) in stm_dt.iter().enumerate() {
-                    d_x[i + <Spacecraft as State>::Size::dim()] = *val;
+                for (i, val) in stm_dt.iter().copied().enumerate() {
+                    d_x[i + <Spacecraft as State>::Size::dim()] = val;
                 }
             }
             None => {
@@ -291,9 +291,10 @@ impl Dynamics for SpacecraftDynamics {
                     .orbital_dyn
                     .eom(&osc_sc.orbit, almanac.clone())?
                     .iter()
+                    .copied()
                     .enumerate()
                 {
-                    d_x[i] = *val;
+                    d_x[i] = val;
                 }
 
                 // Apply the force models for non STM propagation
@@ -411,8 +412,14 @@ impl Dynamics for SpacecraftDynamics {
                 // Add the velocity changes
                 d_x[i + 3] += model_frc[i] / total_mass;
                 // Add the velocity partials
-                for j in 1..4 {
-                    grad[(i + 3, j - 1)] += model_grad[(i, j - 1)] / total_mass;
+                for j in 0..3 {
+                    grad[(i + 3, j)] += model_grad[(i, j)] / total_mass;
+                }
+            }
+            // Add this force model's estimation if applicable.
+            if let Some(idx) = model.estimation_index() {
+                for j in 0..3 {
+                    grad[(j + 3, idx)] += model_grad[(3, j)] / total_mass;
                 }
             }
         }

Diff for: src/od/filter/kalman.rs

@@ -288,7 +288,6 @@ where
         let epoch = nominal_state.epoch();
 
         let mut covar_bar = stm * self.prev_estimate.covar * stm.transpose();
-        let mut snc_used = false;
         // Try to apply an SNC, if applicable
         for (i, snc) in self.process_noise.iter().enumerate().rev() {
             if let Some(snc_matrix) = snc.to_matrix(epoch) {
@@ -327,16 +326,11 @@ where
                 }
                 // Let's add the process noise
                 covar_bar += &gamma * snc_matrix * &gamma.transpose();
-                snc_used = true;
                 // And break so we don't add any more process noise
                 break;
             }
         }
 
-        if !snc_used {
-            debug!("@{} No SNC", epoch);
-        }
-
         let h_tilde_t = &self.h_tilde.transpose();
         let h_p_ht = &self.h_tilde * covar_bar * h_tilde_t;
         // Account for state uncertainty in the measurement noise. Equation 4.10 of ODTK MathSpec.
@@ -363,12 +357,12 @@ where
         }
 
         // Compute the Kalman gain but first adding the measurement noise to H⋅P⋅H^T
-        let mut invertible_part = h_p_ht + &r_k;
-        if !invertible_part.try_inverse_mut() {
+        let mut innovation_covar = h_p_ht + &r_k;
+        if !innovation_covar.try_inverse_mut() {
             return Err(ODError::SingularKalmanGain);
         }
 
-        let gain = covar_bar * h_tilde_t * &invertible_part;
+        let gain = covar_bar * h_tilde_t * &innovation_covar;
 
         // Compute the state estimate
         let (state_hat, res) = if self.ekf {

Diff for: src/od/process/mod.rs

@@ -153,7 +153,7 @@ where
     ) -> Self {
         let init_state = prop.state;
         Self {
-            prop,
+            prop: prop.quiet(),
             kf,
             estimates: Vec::with_capacity(10_000),
             residuals: Vec::with_capacity(10_000),
@@ -790,7 +790,7 @@ where
     ) -> Self {
         let init_state = prop.state;
         Self {
-            prop,
+            prop: prop.quiet(),
             kf,
             estimates: Vec::with_capacity(10_000),
             residuals: Vec::with_capacity(10_000),