Skip to content

Commit

Permalink
Merge pull request #340 from nyx-space/331-measurement-update-and-res…
Browse files Browse the repository at this point in the history
…idual-rejection-shall-account-for-each-participants-noise-level

Refactor orbit/spacecraft state generator to always use the multivariate normal
ChristopherRabotin authored Jul 20, 2024
2 parents 97c16cd + ef77dcc commit 7103621
Showing 29 changed files with 620 additions and 694 deletions.
22 changes: 15 additions & 7 deletions examples/02_jwst_covar_monte_carlo/README.md
Original file line number Diff line number Diff line change
@@ -47,12 +47,12 @@ INFO anise::almanac > Loading almanac from /home/crabotin/.
INFO anise::almanac > Loading as DAF/PCK
INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/jwst_rec.bsp
INFO anise::almanac > Loading as DAF/SPK
JWST defined from 2024-03-30T22:40:09.185653761 ET to 2024-06-24T00:01:09.184303103 ET
[Earth J2000] 2024-06-24T00:01:09.184303103 ET sma = 881064.546158 km ecc = 0.989962 inc = 42.614418 deg raan = 37.843422 deg aop = 62.970831 deg ta = 180.020951 deg
total mass = 6200.000 kg @ [Earth J2000] 2024-06-24T00:01:09.184303103 ET position = [76518.064167, -1396268.919369, -1057612.565026] km velocity = [0.043331, 0.014876, -0.013649] km/s Coast
RIC Σ_x = 0.5 km Σ_y = 0.3 km Σ_z = 1.5 km
RIC Σ_vx = 0.0001 km/s Σ_vy = 0.0006 km/s Σ_vz = 0.003 km/s
Σ_cr = 0 Σ_cd = 0 Σ_mass = 0 kg
JWST defined from 2024-04-13T22:40:09.185630849 ET to 2024-07-08T00:01:09.183912447 ET
[Earth J2000] 2024-07-08T00:01:09.183912447 ET sma = 876856.027357 km ecc = 0.985029 inc = 52.340344 deg raan = 310.126394 deg aop = 130.798599 deg ta = 180.103906 deg
total mass = 6200.000 kg @ [Earth J2000] 2024-07-08T00:01:09.183912447 ET position = [119901.070276, -1389299.665421, -1041369.150539] km velocity = [0.045956, -0.013168, 0.034535] km/s Coast
RIC σ_x = 0.5 km σ_y = 0.3 km σ_z = 1.5 km
RIC σ_vx = 0.0001 km/s σ_vy = 0.0006 km/s σ_vz = 0.003 km/s
σ_cr = 0 σ_cd = 0 σ_mass = 0 kg

INFO nyx_space::od::process > Mapping covariance for 6 days 12 h with 1 min step
INFO nyx_space::od::process::export > Exporting orbit determination result to parquet file...
@@ -65,9 +65,11 @@ INFO nyx_space::mc::results > Evaluating 2 event(s)
INFO nyx_space::mc::results > Trajectory written to 02_jwst_monte_carlo.parquet in 41 s 603 ms 696 μs 128 ns
```

We then run `╰─(.venv) ⠠⠵ ipython examples/02_jwst_covar_monte_carlo/plotting.py` from the virtual environment, whose requirements are in the [requirements.txt](./requirements.txt)

## Analysis

Overall, we can confirm that the 3-sigma covariance is a good approximation of the uncertainty. Notably, however, there are some dispersed trajectories whose Keplerian orbital elements are outside of the 3-sigma bound. This is probably due to the fact that Keplerian orbital elements are defined for orbits where there is a central body, but the James Webb Space Telescope is in a three body orbit, since it's near a Lagrange point.
Overall, we can confirm that the 3-sigma covariance is a good approximation of the uncertainty. Notably, all of the dispersed trajectories fit (nearly) within the 3-σ bounds in the state space and in the Keplerian orbital element space. The rotation from the Keplerioan orbital element space into the Cartesian space is done by building the Jacobian from the desired Keplerian elements into the Cartesian space using hyperdual numbers (also called "automatic differentiation" in the machine learning world). This ensure that we don't lose any precision in the state space change, and that's shown to be the case in these plots.

### State uncertainties

@@ -83,6 +85,12 @@ As expected from any orbit determination software, Nyx can output uncertainties

![JWST MC Z (km)](./plots/jwst_mc_Z_km.png)

![JWST MC VX (km/s)](./plots/jwst_mc_VX_km_s.png)

![JWST MC VY (km/s)](./plots/jwst_mc_VY_km_s.png)

![JWST MC VZ (km/s)](./plots/jwst_mc_VZ_km_s.png)

### Keplerian uncertainties

A few tools try to provide Keplerian uncertainties, but often fail to do so correctly (<small>I'm looking at you, ODTK</small>). Nyx rotates the covariance from its Cartesian form into the Keplerian state space by computing the partial derivatives of each requested parameter with respect to the nominal state. This computation is flawless because it uses automatic differentiation (via _hyperdual numbers_). As such, the OD export also includes all of the state computations supported in Nyx, including uncommon ones like the uncertainties in the energy of the orbit or in the true anomaly.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_X_km.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Y_km.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Z_km.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_aop_deg.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ecc.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_inc_deg.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_raan_deg.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_sma_km.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ta_deg.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_ric_position.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified examples/02_jwst_covar_monte_carlo/plots/jwst_ric_velocity.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
10 changes: 5 additions & 5 deletions examples/02_jwst_covar_monte_carlo/plotting.py
Original file line number Diff line number Diff line change
@@ -3,11 +3,11 @@

if __name__ == "__main__":
df_mc = pl.read_parquet("./02_jwst_monte_carlo.parquet")
df_mc = df_mc.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f"))
df_mc = df_mc.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort("Epoch (UTC)", descending=False)
print(df_mc.describe())

df_covar = pl.read_parquet("./02_jwst_covar_map.parquet")
df_covar = df_covar.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f"))
df_covar = df_covar.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort("Epoch (UTC)", descending=False)
print(df_covar.describe())

# Build the position plots
@@ -18,7 +18,7 @@
go.Scattergl(
x=df_mc["Epoch (UTC)"],
y=df_mc[col],
mode="lines",
mode="markers",
opacity=0.05,
showlegend=True,
name=f"[MC] {coord} (km)",
@@ -63,7 +63,7 @@
go.Scattergl(
x=df_mc["Epoch (UTC)"],
y=df_mc[col],
mode="lines",
mode="markers",
opacity=0.05,
showlegend=True,
name=f"[MC] {coord} (km/s)",
@@ -156,7 +156,7 @@
go.Scattergl(
x=df_mc["Epoch (UTC)"],
y=df_mc[col],
mode="lines",
mode="markers",
opacity=0.05,
showlegend=True,
name=f"[MC] {col}",
4 changes: 2 additions & 2 deletions src/errors.rs
Original file line number Diff line number Diff line change
@@ -100,9 +100,9 @@ impl From<ConfigError> for NyxError {
#[derive(Debug, PartialEq, Snafu)]
#[snafu(visibility(pub(crate)))]
pub enum StateError {
#[snafu(display("{param} is unavailable for this kind of state"))]
#[snafu(display("{param} is unavailable in this context"))]
Unavailable { param: StateParameter },
#[snafu(display("{param} is read only for this kind of state"))]
#[snafu(display("{param} is read only in this context"))]
ReadOnly { param: StateParameter },
#[snafu(display("{param} computation caused {source}"))]
StateAstroError {
40 changes: 40 additions & 0 deletions src/mc/dispersion.rs
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
/*
Nyx, blazing fast astrodynamics
Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published
by the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Affero General Public License for more details.
You should have received a copy of the GNU Affero General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

use crate::md::StateParameter;
use typed_builder::TypedBuilder;

/// A dispersions configuration, allows specifying min/max bounds (by default, they are not set)
#[derive(Copy, Clone, TypedBuilder)]
pub struct StateDispersion {
pub param: StateParameter,
#[builder(default, setter(strip_option))]
pub mean: Option<f64>,
#[builder(default, setter(strip_option))]
pub std_dev: Option<f64>,
}

impl StateDispersion {
pub fn zero_mean(param: StateParameter, std_dev: f64) -> Self {
Self {
param,
std_dev: Some(std_dev),
mean: Some(0.0),
}
}
}
438 changes: 0 additions & 438 deletions src/mc/generator.rs

Large diffs are not rendered by default.

5 changes: 4 additions & 1 deletion src/mc/mod.rs
Original file line number Diff line number Diff line change
@@ -25,8 +25,11 @@ mod montecarlo;

pub use montecarlo::MonteCarlo;

mod dispersion;
pub use dispersion::StateDispersion;

mod generator;
pub use generator::{DispersedState, Dispersion, GaussianGenerator, Generator};
pub use generator::{DispersedState, Dispersion};

mod multivariate;
pub use multivariate::MultivariateNormal;
671 changes: 497 additions & 174 deletions src/mc/multivariate.rs

Large diffs are not rendered by default.

62 changes: 30 additions & 32 deletions src/od/estimate/kfestimate.rs
Original file line number Diff line number Diff line change
@@ -20,16 +20,17 @@ use super::{Estimate, State};
use crate::cosmic::AstroError;
use crate::linalg::allocator::Allocator;
use crate::linalg::{DefaultAllocator, DimName, Matrix, OMatrix, OVector};
use crate::mc::{GaussianGenerator, MultivariateNormal};
use crate::mc::{MultivariateNormal, StateDispersion};
use crate::md::prelude::OrbitDual;
use crate::md::StateParameter;
use crate::{NyxError, Spacecraft};
use crate::Spacecraft;
use na::SMatrix;
use nalgebra::Const;
use rand::SeedableRng;
use rand_distr::Distribution;
use rand_pcg::Pcg64Mcg;
use std::cmp::PartialEq;
use std::error::Error;
use std::fmt;

/// Kalman filter Estimate
@@ -101,20 +102,20 @@ impl KfEstimate<Spacecraft> {
/// Generates an initial Kalman filter state estimate dispersed from the nominal state using the provided standard deviation parameters.
///
/// The resulting estimate will have a diagonal covariance matrix constructed from the variances of each parameter.
/// *Limitation:* This method incorrectly assumes all parameters are statistically independent.
/// *Limitation:* This method may not work correctly for all Keplerian orbital elements, refer to
/// <https://github.com/nyx-space/nyx/issues/339> for details.
pub fn disperse_from_diag(
nominal_state: Spacecraft,
params: &[(StateParameter, f64)],
dispersions: Vec<StateDispersion>,
seed: Option<u128>,
) -> Self {
// Build a generator.
let gen = GaussianGenerator::from_3std_devs(nominal_state, params).unwrap();
) -> Result<Self, Box<dyn Error>> {
let generator = MultivariateNormal::new(nominal_state, dispersions)?;

let mut rng = match seed {
Some(seed) => Pcg64Mcg::new(seed),
None => Pcg64Mcg::from_entropy(),
};
let dispersed_state = gen.sample(&mut rng);
let dispersed_state = generator.sample(&mut rng);

// Compute the difference between both states
let delta_orbit = (nominal_state.orbit - dispersed_state.state.orbit).unwrap();
@@ -138,32 +139,22 @@ impl KfEstimate<Spacecraft> {
// Build the covar from the diagonal
let covar = Matrix::from_diagonal(&diag);

Self {
Ok(Self {
nominal_state: dispersed_state.state,
state_deviation: OVector::<f64, Const<9>>::zeros(),
covar,
covar_bar: covar,
predicted: true,
stm: OMatrix::<f64, Const<9>, Const<9>>::identity(),
}
})
}

/// Builds a multivariate random variable from this estimate's nominal state and covariance, zero mean.
pub fn to_random_variable(&self) -> Result<MultivariateNormal<Spacecraft>, NyxError> {
MultivariateNormal::zero_mean(
pub fn to_random_variable(&self) -> Result<MultivariateNormal, Box<dyn Error>> {
MultivariateNormal::from_spacecraft_cov(
self.nominal_state,
vec![
StateParameter::X,
StateParameter::Y,
StateParameter::Z,
StateParameter::VX,
StateParameter::VY,
StateParameter::VZ,
StateParameter::Cr,
StateParameter::Cd,
StateParameter::FuelMass,
],
self.covar,
self.state_deviation,
)
}

@@ -345,7 +336,10 @@ where

#[cfg(test)]
mod ut_kfest {
use crate::{md::StateParameter, od::estimate::KfEstimate, Spacecraft, GMAT_EARTH_GM};
use crate::{
mc::StateDispersion, md::StateParameter, od::estimate::KfEstimate, Spacecraft,
GMAT_EARTH_GM,
};
use anise::{constants::frames::EARTH_J2000, prelude::Orbit};
use hifitime::Epoch;

@@ -361,14 +355,18 @@ mod ut_kfest {

let initial_estimate = KfEstimate::disperse_from_diag(
initial_state,
&[
(StateParameter::SMA, 1.1),
(StateParameter::Inclination, 0.0025),
(StateParameter::RAAN, 0.022),
(StateParameter::AoP, 0.02),
vec![
StateDispersion::builder()
.param(StateParameter::SMA)
.std_dev(1.1)
.build(),
StateDispersion::zero_mean(StateParameter::Inclination, 0.2),
StateDispersion::zero_mean(StateParameter::RAAN, 0.2),
StateDispersion::zero_mean(StateParameter::AoP, 0.2),
],
Some(0),
);
)
.unwrap();

let initial_state_dev = initial_estimate.nominal_state;

@@ -380,9 +378,9 @@ mod ut_kfest {
println!("Truth initial state:\n{initial_state}\n{initial_state:x}");
println!("Filter initial state:\n{initial_state_dev}\n{initial_state_dev:x}");
println!(
"Initial state dev:\t{init_rss_pos_km:.3} km\t{init_rss_vel_km_s:.3} km/s\n{delta}",
"Initial state dev:\t{init_rss_pos_km:.6} km\t{init_rss_vel_km_s:.6} km/s\n{delta}",
);
println!("covariance: {}", initial_estimate.covar);
println!("covariance: {:.6}", initial_estimate.covar);

// Check that the error is in the square root of the covariance
assert!(delta.radius_km.x < initial_estimate.covar[(0, 0)].sqrt());
2 changes: 2 additions & 0 deletions src/od/mod.rs
Original file line number Diff line number Diff line change
@@ -204,4 +204,6 @@ pub enum ODError {
source: Box<AlmanacError>,
action: &'static str,
},
#[snafu(display("not enough residuals to {action}"))]
ODNoResiduals { action: &'static str },
}
10 changes: 2 additions & 8 deletions src/od/process/export.rs
Original file line number Diff line number Diff line change
@@ -41,14 +41,8 @@ use std::path::{Path, PathBuf};

use super::ODProcess;

impl<
'a,
D: Dynamics,
E: ErrorCtrl,
Msr: Measurement,
A: DimName,
// K: Filter<Spacecraft, A, Msr::MeasurementSize>,
> ODProcess<'a, D, E, Msr, A, Spacecraft, KF<Spacecraft, A, Msr::MeasurementSize>>
impl<'a, D: Dynamics, E: ErrorCtrl, Msr: Measurement, A: DimName>
ODProcess<'a, D, E, Msr, A, Spacecraft, KF<Spacecraft, A, Msr::MeasurementSize>>
where
D::StateType:
Interpolatable + Add<OVector<f64, <Spacecraft as State>::Size>, Output = D::StateType>,
3 changes: 3 additions & 0 deletions src/od/process/mod.rs
Original file line number Diff line number Diff line change
@@ -336,6 +336,9 @@ where
let cur_rms_num = (new_rms - previous_rms).abs();
let cur_rel_rms = cur_rms_num / previous_rms;
if cur_rel_rms < config.relative_tol || cur_rms_num < config.absolute_tol * best_rms {
if previous_rms < best_rms {
best_rms = previous_rms;
}
info!("*****************");
info!("*** CONVERGED ***");
info!("*****************");
1 change: 0 additions & 1 deletion src/od/process/trigger.rs
Original file line number Diff line number Diff line change
@@ -27,7 +27,6 @@ use crate::State;
/// An EkfTrigger on the number of measurements processed and a time between measurements.
pub struct EkfTrigger {
pub num_msrs: usize,
/// In seconds!
pub disable_time: Duration,
/// Set to the sigma number needed to switch to the EKF (cf. 68–95–99.7 rule). If number is negative, this is ignored.
pub within_sigma: f64,
9 changes: 5 additions & 4 deletions tests/monte_carlo/framework.rs
Original file line number Diff line number Diff line change
@@ -32,11 +32,12 @@ fn test_monte_carlo_epoch(almanac: Arc<Almanac>) {
// Build the state generator using a Gaussian distribution (you may use any distribution from rand_distr)
// 5% error on SMA and 5% on Eccentricity
let nominal_state = Spacecraft::from(state);
let random_state = GaussianGenerator::from_std_dev_prcts(

let random_state = MultivariateNormal::new(
nominal_state,
&[
(StateParameter::SMA, 0.05),
(StateParameter::Eccentricity, 0.05),
vec![
StateDispersion::zero_mean(StateParameter::SMA, 0.05),
StateDispersion::zero_mean(StateParameter::Eccentricity, 0.05),
],
)
.unwrap();
37 changes: 15 additions & 22 deletions tests/orbit_determination/robust.rs
Original file line number Diff line number Diff line change
@@ -13,6 +13,7 @@ use nyx::propagators::{PropOpts, Propagator, RK4Fixed};
use nyx::time::{Epoch, TimeUnits, Unit};
use nyx::utils::rss_orbit_errors;
use nyx::Spacecraft;
use nyx_space::mc::StateDispersion;
use polars::prelude::*;
use std::collections::BTreeMap;
use std::env;
@@ -92,13 +93,15 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc<Almanac>) {
// The initial covariance is computed based on the realized dispersions.
let initial_estimate = KfEstimate::disperse_from_diag(
initial_state,
&[
(StateParameter::Inclination, 0.0025),
(StateParameter::RAAN, 0.022),
(StateParameter::AoP, 0.02),
vec![
StateDispersion::zero_mean(StateParameter::Inclination, 0.0025),
StateDispersion::zero_mean(StateParameter::RAAN, 0.022),
StateDispersion::zero_mean(StateParameter::AoP, 0.02),
],
Some(0),
);
)
.unwrap();

println!("Initial estimate:\n{}", initial_estimate);

let initial_state_dev = initial_estimate.nominal_state;
@@ -204,12 +207,6 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc<Almanac>) {
);
}
}
for i in 0..6 {
assert!(
est.covar[(i, i)] < initial_estimate.covar[(i, i)],
"covar[({i}, {i})] did not decrease"
);
}

assert_eq!(
final_truth_state.epoch(),
@@ -285,13 +282,15 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc<Almanac>) {
// The initial covariance is computed based on the realized dispersions.
let initial_estimate = KfEstimate::disperse_from_diag(
initial_state,
&[
(StateParameter::Inclination, 0.0025),
(StateParameter::RAAN, 0.022),
(StateParameter::AoP, 0.02),
vec![
StateDispersion::zero_mean(StateParameter::Inclination, 0.0025),
StateDispersion::zero_mean(StateParameter::RAAN, 0.022),
StateDispersion::zero_mean(StateParameter::AoP, 0.02),
],
Some(0),
);
)
.unwrap();

println!("Initial estimate:\n{}", initial_estimate);

let initial_state_dev = initial_estimate.nominal_state;
@@ -531,12 +530,6 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc<Almanac>) {
);
}
}
for i in 0..6 {
assert!(
est.covar[(i, i)] < initial_estimate.covar[(i, i)],
"covar[({i}, {i})] did not decrease"
);
}

assert_eq!(
final_truth_state.epoch(),

0 comments on commit 7103621

Please sign in to comment.