diff --git a/.github/workflows/coverage.yaml b/.github/workflows/coverage.yaml deleted file mode 100644 index 84dfd1ef..00000000 --- a/.github/workflows/coverage.yaml +++ /dev/null @@ -1,51 +0,0 @@ -name: Daily Workflow - -on: - push: - branches: - - master - workflow_dispatch: - -jobs: - full-coverage: - name: Unit test and integration test coverage analysis - runs-on: ubuntu-latest - - steps: - - name: Checkout - uses: actions/checkout@v3 - - - name: Install stable toolchain - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable - components: rustfmt, clippy - - - name: Download data - run: | - wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de440s.bsp - wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de438.bsp # GMAT validation cases - wget -O data/pck08.pca http://public-data.nyxspace.com/anise/v0.4/pck08.pca - wget -O data/earth_latest_high_prec.bpc http://public-data.nyxspace.com/anise/ci/earth_latest_high_prec-2023-09-08.bpc - - - name: Install cargo-llvm-cov - uses: taiki-e/install-action@cargo-llvm-cov - - name: Generate full code coverage - run: | - # cargo llvm-cov test --lib --no-report --tests - echo "STOP CONDITIONS" - cargo llvm-cov test --no-report stop_ -- --nocapture --ignored - echo "GMAT VALIDATION" - cargo llvm-cov test --no-report gmat_val -- --nocapture --ignored - echo "ORBIT DETERMINATION" - cargo llvm-cov test --no-report od_ -- --nocapture --ignored - echo "MD TARGETERS" - cargo llvm-cov test --no-report tgt_ -- --nocapture --ignored - cargo llvm-cov report --lcov > lcov.txt - # cargo llvm-cov --lcov --output-path lcov.info - - name: Upload coverage to Codecov - uses: codecov/codecov-action@v3 - with: - token: ${{ secrets.CODECOV_TOKEN }} # not required for public repos - files: lcov.info - fail_ci_if_error: false diff --git a/.github/workflows/rust.yaml b/.github/workflows/rust.yaml index 8434d6ee..f7cceffa 100644 --- a/.github/workflows/rust.yaml +++ b/.github/workflows/rust.yaml @@ -73,7 +73,11 @@ jobs: restore-keys: ${{ runner.os }}-cargo- - name: Unit Test (debug) - run: cargo test --lib + run: | + cat /home/runner/work/nyx/nyx/data/tests/config/many_ground_stations.yaml + cat /home/runner/work/nyx/nyx/data/tests/config/one_ground_station.yaml + ls -lh /home/runner/work/nyx/nyx/data/tests/config/ + cargo test --lib - name: All integration tests (release) run: cargo test --release --test "*" @@ -89,6 +93,41 @@ jobs: cargo build --example 03_geo_raise --release cargo build --example 03_geo_sk --release + coverage: + name: Unit test and integration test coverage analysis + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v3 + + - name: Install stable toolchain + uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + components: rustfmt, clippy + + - name: Download data + run: | + wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de440s.bsp + wget -O data/de440s.bsp http://public-data.nyxspace.com/anise/de438.bsp # GMAT validation cases + wget -O data/pck08.pca http://public-data.nyxspace.com/anise/v0.4/pck08.pca + wget -O data/earth_latest_high_prec.bpc http://public-data.nyxspace.com/anise/ci/earth_latest_high_prec-2023-09-08.bpc + + - name: Install cargo-llvm-cov + uses: taiki-e/install-action@cargo-llvm-cov + - name: Generate full code coverage + run: | + cargo llvm-cov test --release --lib --no-report + cargo llvm-cov test --release cov_test --no-report + cargo llvm-cov report --release --lcov --output-path lcov.info + - name: Upload coverage to Codecov + uses: codecov/codecov-action@v3 + with: + token: ${{ secrets.CODECOV_TOKEN }} # not required for public repos + files: lcov.info + fail_ci_if_error: false + lints: name: Lints runs-on: ubuntu-latest diff --git a/Cargo.toml b/Cargo.toml index 9b5f34e0..7eb0b6aa 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -38,8 +38,7 @@ gitlab = { repository = "nyx-space/nyx", branch = "master" } nalgebra = "0.33" log = "0.4" hifitime = "4.0.0" -# anise = "0.5.0" # UNRELEASED -anise = { git = "https://github.com/nyx-space/anise.git", branch = "master" } +anise = "0.5.0" flate2 = { version = "1.0", features = [ "rust_backend", ], default-features = false } @@ -50,7 +49,6 @@ hyperdual = "1.3.0" bytes = "1.0" rand = "0.8" rand_distr = "0.4" -rust-embed = "8" regex = "1.5" rayon = "1.6" lazy_static = "1.4.0" @@ -64,7 +62,7 @@ parquet = { version = "53.0.0", default-features = false, features = [ ] } arrow = "53.0.0" shadow-rs = { version = "0.36.0", default-features = false } -serde_yaml = "0.9.21" +serde_yml = "0.0.12" whoami = "1.3.0" either = { version = "1.8.1", features = ["serde"] } num = "0.4.0" @@ -73,6 +71,7 @@ getrandom = { version = "0.2", features = ["js"] } typed-builder = "0.20.0" snafu = { version = "0.8.3", features = ["backtrace"] } serde_dhall = "0.12" +indexmap = {version = "2.6.0", features = ["serde"]} [dev-dependencies] diff --git a/data/tests/config/many_ground_stations.yaml b/data/tests/config/many_ground_stations.yaml index e21eec8c..9d0f7d3e 100644 --- a/data/tests/config/many_ground_stations.yaml +++ b/data/tests/config/many_ground_stations.yaml @@ -5,18 +5,22 @@ mu_km3_s2: 398600.435436096 shape: null elevation_mask_deg: 5.0 - range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m - doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_s: + bias: + tau: 24 h + process_noise: 50.0e-6 # 5 cm/s light_time_correction: false latitude_deg: 2.3522 longitude_deg: 48.8566 height_km: 0.4 + measurement_types: + - range_km + - doppler_km_s - name: Canberra frame: @@ -28,12 +32,16 @@ longitude_deg: 148.981944 height_km: 0.691750 elevation_mask_deg: 5.0 - range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m - doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_s: + bias: + tau: 24 h + process_noise: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - range_km + - doppler_km_s \ No newline at end of file diff --git a/data/tests/config/one_ground_station.yaml b/data/tests/config/one_ground_station.yaml index 98bd716a..3a7aff68 100644 --- a/data/tests/config/one_ground_station.yaml +++ b/data/tests/config/one_ground_station.yaml @@ -8,12 +8,17 @@ elevation_mask_deg: 5.0 latitude_deg: 2.3522 longitude_deg: 48.8566 height_km: 0.4 -range_noise_km: - bias: - tau: 24 h - process_noise: 5.0e-3 # 5 m -doppler_noise_km_s: - bias: - tau: 24 h - process_noise: 50.0e-6 # 5 cm/s +stochastic_noises: + range_km: + bias: + tau: 24 h + process_noise: 5.0e-3 # 5 m + doppler_km_s: + bias: + tau: 24 h + process_noise: 50.0e-6 # 5 cm/s light_time_correction: false +measurement_types: + - range_km + - doppler_km_s +integration_time: 1 min \ No newline at end of file diff --git a/examples/02_jwst_covar_monte_carlo/main.rs b/examples/02_jwst_covar_monte_carlo/main.rs index cf58f687..a5cbfc3f 100644 --- a/examples/02_jwst_covar_monte_carlo/main.rs +++ b/examples/02_jwst_covar_monte_carlo/main.rs @@ -16,12 +16,12 @@ use nyx::{ dynamics::{guidance::LocalFrame, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, io::ExportCfg, mc::MonteCarlo, - od::{prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess}, + od::{msr::TrackingDataArc, prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess}, propagators::Propagator, Spacecraft, State, }; -use std::{error::Error, sync::Arc}; +use std::{collections::BTreeMap, error::Error, sync::Arc}; fn main() -> Result<(), Box> { pel::init(); @@ -116,13 +116,17 @@ fn main() -> Result<(), Box> { // Build the propagation instance for the OD process. let prop = setup.with(jwst.with_stm(), almanac.clone()); - let mut odp = SpacecraftODProcess::ckf(prop, ckf, None, almanac.clone()); + let mut odp = SpacecraftODProcess::ckf(prop, ckf, BTreeMap::new(), None, almanac.clone()); // Define the prediction step, i.e. how often we want to know the covariance. let step = 1_i64.minutes(); // Finally, predict, and export the trajectory with covariance to a parquet file. odp.predict_for(step, prediction_duration)?; - odp.to_parquet("./02_jwst_covar_map.parquet", ExportCfg::default())?; + odp.to_parquet( + &TrackingDataArc::default(), + "./02_jwst_covar_map.parquet", + ExportCfg::default(), + )?; // === Monte Carlo framework === // Nyx comes with a complete multi-threaded Monte Carlo frame. It's blazing fast. diff --git a/examples/03_geo_analysis/stationkeeping.rs b/examples/03_geo_analysis/stationkeeping.rs index eef6a45d..8fdc730c 100644 --- a/examples/03_geo_analysis/stationkeeping.rs +++ b/examples/03_geo_analysis/stationkeeping.rs @@ -18,7 +18,7 @@ use nyx::{ Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, }, io::{gravity::HarmonicsMem, ExportCfg}, - mc::{MonteCarlo, MultivariateNormal, StateDispersion}, + mc::{MonteCarlo, MvnSpacecraft, StateDispersion}, md::{prelude::Objective, StateParameter}, propagators::{ErrorControl, IntegratorOptions, Propagator}, Spacecraft, State, @@ -88,7 +88,7 @@ fn main() -> Result<(), Box> { // Let's start by defining the dispersion. // The MultivariateNormal structure allows us to define the dispersions in any of the orbital parameters, but these are applied directly in the Cartesian state space. // Note that additional validation on the MVN is in progress -- https://github.com/nyx-space/nyx/issues/339. - let mc_rv = MultivariateNormal::new( + let mc_rv = MvnSpacecraft::new( sc, vec![StateDispersion::zero_mean(StateParameter::SMA, 3.0)], )?; diff --git a/examples/04_lro_od/dsn-network.yaml b/examples/04_lro_od/dsn-network.yaml index 4e8168ea..5f6bc446 100644 --- a/examples/04_lro_od/dsn-network.yaml +++ b/examples/04_lro_od/dsn-network.yaml @@ -1,24 +1,30 @@ -- name: DSS-65 Madrid +DSS-65 Madrid: + name: DSS-65 Madrid frame: ephemeris_id: 399 orientation_id: 399 mu_km3_s2: 398600.435436096 shape: null elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false latitude_deg: 40.427222 longitude_deg: 4.250556 height_km: 0.834939 + measurement_types: + - range_km + - doppler_km_s -- name: DSS-34 Canberra +DSS-34 Canberra: + name: DSS-34 Canberra frame: ephemeris_id: 399 orientation_id: 399 @@ -28,17 +34,22 @@ longitude_deg: 148.981944 height_km: 0.691750 elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - range_km + - doppler_km_s -- name: DSS-13 Goldstone +DSS-13 Goldstone: + name: DSS-13 Goldstone frame: ephemeris_id: 399 orientation_id: 399 @@ -48,12 +59,16 @@ longitude_deg: 243.205 height_km: 1.071149 elevation_mask_deg: 5.0 - range_noise_km: - white_noise: - mean: 0.0 - sigma: 5.0e-3 # 5 m - doppler_noise_km_s: - white_noise: - mean: 0.0 - sigma: 50.0e-6 # 5 cm/s + stochastic_noises: + range_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s light_time_correction: false + measurement_types: + - range_km + - doppler_km_s diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs index 3a2a724f..c948a0e3 100644 --- a/examples/04_lro_od/main.rs +++ b/examples/04_lro_od/main.rs @@ -19,11 +19,10 @@ use nyx::{ io::{ConfigRepr, ExportCfg}, md::prelude::{HarmonicsMem, Traj}, od::{ - msr::RangeDoppler, prelude::{TrackingArcSim, TrkConfig, KF}, - process::{Estimate, NavSolution, ODProcess, ResidRejectCrit, SpacecraftUncertainty}, + process::{Estimate, NavSolution, ResidRejectCrit, SpacecraftUncertainty}, snc::SNC3, - GroundStation, + GroundStation, SpacecraftODProcess, }, propagators::Propagator, Orbit, Spacecraft, State, @@ -198,7 +197,7 @@ fn main() -> Result<(), Box> { .iter() .collect(); - let devices = GroundStation::load_many(ground_station_file)?; + let devices = GroundStation::load_named(ground_station_file)?; // Typical OD software requires that you specify your own tracking schedule or you'll have overlapping measurements. // Nyx can build a tracking schedule for you based on the first station with access. @@ -214,8 +213,8 @@ fn main() -> Result<(), Box> { let configs: BTreeMap = TrkConfig::load_named(trkconfg_yaml)?; // Build the tracking arc simulation to generate a "standard measurement". - let mut trk = TrackingArcSim::::new( - devices, + let mut trk = TrackingArcSim::::new( + devices.clone(), traj_as_flown.clone(), configs, )?; @@ -258,14 +257,15 @@ fn main() -> Result<(), Box> { ); // We'll set up the OD process to reject measurements whose residuals are mover than 4 sigmas away from what we expect. - let mut odp = ODProcess::ckf( + let mut odp = SpacecraftODProcess::ckf( setup.with(initial_estimate.state().with_stm(), almanac.clone()), kf, + devices, Some(ResidRejectCrit::default()), almanac.clone(), ); - odp.process_arc::(&arc)?; + odp.process_arc(&arc)?; let ric_err = traj_as_flown .at(odp.estimates.last().unwrap().epoch())? @@ -275,7 +275,7 @@ fn main() -> Result<(), Box> { println!("RIC Position (m): {}", ric_err.radius_km * 1e3); println!("RIC Velocity (m/s): {}", ric_err.velocity_km_s * 1e3); - odp.to_parquet("./04_lro_od_results.parquet", ExportCfg::default())?; + odp.to_parquet(&arc, "./04_lro_od_results.parquet", ExportCfg::default())?; // In our case, we have the truth trajectory from NASA. // So we can compute the RIC state difference between the real LRO ephem and what we've just estimated. diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py index 4d06dffb..2d854014 100644 --- a/examples/04_lro_od/plot_od_rslt.py +++ b/examples/04_lro_od/plot_od_rslt.py @@ -5,47 +5,49 @@ import plotly.express as px if __name__ == "__main__": - df = pl.read_parquet("./04_lro_od_results.parquet") + df = pl.read_parquet("output_data/ekf_rng_dpl_az_el_odp.parquet") df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( "Epoch (UTC)", descending=False ) - # Add the +/- 3 sigmas on measurement noise - df = df.with_columns( - [ - (3.0 * pl.col("Measurement noise: Range (km)")).alias( - "Measurement noise 3-Sigma: Range (km)" - ), - (-3.0 * pl.col("Measurement noise: Range (km)")).alias( - "Measurement noise -3-Sigma: Range (km)" - ), - ] - ) - df = df.with_columns( - [ - (3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( - "Measurement noise 3-Sigma: Doppler (km/s)" - ), - (-3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( - "Measurement noise -3-Sigma: Doppler (km/s)" - ), - ] - ) + + all_msr_types = ["Range (km)", "Doppler (km/s)", "Azimuth (deg)", "Elevation (deg)"] + msr_type_count = 0 + msr_types = [] + + for msr_type in all_msr_types: + if f"Measurement noise: {msr_type}" in df.columns: + print(f"Found data for {msr_type}") + msr_type_count += 1 + msr_types += [msr_type] + # Add the +/- 3 sigmas on measurement noise + df = df.with_columns( + [ + (3.0 * pl.col(f"Measurement noise: {msr_type}")).alias( + f"Measurement noise 3-Sigma: {msr_type}" + ), + (-3.0 * pl.col(f"Measurement noise: {msr_type}")).alias( + f"Measurement noise -3-Sigma: {msr_type}" + ), + ] + ) # == Residual plots == # Nyx uses the Mahalanobis distance for the residual ratios, so we test the goodness using the Chi Square distribution. - freedoms = 2 # Two degrees of freedoms for the range and the range rate. + freedoms = msr_type_count # Two degrees of freedoms for the range and the range rate. x_chi = np.linspace(chi2.ppf(0.01, freedoms), chi2.ppf(0.99, freedoms), 100) y_chi = chi2.pdf(x_chi, freedoms) # Compute the scaling factor hist = np.histogram(df["Residual ratio"].fill_null(0.0), bins=50)[0] - max_hist = max(hist[1:]) # Ignore the bin of zeros + max_hist = max(hist[1:]) # Ignore the bin of zeros max_chi2_pdf = max(y_chi) scale_factor = max_hist / max_chi2_pdf fig = go.Figure() - fig.add_trace(go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared")) + fig.add_trace( + go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared") + ) fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) fig.show() @@ -63,7 +65,7 @@ df_resid_ok = df.filter(df["Residual Rejected"] == False) # Plot the measurement residuals and their noises. - for msr in ["Range (km)", "Doppler (km/s)"]: + for msr in msr_types: y_cols = [ f"{col}: {msr}" for col in [ @@ -86,6 +88,8 @@ connectgaps=True, line=dict(dash="dash", color="black"), ) + unit = msr.split()[-1][1:-1] + fig.update_layout(yaxis_title=unit) fig.show() # Plot the RIC uncertainty @@ -99,6 +103,7 @@ y=["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"], ).show() + raise AssertionError("stop") # Load the RIC diff. for fname, errname in [ ("04_lro_od_truth_error", "OD vs Flown"), diff --git a/src/cosmic/spacecraft.rs b/src/cosmic/spacecraft.rs index fcc4d16e..9f427ebf 100644 --- a/src/cosmic/spacecraft.rs +++ b/src/cosmic/spacecraft.rs @@ -808,7 +808,7 @@ impl ConfigRepr for Spacecraft {} #[test] fn test_serde() { - use serde_yaml; + use serde_yml; use std::str::FromStr; use anise::constants::frames::EARTH_J2000; @@ -826,10 +826,10 @@ fn test_serde() { let sc = Spacecraft::new(orbit, 500.0, 159.0, 2.0, 2.0, 1.8, 2.2); - let serialized_sc = serde_yaml::to_string(&sc).unwrap(); + let serialized_sc = serde_yml::to_string(&sc).unwrap(); println!("{}", serialized_sc); - let deser_sc: Spacecraft = serde_yaml::from_str(&serialized_sc).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(&serialized_sc).unwrap(); assert_eq!(sc, deser_sc); @@ -860,7 +860,7 @@ drag: cd: 2.2 "#; - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); assert_eq!(sc, deser_sc); // Check that we can specify a thruster info entirely. @@ -898,7 +898,7 @@ thruster: isp_s: 300.5, thrust_N: 1e-5, }); - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); assert_eq!(sc_thruster, deser_sc); // Tests the minimum definition which will set all of the defaults too @@ -922,7 +922,7 @@ dry_mass_kg: 500.0 fuel_mass_kg: 159.0 "#; - let deser_sc: Spacecraft = serde_yaml::from_str(s).unwrap(); + let deser_sc: Spacecraft = serde_yml::from_str(s).unwrap(); let sc = Spacecraft::new(orbit, 500.0, 159.0, 0.0, 0.0, 1.8, 2.2); assert_eq!(sc, deser_sc); diff --git a/src/io/matrices.rs b/src/io/matrices.rs index 99018615..619e2543 100644 --- a/src/io/matrices.rs +++ b/src/io/matrices.rs @@ -84,16 +84,16 @@ pub struct Mat6([[f64; 6]; 6]); #[test] fn test_serde2() { - use serde_yaml; + use serde_yml; let m_diag = Matrix2Serde { inner: Either::Left(Diag2([1.0, 2.0])), }; - println!("Diag -- \n{}", serde_yaml::to_string(&m_diag).unwrap()); + println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); // Load from one line list let diag_s = "[1.0, 2.0]"; - let diag_loaded: Matrix2Serde = serde_yaml::from_str(diag_s).unwrap(); + let diag_loaded: Matrix2Serde = serde_yml::from_str(diag_s).unwrap(); assert_eq!(diag_loaded, m_diag); let m_full = Matrix2Serde { @@ -101,30 +101,30 @@ fn test_serde2() { }; // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yaml::to_string(&m_full).unwrap()); + println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); // Load from list let full_mat = r#" - [1.0, 2.0] # Row 1 - [1.0, 2.0] # Row 2 "#; - let full_loaded: Matrix2Serde = serde_yaml::from_str(full_mat).unwrap(); + let full_loaded: Matrix2Serde = serde_yml::from_str(full_mat).unwrap(); assert_eq!(full_loaded, m_full); } #[test] fn test_serde6() { - use serde_yaml; + use serde_yml; let m_diag = Matrix6Serde { inner: Either::Left(Diag6([1.0, 2.0, 3.0, 4.0, 5.0, 6.0])), }; - println!("Diag -- \n{}", serde_yaml::to_string(&m_diag).unwrap()); + println!("Diag -- \n{}", serde_yml::to_string(&m_diag).unwrap()); // Load from one line list let diag_s = "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]"; - let diag_loaded: Matrix6Serde = serde_yaml::from_str(diag_s).unwrap(); + let diag_loaded: Matrix6Serde = serde_yml::from_str(diag_s).unwrap(); assert_eq!(diag_loaded, m_diag); let m_full = Matrix6Serde { @@ -132,7 +132,7 @@ fn test_serde6() { }; // Serialization will print this as an exhaustive list of lists. - println!("Full -- \n{}", serde_yaml::to_string(&m_full).unwrap()); + println!("Full -- \n{}", serde_yml::to_string(&m_full).unwrap()); // Load from list let full_mat = r#" - [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 1 @@ -143,7 +143,7 @@ fn test_serde6() { - [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] # Row 6 "#; - let full_loaded: Matrix6Serde = serde_yaml::from_str(full_mat).unwrap(); + let full_loaded: Matrix6Serde = serde_yml::from_str(full_mat).unwrap(); assert_eq!(full_loaded, m_full); } diff --git a/src/io/mod.rs b/src/io/mod.rs index 0031377e..ca2d321f 100644 --- a/src/io/mod.rs +++ b/src/io/mod.rs @@ -29,7 +29,7 @@ use hifitime::Duration; use serde::de::DeserializeOwned; use serde::{Deserialize, Deserializer}; use serde::{Serialize, Serializer}; -use serde_yaml::Error as YamlError; +use serde_yml::Error as YamlError; use std::collections::{BTreeMap, HashMap}; use std::convert::From; use std::fmt::Debug; @@ -46,7 +46,7 @@ pub mod estimate; /// Handles loading of gravity models using files of NASA PDS and GMAT COF. Several gunzipped files are provided with nyx. pub mod gravity; pub mod matrices; -pub mod tracking_data; +// pub mod tracking_data; pub mod trajectory_data; use std::io; @@ -163,7 +163,7 @@ pub enum ConfigError { ReadError { source: io::Error }, #[snafu(display("failed to parse YAML configuration file: {source}"))] - ParseError { source: serde_yaml::Error }, + ParseError { source: serde_yml::Error }, #[snafu(display("of invalid configuration: {msg}"))] InvalidConfig { msg: String }, @@ -204,6 +204,8 @@ pub enum InputOutputError { ParseDhall { data: String, err: String }, #[snafu(display("error serializing {what} to Dhall: {err}"))] SerializeDhall { what: String, err: String }, + #[snafu(display("empty dataset error when (de)serializing for {action}"))] + EmptyDataset { action: &'static str }, } impl PartialEq for InputOutputError { @@ -221,7 +223,7 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided path to a yaml @@ -232,7 +234,7 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a map of names to "selves" from the provided path to a yaml @@ -243,19 +245,19 @@ pub trait ConfigRepr: Debug + Sized + Serialize + DeserializeOwned { let file = File::open(path).context(ReadSnafu)?; let reader = BufReader::new(file); - serde_yaml::from_reader(reader).context(ParseSnafu) + serde_yml::from_reader(reader).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided string of a yaml fn loads_many(data: &str) -> Result, ConfigError> { debug!("Loading YAML:\n{data}"); - serde_yaml::from_str(data).context(ParseSnafu) + serde_yml::from_str(data).context(ParseSnafu) } /// Builds a sequence of "Selves" from the provided string of a yaml fn loads_named(data: &str) -> Result, ConfigError> { debug!("Loading YAML:\n{data}"); - serde_yaml::from_str(data).context(ParseSnafu) + serde_yml::from_str(data).context(ParseSnafu) } } diff --git a/src/io/tracking_data.rs b/src/io/tracking_data.rs index 1ac59bfa..227796a3 100644 --- a/src/io/tracking_data.rs +++ b/src/io/tracking_data.rs @@ -19,7 +19,7 @@ use crate::{ io::{MissingDataSnafu, ParquetSnafu}, linalg::{allocator::Allocator, DefaultAllocator, OVector}, - od::{msr::TrackingArc, Measurement}, + od::{msr::TrackingDataArc, Measurement}, }; #[cfg(feature = "python")] diff --git a/src/mc/mod.rs b/src/mc/mod.rs index 9a298869..8494ec91 100644 --- a/src/mc/mod.rs +++ b/src/mc/mod.rs @@ -32,7 +32,7 @@ mod generator; pub use generator::{DispersedState, Dispersion}; mod multivariate; -pub use multivariate::MultivariateNormal; +pub use multivariate::MvnSpacecraft; mod results; pub use results::{Results, Stats}; diff --git a/src/mc/multivariate.rs b/src/mc/multivariate.rs index 3aa1dd82..fd8c94b8 100644 --- a/src/mc/multivariate.rs +++ b/src/mc/multivariate.rs @@ -27,10 +27,8 @@ use nalgebra::{DMatrix, DVector, SMatrix, SVector}; use rand_distr::{Distribution, Normal}; use snafu::ResultExt; -/// A multivariate state generator for Monte Carlo analyses. Ensures that the covariance is properly applied on all provided state variables. -pub struct MultivariateNormal -// TODO: Rename to MultivariateNormalSpacecraft ?? -{ +/// A multivariate spacecraft state generator for Monte Carlo analyses. Ensures that the covariance is properly applied on all provided state variables. +pub struct MvnSpacecraft { /// The template state pub template: Spacecraft, pub dispersions: Vec, @@ -42,7 +40,7 @@ pub struct MultivariateNormal pub std_norm_distr: Normal, } -impl MultivariateNormal { +impl MvnSpacecraft { /// Creates a new mulivariate state generator from a mean and covariance on the set of state parameters. /// The covariance must be positive semi definite. /// @@ -266,7 +264,7 @@ impl MultivariateNormal { } } -impl Distribution> for MultivariateNormal { +impl Distribution> for MvnSpacecraft { fn sample(&self, rng: &mut R) -> DispersedState { // Generate the vector representing the state let x_rng = SVector::::from_fn(|_, _| self.std_norm_distr.sample(rng)); @@ -331,7 +329,7 @@ mod multivariate_ut { // Check that we can modify the radius magnitude let std_dev = 1.0; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![StateDispersion::builder() .param(StateParameter::Rmag) @@ -380,7 +378,7 @@ mod multivariate_ut { let std_dev = [10.0, 10.0, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.0]; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( Spacecraft { orbit: state, ..Default::default() @@ -462,7 +460,7 @@ mod multivariate_ut { let angle_sigma_deg = 0.2; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![StateDispersion::zero_mean( StateParameter::RAAN, @@ -530,7 +528,7 @@ mod multivariate_ut { let inc_sigma_deg = 0.15; let angle_sigma_deg = 0.02; - let generator = MultivariateNormal::new( + let generator = MvnSpacecraft::new( state, vec![ StateDispersion::zero_mean(StateParameter::SMA, sma_sigma_km), diff --git a/src/md/opti/mod.rs b/src/md/opti/mod.rs index 1524a795..51485570 100644 --- a/src/md/opti/mod.rs +++ b/src/md/opti/mod.rs @@ -25,9 +25,6 @@ pub mod raphson_finite_diff; pub mod raphson_hyperdual; pub mod solution; pub mod target_variable; -/// Uses a Levenberg Marquardt minimizer to solve the damped least squares problem. -// #[cfg(feature = "broken-donotuse")] -// pub mod minimize_lm; pub mod targeter; #[derive(Copy, Clone, Debug, PartialEq, Eq)] diff --git a/src/od/estimate/kfestimate.rs b/src/od/estimate/kfestimate.rs index f2b3ea00..b141e95d 100644 --- a/src/od/estimate/kfestimate.rs +++ b/src/od/estimate/kfestimate.rs @@ -20,7 +20,7 @@ use super::{Estimate, State}; use crate::cosmic::AstroError; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, Matrix, OMatrix, OVector}; -use crate::mc::{MultivariateNormal, StateDispersion}; +use crate::mc::{MvnSpacecraft, StateDispersion}; use crate::md::prelude::OrbitDual; use crate::md::StateParameter; use crate::Spacecraft; @@ -108,7 +108,7 @@ impl KfEstimate { dispersions: Vec, seed: Option, ) -> Result> { - let generator = MultivariateNormal::new(nominal_state, dispersions)?; + let generator = MvnSpacecraft::new(nominal_state, dispersions)?; let mut rng = match seed { Some(seed) => Pcg64Mcg::new(seed), @@ -149,12 +149,8 @@ impl KfEstimate { } /// Builds a multivariate random variable from this estimate's nominal state and covariance, zero mean. - pub fn to_random_variable(&self) -> Result> { - MultivariateNormal::from_spacecraft_cov( - self.nominal_state, - self.covar, - self.state_deviation, - ) + pub fn to_random_variable(&self) -> Result> { + MvnSpacecraft::from_spacecraft_cov(self.nominal_state, self.covar, self.state_deviation) } /// Returns the 1-sigma uncertainty for a given parameter, in that parameter's unit diff --git a/src/od/estimate/param.rs b/src/od/estimate/param.rs deleted file mode 100644 index 4c647ba3..00000000 --- a/src/od/estimate/param.rs +++ /dev/null @@ -1,274 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::md::StateParameter; - -use super::EpochFormat; -use std::cmp::PartialEq; -use std::fmt; - -/// Allowed headers, with an optional frame. -/// TODO: Support units -#[allow(non_camel_case_types)] -#[derive(Clone, Debug, PartialEq)] -pub enum EstimateParameter { - /// The epoch in the specified format - Epoch(EpochFormat), - /// Parameters of the estimated state - EstimatedState(Vec), - /// Parameters of the nominal state - NominalState(Vec), - /// Orbit deviation X (km) - Delta_x, - /// Orbit deviation Y (km) - Delta_y, - /// Orbit deviation Z (km) - Delta_z, - /// Orbit deviation VX (km/s) - Delta_vx, - /// Orbit deviation VY (km/s) - Delta_vy, - /// Orbit deviation VZ (km/s) - Delta_vz, - /// Covariance matrix [1,1] - Cx_x { frame: Option }, - /// Covariance matrix [2,1] - Cy_x { frame: Option }, - /// Covariance matrix [2,2] - Cy_y { frame: Option }, - /// Covariance matrix [3,1] - Cz_x { frame: Option }, - /// Covariance matrix [3,2] - Cz_y { frame: Option }, - /// Covariance matrix [3,3] - Cz_z { frame: Option }, - /// Covariance matrix [4,1] - Cx_dot_x { frame: Option }, - /// Covariance matrix [4,2] - Cx_dot_y { frame: Option }, - /// Covariance matrix [4,3] - Cx_dot_z { frame: Option }, - /// Covariance matrix [4,4] - Cx_dot_x_dot { frame: Option }, - /// Covariance matrix [5,1] - Cy_dot_x { frame: Option }, - /// Covariance matrix [5,2] - Cy_dot_y { frame: Option }, - /// Covariance matrix [5,3] - Cy_dot_z { frame: Option }, - /// Covariance matrix [5,4] - Cy_dot_x_dot { frame: Option }, - /// Covariance matrix [5,5] - Cy_dot_y_dot { frame: Option }, - /// Covariance matrix [6,1] - Cz_dot_x { frame: Option }, - /// Covariance matrix [6,2] - Cz_dot_y { frame: Option }, - /// Covariance matrix [6,3] - Cz_dot_z { frame: Option }, - /// Covariance matrix [6,4] - Cz_dot_x_dot { frame: Option }, - /// Covariance matrix [6,5] - Cz_dot_y_dot { frame: Option }, - /// Covariance matrix [6,6] - Cz_dot_z_dot { frame: Option }, - /// Boolean specifying whether this is a prediction or not - Prediction, - /// Norm of the position items of the covariance (Cx_x, Cy_y, Cz_z) - Covar_pos, - /// Norm of the velocity items of the covariance (Cx_dot_x_dot, Cy_dot_y_dot, Cz_dot_z_dot) - Covar_vel, -} - -impl fmt::Display for EstimateParameter { - fn fmt(&self, fh: &mut fmt::Formatter) -> fmt::Result { - match self { - EstimateParameter::Epoch(efmt) => write!(fh, "Epoch:{efmt:?}"), - EstimateParameter::EstimatedState(hdr) => { - let mut seq = Vec::with_capacity(hdr.len()); - for element in hdr { - seq.push(format!("Estimate:{element}")); - } - write!(fh, "{}", seq.join(",")) - } - EstimateParameter::NominalState(hdr) => { - let mut seq = Vec::with_capacity(hdr.len()); - for element in hdr { - seq.push(format!("Nominal:{element}")); - } - write!(fh, "{}", seq.join(",")) - } - EstimateParameter::Delta_x => write!(fh, "delta_x"), - EstimateParameter::Delta_y => write!(fh, "delta_y"), - EstimateParameter::Delta_z => write!(fh, "delta_z"), - EstimateParameter::Delta_vx => write!(fh, "delta_vx"), - EstimateParameter::Delta_vy => write!(fh, "delta_vy"), - EstimateParameter::Delta_vz => write!(fh, "delta_vz"), - EstimateParameter::Cx_x { frame } => { - if let Some(f) = frame { - write!(fh, "cx_x:{f}") - } else { - write!(fh, "cx_x") - } - } - EstimateParameter::Cy_x { frame } => { - if let Some(f) = frame { - write!(fh, "cy_x:{f}") - } else { - write!(fh, "cy_x") - } - } - EstimateParameter::Cy_y { frame } => { - if let Some(f) = frame { - write!(fh, "cy_y:{f}") - } else { - write!(fh, "cy_y") - } - } - EstimateParameter::Cz_x { frame } => { - if let Some(f) = frame { - write!(fh, "cz_x:{f}") - } else { - write!(fh, "cz_x") - } - } - EstimateParameter::Cz_y { frame } => { - if let Some(f) = frame { - write!(fh, "cz_y:{f}") - } else { - write!(fh, "cz_y") - } - } - EstimateParameter::Cz_z { frame } => { - if let Some(f) = frame { - write!(fh, "cz_z:{f}") - } else { - write!(fh, "cz_z") - } - } - EstimateParameter::Cx_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_x:{f}") - } else { - write!(fh, "cx_dot_x") - } - } - EstimateParameter::Cx_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_y:{f}") - } else { - write!(fh, "cx_dot_y") - } - } - EstimateParameter::Cx_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_z:{f}") - } else { - write!(fh, "cx_dot_z") - } - } - EstimateParameter::Cx_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cx_dot_x_dot:{f}") - } else { - write!(fh, "cx_dot_x_dot") - } - } - EstimateParameter::Cy_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_x:{f}") - } else { - write!(fh, "cy_dot_x") - } - } - EstimateParameter::Cy_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_y:{f}") - } else { - write!(fh, "cy_dot_y") - } - } - EstimateParameter::Cy_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_z:{f}") - } else { - write!(fh, "cy_dot_z") - } - } - EstimateParameter::Cy_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_x_dot:{f}") - } else { - write!(fh, "cy_dot_x_dot") - } - } - EstimateParameter::Cy_dot_y_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cy_dot_y_dot:{f}") - } else { - write!(fh, "cy_dot_y_dot") - } - } - EstimateParameter::Cz_dot_x { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_x:{f}") - } else { - write!(fh, "cz_dot_x") - } - } - EstimateParameter::Cz_dot_y { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_y:{f}") - } else { - write!(fh, "cz_dot_y") - } - } - EstimateParameter::Cz_dot_z { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_z:{f}") - } else { - write!(fh, "cz_dot_z") - } - } - EstimateParameter::Cz_dot_x_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_x_dot:{f}") - } else { - write!(fh, "cz_dot_x_dot") - } - } - EstimateParameter::Cz_dot_y_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_y_dot:{f}") - } else { - write!(fh, "cz_dot_y_dot") - } - } - EstimateParameter::Cz_dot_z_dot { frame } => { - if let Some(f) = frame { - write!(fh, "cz_dot_z_dot:{f}") - } else { - write!(fh, "cz_dot_z_dot") - } - } - EstimateParameter::Prediction => write!(fh, "prediction"), - EstimateParameter::Covar_pos => write!(fh, "covar_position"), - EstimateParameter::Covar_vel => write!(fh, "covar_velocity"), - } - } -} diff --git a/src/od/estimate/residual.rs b/src/od/estimate/residual.rs index 30d3d0b1..4cdd93c4 100644 --- a/src/od/estimate/residual.rs +++ b/src/od/estimate/residual.rs @@ -18,7 +18,9 @@ use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, OVector}; +use crate::od::msr::MeasurementType; use hifitime::Epoch; +use indexmap::IndexSet; use std::fmt; /// Stores an Estimate, as the result of a `time_update` or `measurement_update`. @@ -44,6 +46,8 @@ where pub rejected: bool, /// Name of the tracker that caused this residual pub tracker: Option, + /// Measurement types used to compute this residual (in order) + pub msr_types: IndexSet, } impl Residual @@ -61,6 +65,7 @@ where ratio: 0.0, rejected: true, tracker: None, + msr_types: IndexSet::new(), } } @@ -79,6 +84,7 @@ where tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: true, tracker: None, + msr_types: IndexSet::new(), } } @@ -97,8 +103,30 @@ where tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: false, tracker: None, + msr_types: IndexSet::new(), } } + + /// Returns the prefit for this measurement type, if available + pub fn prefit(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.prefit[idx]) + } + + /// Returns the postfit for this measurement type, if available + pub fn postfit(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.postfit[idx]) + } + + /// Returns the tracker noise for this measurement type, if available + pub fn trk_noise(&self, msr_type: MeasurementType) -> Option { + self.msr_types + .get_index_of(&msr_type) + .map(|idx| self.tracker_msr_noise[idx]) + } } impl fmt::Display for Residual @@ -109,7 +137,8 @@ where fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { write!( f, - "Residual from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}", + "Residual of {:?} from {} at {}: ratio = {:.3}\nPrefit {} Postfit {}", + self.msr_types, self.tracker.as_ref().unwrap_or(&"Unknown".to_string()), self.epoch, self.ratio, diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 3ebca8ce..575e2620 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -297,10 +297,22 @@ where // Compute observation deviation (usually marked as y_i) let prefit = real_obs - computed_obs; - // Compute the prefit ratio for the automatic rejection - let r_k_inv = r_k.clone().try_inverse().ok_or(ODError::SingularNoiseRk)?; - let ratio_mat = prefit.transpose() * r_k_inv * &prefit; - let ratio = ratio_mat[0].sqrt(); + // Compute the prefit ratio for the automatic rejection. + // The measurement covariance is the square of the measurement itself. + // So we compute its Cholesky decomposition to return to the non squared values. + let r_k_chol_inv = r_k + .clone() + .cholesky() + .ok_or(ODError::SingularNoiseRk)? + .l() + .try_inverse() + .ok_or(ODError::SingularNoiseRk)?; + + // Then we multiply the inverted r_k matrix with prefit, giving us the vector of + // ratios of the prefit over the r_k matrix diagonal. + let ratio_vec = r_k_chol_inv * &prefit; + // Compute the ratio as the dot product. + let ratio = ratio_vec.dot(&ratio_vec); if let Some(resid_reject) = resid_rejection { if ratio > resid_reject.num_sigmas { diff --git a/src/od/ground_station.rs b/src/od/ground_station.rs deleted file mode 100644 index 3d8f753f..00000000 --- a/src/od/ground_station.rs +++ /dev/null @@ -1,567 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use anise::astro::{Aberration, AzElRange, PhysicsResult}; -use anise::errors::AlmanacResult; -use anise::prelude::{Almanac, Frame, Orbit}; - -use super::msr::RangeDoppler; -use super::noise::StochasticNoise; -use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDeviceSim}; -use crate::errors::EventError; -use crate::io::ConfigRepr; -use crate::md::prelude::{Interpolatable, Traj}; -use crate::md::EventEvaluator; -use crate::time::Epoch; -use crate::Spacecraft; -use hifitime::{Duration, Unit}; -use nalgebra::{allocator::Allocator, DefaultAllocator, OMatrix}; -use rand_pcg::Pcg64Mcg; -use serde_derive::{Deserialize, Serialize}; -use snafu::ResultExt; -use std::fmt; -use std::sync::Arc; - -#[cfg(feature = "python")] -use pyo3::prelude::*; - -/// GroundStation defines a two-way ranging and doppler station. -#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] -#[cfg_attr(feature = "python", pyclass)] -#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] -pub struct GroundStation { - pub name: String, - /// in degrees - pub elevation_mask_deg: f64, - /// in degrees - pub latitude_deg: f64, - /// in degrees - pub longitude_deg: f64, - /// in km - pub height_km: f64, - pub frame: Frame, - /// Duration needed to generate a measurement (if unset, it is assumed to be instantaneous) - #[serde(skip)] - pub integration_time: Option, - /// Whether to correct for light travel time - pub light_time_correction: bool, - /// Noise on the timestamp of the measurement - pub timestamp_noise_s: Option, - /// Noise on the range data of the measurement - pub range_noise_km: Option, - /// Noise on the Doppler data of the measurement - pub doppler_noise_km_s: Option, -} - -impl GroundStation { - /// Initializes a point on the surface of a celestial object. - /// This is meant for analysis, not for spacecraft navigation. - pub fn from_point( - name: String, - latitude_deg: f64, - longitude_deg: f64, - height_km: f64, - frame: Frame, - ) -> Self { - Self { - name, - elevation_mask_deg: 0.0, - latitude_deg, - longitude_deg, - height_km, - frame, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: None, - doppler_noise_km_s: None, - } - } - - pub fn dss65_madrid( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - Self { - name: "Madrid".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: 40.427_222, - longitude_deg: 4.250_556, - height_km: 0.834_939, - frame: iau_earth, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - - pub fn dss34_canberra( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - Self { - name: "Canberra".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: -35.398_333, - longitude_deg: 148.981_944, - height_km: 0.691_750, - frame: iau_earth, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - - pub fn dss13_goldstone( - elevation_mask: f64, - range_noise_km: StochasticNoise, - doppler_noise_km_s: StochasticNoise, - iau_earth: Frame, - ) -> Self { - Self { - name: "Goldstone".to_string(), - elevation_mask_deg: elevation_mask, - latitude_deg: 35.247_164, - longitude_deg: 243.205, - height_km: 1.071_149_04, - frame: iau_earth, - integration_time: None, - light_time_correction: false, - timestamp_noise_s: None, - range_noise_km: Some(range_noise_km), - doppler_noise_km_s: Some(doppler_noise_km_s), - } - } - - /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. - /// This is a shortcut to almanac.azimuth_elevation_range_sez. - pub fn azimuth_elevation_of( - &self, - rx: Orbit, - obstructing_body: Option, - almanac: &Almanac, - ) -> AlmanacResult { - let ab_corr = if self.light_time_correction { - Aberration::LT - } else { - Aberration::NONE - }; - almanac.azimuth_elevation_range_sez( - rx, - self.to_orbit(rx.epoch, almanac).unwrap(), - obstructing_body, - ab_corr, - ) - } - - /// Return this ground station as an orbit in its current frame - pub fn to_orbit(&self, epoch: Epoch, almanac: &Almanac) -> PhysicsResult { - use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; - Orbit::try_latlongalt( - self.latitude_deg, - self.longitude_deg, - self.height_km, - MEAN_EARTH_ANGULAR_VELOCITY_DEG_S, - epoch, - almanac.frame_from_uid(self.frame).unwrap(), - ) - } - - /// Returns the timestamp noise, range noise, and doppler noise for this ground station at the provided epoch. - fn noises( - &mut self, - epoch: Epoch, - rng: Option<&mut Pcg64Mcg>, - ) -> Result<(f64, f64, f64), ODError> { - let timestamp_noise_s; - let range_noise_km; - let doppler_noise_km_s; - - match rng { - Some(rng) => { - // Add the range noise, or return an error if it's not configured. - range_noise_km = self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .sample(epoch, rng); - - // Add the Doppler noise, or return an error if it's not configured. - doppler_noise_km_s = self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .sample(epoch, rng); - - // Only add the epoch noise if it's configured, it's valid to not have any noise on the clock. - if let Some(mut timestamp_noise) = self.timestamp_noise_s { - timestamp_noise_s = timestamp_noise.sample(epoch, rng); - } else { - timestamp_noise_s = 0.0; - } - } - None => { - timestamp_noise_s = 0.0; - range_noise_km = 0.0; - doppler_noise_km_s = 0.0; - } - }; - - Ok((timestamp_noise_s, range_noise_km, doppler_noise_km_s)) - } -} - -impl ConfigRepr for GroundStation {} - -impl TrackingDeviceSim for GroundStation { - /// Perform a measurement from the ground station to the receiver (rx). - fn measure( - &mut self, - epoch: Epoch, - traj: &Traj, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, - ) -> Result, ODError> { - match self.integration_time { - Some(integration_time) => { - let rx_0 = traj.at(epoch - integration_time).context(ODTrajSnafu)?; - let rx_1 = traj.at(epoch).context(ODTrajSnafu)?; - - let obstructing_body = if !self.frame.ephem_origin_match(rx_0.frame()) { - Some(rx_0.frame()) - } else { - None - }; - - let aer_t0 = self - .azimuth_elevation_of(rx_0.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - let aer_t1 = self - .azimuth_elevation_of(rx_1.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - - if aer_t0.elevation_deg < self.elevation_mask_deg - || aer_t1.elevation_deg < self.elevation_mask_deg - { - debug!( - "{} (el. mask {:.3} deg) but object moves from {:.3} to {:.3} deg -- no measurement", - self.name, self.elevation_mask_deg, aer_t0.elevation_deg, aer_t1.elevation_deg - ); - return Ok(None); - } - - // Noises are computed at the midpoint of the integration time. - let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = - self.noises(epoch - integration_time * 0.5, rng)?; - - Ok(Some(RangeDoppler::two_way( - aer_t0, - aer_t1, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ))) - } - None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac), - } - } - - fn name(&self) -> String { - self.name.clone() - } - - fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult { - almanac.transform_to(self.to_orbit(epoch, &almanac).unwrap(), frame, None) - } - - fn measure_instantaneous( - &mut self, - rx: Spacecraft, - rng: Option<&mut Pcg64Mcg>, - almanac: Arc, - ) -> Result, ODError> { - let obstructing_body = if !self.frame.ephem_origin_match(rx.frame()) { - Some(rx.frame()) - } else { - None - }; - - let aer = self - .azimuth_elevation_of(rx.orbit, obstructing_body, &almanac) - .context(ODAlmanacSnafu { - action: "computing AER", - })?; - - if aer.elevation_deg >= self.elevation_mask_deg { - // Only update the noises if the measurement is valid. - let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = - self.noises(rx.orbit.epoch, rng)?; - - Ok(Some(RangeDoppler::one_way( - aer, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ))) - } else { - debug!( - "{} {} (el. mask {:.3} deg), object at {:.3} deg -- no measurement", - self.name, rx.orbit.epoch, self.elevation_mask_deg, aer.elevation_deg - ); - Ok(None) - } - } - - /// Returns the measurement noise of this ground station. - /// - /// # Methodology - /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise. - /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is - /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The - /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. - fn measurement_covar( - &mut self, - epoch: Epoch, - ) -> Result< - OMatrix< - f64, - ::MeasurementSize, - ::MeasurementSize, - >, - ODError, - > { - let range_noise_km2 = self - .range_noise_km - .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .covariance(epoch); - let doppler_noise_km2_s2 = self - .doppler_noise_km_s - .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .covariance(epoch); - - let mut msr_noises = OMatrix::< - f64, - ::MeasurementSize, - ::MeasurementSize, - >::zeros(); - msr_noises[(0, 0)] = range_noise_km2; - msr_noises[(1, 1)] = doppler_noise_km2_s2; - - Ok(msr_noises) - } -} - -impl fmt::Display for GroundStation { - // Prints the Keplerian orbital elements with units - fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!( - f, - "{} (lat.: {:.4} deg long.: {:.4} deg alt.: {:.3} m) [{}]", - self.name, - self.latitude_deg, - self.longitude_deg, - self.height_km * 1e3, - self.frame, - ) - } -} - -impl EventEvaluator for &GroundStation -where - DefaultAllocator: Allocator + Allocator + Allocator, -{ - /// Compute the elevation in the SEZ frame. This call will panic if the frame of the input state does not match that of the ground station. - fn eval(&self, rx_gs_frame: &S, almanac: Arc) -> Result { - let dt = rx_gs_frame.epoch(); - // Then, compute the rotation matrix from the body fixed frame of the ground station to its topocentric frame SEZ. - let tx_gs_frame = self.to_orbit(dt, &almanac).unwrap(); - - let from = tx_gs_frame.frame.orientation_id * 1_000 + 1; - let dcm_topo2fixed = tx_gs_frame - .dcm_from_topocentric_to_body_fixed(from) - .unwrap() - .transpose(); - - // Now, rotate the spacecraft in the SEZ frame to compute its elevation as seen from the ground station. - // We transpose the DCM so that it's the fixed to topocentric rotation. - let rx_sez = (dcm_topo2fixed * rx_gs_frame.orbit()).unwrap(); - let tx_sez = (dcm_topo2fixed * tx_gs_frame).unwrap(); - // Now, let's compute the range ρ. - let rho_sez = (rx_sez - tx_sez).unwrap(); - - // Finally, compute the elevation (math is the same as declination) - // Source: Vallado, section 4.4.3 - // Only the sine is needed as per Vallado, and the formula is the same as the declination - // because we're in the SEZ frame. - Ok(rho_sez.declination_deg() - self.elevation_mask_deg) - } - - fn eval_string(&self, state: &S, almanac: Arc) -> Result { - Ok(format!( - "Elevation from {} is {:.6} deg on {}", - self.name, - self.eval(state, almanac)? + self.elevation_mask_deg, - state.epoch() - )) - } - - /// Epoch precision of the election evaluator is 1 ms - fn epoch_precision(&self) -> Duration { - 1 * Unit::Second - } - - /// Angle precision of the elevation evaluator is 1 millidegree. - fn value_precision(&self) -> f64 { - 1e-3 - } -} - -#[cfg(test)] -mod gs_ut { - use anise::constants::frames::IAU_EARTH_FRAME; - - use crate::io::ConfigRepr; - use crate::od::prelude::*; - - #[test] - fn test_load_single() { - use std::env; - use std::path::PathBuf; - - use hifitime::TimeUnits; - - // Get the path to the root directory of the current Cargo project - let test_data: PathBuf = [ - env::var("CARGO_MANIFEST_DIR").unwrap(), - "data".to_string(), - "tests".to_string(), - "config".to_string(), - "one_ground_station.yaml".to_string(), - ] - .iter() - .collect(); - - assert!(test_data.exists(), "Could not find the test data"); - - let gs = GroundStation::load(test_data).unwrap(); - - dbg!(&gs); - - let expected_gs = GroundStation { - name: "Demo ground station".to_string(), - frame: IAU_EARTH_FRAME, - elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), - latitude_deg: 2.3522, - longitude_deg: 48.8566, - height_km: 0.4, - light_time_correction: false, - timestamp_noise_s: None, - integration_time: None, - }; - - assert_eq!(expected_gs, gs); - } - - #[test] - fn test_load_many() { - use hifitime::TimeUnits; - use std::env; - use std::path::PathBuf; - - // Get the path to the root directory of the current Cargo project - - let test_file: PathBuf = [ - env::var("CARGO_MANIFEST_DIR").unwrap(), - "data".to_string(), - "tests".to_string(), - "config".to_string(), - "many_ground_stations.yaml".to_string(), - ] - .iter() - .collect(); - - let stations = GroundStation::load_many(test_file).unwrap(); - - dbg!(&stations); - - let expected = vec![ - GroundStation { - name: "Demo ground station".to_string(), - frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), - elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), - latitude_deg: 2.3522, - longitude_deg: 48.8566, - height_km: 0.4, - light_time_correction: false, - timestamp_noise_s: None, - integration_time: None, - }, - GroundStation { - name: "Canberra".to_string(), - frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), - elevation_mask_deg: 5.0, - range_noise_km: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), - ..Default::default() - }), - doppler_noise_km_s: Some(StochasticNoise { - bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), - ..Default::default() - }), - latitude_deg: -35.398333, - longitude_deg: 148.981944, - height_km: 0.691750, - light_time_correction: false, - timestamp_noise_s: None, - integration_time: None, - }, - ]; - - assert_eq!(expected, stations); - - // Serialize back - let reser = serde_yaml::to_string(&expected).unwrap(); - dbg!(reser); - } -} diff --git a/src/od/ground_station/builtin.rs b/src/od/ground_station/builtin.rs new file mode 100644 index 00000000..07e398d0 --- /dev/null +++ b/src/od/ground_station/builtin.rs @@ -0,0 +1,108 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::*; + +impl GroundStation { + pub fn dss65_madrid( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + + Self { + name: "Madrid".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: 40.427_222, + longitude_deg: 4.250_556, + height_km: 0.834_939, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + stochastic_noises: Some(stochastics), + } + } + + pub fn dss34_canberra( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + + Self { + name: "Canberra".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: -35.398_333, + longitude_deg: 148.981_944, + height_km: 0.691_750, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + stochastic_noises: Some(stochastics), + } + } + + pub fn dss13_goldstone( + elevation_mask: f64, + range_noise_km: StochasticNoise, + doppler_noise_km_s: StochasticNoise, + iau_earth: Frame, + ) -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, range_noise_km); + stochastics.insert(MeasurementType::Doppler, doppler_noise_km_s); + + Self { + name: "Goldstone".to_string(), + elevation_mask_deg: elevation_mask, + latitude_deg: 35.247_164, + longitude_deg: 243.205, + height_km: 1.071_149_04, + frame: iau_earth, + measurement_types, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + stochastic_noises: Some(stochastics), + } + } +} diff --git a/src/od/ground_station/event.rs b/src/od/ground_station/event.rs new file mode 100644 index 00000000..9a790708 --- /dev/null +++ b/src/od/ground_station/event.rs @@ -0,0 +1,75 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::GroundStation; +use crate::md::EventEvaluator; +use crate::{errors::EventError, md::prelude::Interpolatable}; +use anise::prelude::Almanac; +use hifitime::{Duration, Unit}; +use nalgebra::{allocator::Allocator, DefaultAllocator}; +use std::sync::Arc; + +impl EventEvaluator for &GroundStation +where + DefaultAllocator: Allocator + Allocator + Allocator, +{ + /// Compute the elevation in the SEZ frame. This call will panic if the frame of the input state does not match that of the ground station. + fn eval(&self, rx_gs_frame: &S, almanac: Arc) -> Result { + let dt = rx_gs_frame.epoch(); + // Then, compute the rotation matrix from the body fixed frame of the ground station to its topocentric frame SEZ. + let tx_gs_frame = self.to_orbit(dt, &almanac).unwrap(); + + let from = tx_gs_frame.frame.orientation_id * 1_000 + 1; + let dcm_topo2fixed = tx_gs_frame + .dcm_from_topocentric_to_body_fixed(from) + .unwrap() + .transpose(); + + // Now, rotate the spacecraft in the SEZ frame to compute its elevation as seen from the ground station. + // We transpose the DCM so that it's the fixed to topocentric rotation. + let rx_sez = (dcm_topo2fixed * rx_gs_frame.orbit()).unwrap(); + let tx_sez = (dcm_topo2fixed * tx_gs_frame).unwrap(); + // Now, let's compute the range ρ. + let rho_sez = (rx_sez - tx_sez).unwrap(); + + // Finally, compute the elevation (math is the same as declination) + // Source: Vallado, section 4.4.3 + // Only the sine is needed as per Vallado, and the formula is the same as the declination + // because we're in the SEZ frame. + Ok(rho_sez.declination_deg() - self.elevation_mask_deg) + } + + fn eval_string(&self, state: &S, almanac: Arc) -> Result { + Ok(format!( + "Elevation from {} is {:.6} deg on {}", + self.name, + self.eval(state, almanac)? + self.elevation_mask_deg, + state.epoch() + )) + } + + /// Epoch precision of the election evaluator is 1 ms + fn epoch_precision(&self) -> Duration { + 1 * Unit::Second + } + + /// Angle precision of the elevation evaluator is 1 millidegree. + fn value_precision(&self) -> f64 { + 1e-3 + } +} diff --git a/src/od/ground_station/mod.rs b/src/od/ground_station/mod.rs new file mode 100644 index 00000000..5ddbc476 --- /dev/null +++ b/src/od/ground_station/mod.rs @@ -0,0 +1,378 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use anise::astro::{Aberration, AzElRange, PhysicsResult}; +use anise::constants::frames::EARTH_J2000; +use anise::errors::AlmanacResult; +use anise::prelude::{Almanac, Frame, Orbit}; +use indexmap::{IndexMap, IndexSet}; +use snafu::ensure; + +use super::msr::MeasurementType; +use super::noise::StochasticNoise; +use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; +use crate::io::ConfigRepr; +use crate::od::NoiseNotConfiguredSnafu; +use crate::time::Epoch; +use hifitime::Duration; +use rand_pcg::Pcg64Mcg; +use serde_derive::{Deserialize, Serialize}; +use std::fmt; + +pub mod builtin; +pub mod event; +pub mod trk_device; + +#[cfg(feature = "python")] +use pyo3::prelude::*; + +/// GroundStation defines a two-way ranging and doppler station. +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] +#[cfg_attr(feature = "python", pyclass)] +#[cfg_attr(feature = "python", pyo3(module = "nyx_space.orbit_determination"))] +pub struct GroundStation { + pub name: String, + /// in degrees + pub elevation_mask_deg: f64, + /// in degrees + pub latitude_deg: f64, + /// in degrees + pub longitude_deg: f64, + /// in km + pub height_km: f64, + pub frame: Frame, + pub measurement_types: IndexSet, + /// Duration needed to generate a measurement (if unset, it is assumed to be instantaneous) + pub integration_time: Option, + /// Whether to correct for light travel time + pub light_time_correction: bool, + /// Noise on the timestamp of the measurement + pub timestamp_noise_s: Option, + pub stochastic_noises: Option>, +} + +impl GroundStation { + /// Initializes a point on the surface of a celestial object. + /// This is meant for analysis, not for spacecraft navigation. + pub fn from_point( + name: String, + latitude_deg: f64, + longitude_deg: f64, + height_km: f64, + frame: Frame, + ) -> Self { + Self { + name, + elevation_mask_deg: 0.0, + latitude_deg, + longitude_deg, + height_km, + frame, + measurement_types: IndexSet::new(), + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + stochastic_noises: None, + } + } + + /// Returns a copy of this ground station with the new measurement type added (or replaced) + pub fn with_msr_type(mut self, msr_type: MeasurementType, noise: StochasticNoise) -> Self { + if self.stochastic_noises.is_none() { + self.stochastic_noises = Some(IndexMap::new()); + } + + self.stochastic_noises + .as_mut() + .unwrap() + .insert(msr_type, noise); + + self.measurement_types.insert(msr_type); + + self + } + + /// Returns a copy of this ground station without the provided measurement type (if defined, else no error) + pub fn without_msr_type(mut self, msr_type: MeasurementType) -> Self { + if let Some(noises) = self.stochastic_noises.as_mut() { + noises.swap_remove(&msr_type); + } + + self.measurement_types.swap_remove(&msr_type); + + self + } + + pub fn with_integration_time(mut self, integration_time: Option) -> Self { + self.integration_time = integration_time; + + self + } + + /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. + /// This is a shortcut to almanac.azimuth_elevation_range_sez. + pub fn azimuth_elevation_of( + &self, + rx: Orbit, + obstructing_body: Option, + almanac: &Almanac, + ) -> AlmanacResult { + let ab_corr = if self.light_time_correction { + Aberration::LT + } else { + Aberration::NONE + }; + almanac.azimuth_elevation_range_sez( + rx, + self.to_orbit(rx.epoch, almanac).unwrap(), + obstructing_body, + ab_corr, + ) + } + + /// Return this ground station as an orbit in its current frame + pub fn to_orbit(&self, epoch: Epoch, almanac: &Almanac) -> PhysicsResult { + use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; + Orbit::try_latlongalt( + self.latitude_deg, + self.longitude_deg, + self.height_km, + MEAN_EARTH_ANGULAR_VELOCITY_DEG_S, + epoch, + almanac.frame_from_uid(self.frame).unwrap(), + ) + } + + /// Returns the noises for all measurement types configured for this ground station at the provided epoch, timestamp noise is the first entry. + fn noises(&mut self, epoch: Epoch, rng: Option<&mut Pcg64Mcg>) -> Result, ODError> { + let mut noises = vec![0.0; self.measurement_types.len() + 1]; + + if let Some(rng) = rng { + ensure!( + self.stochastic_noises.is_some(), + NoiseNotConfiguredSnafu { + kind: "ground station stochastics".to_string(), + } + ); + // Add the timestamp noise first + + if let Some(mut timestamp_noise) = self.timestamp_noise_s { + noises[0] = timestamp_noise.sample(epoch, rng); + } + + let stochastics = self.stochastic_noises.as_mut().unwrap(); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + noises[ii + 1] = stochastics + .get_mut(msr_type) + .ok_or(ODError::NoiseNotConfigured { + kind: format!("{msr_type:?}"), + })? + .sample(epoch, rng); + } + } + + Ok(noises) + } +} + +impl Default for GroundStation { + fn default() -> Self { + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + Self { + name: "UNDEFINED".to_string(), + measurement_types, + elevation_mask_deg: 0.0, + latitude_deg: 0.0, + longitude_deg: 0.0, + height_km: 0.0, + frame: EARTH_J2000, + integration_time: None, + light_time_correction: false, + timestamp_noise_s: None, + stochastic_noises: None, + } + } +} + +impl ConfigRepr for GroundStation {} + +impl fmt::Display for GroundStation { + // Prints the Keplerian orbital elements with units + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!( + f, + "{} (lat.: {:.4} deg long.: {:.4} deg alt.: {:.3} m) [{}]", + self.name, + self.latitude_deg, + self.longitude_deg, + self.height_km * 1e3, + self.frame, + ) + } +} + +#[cfg(test)] +mod gs_ut { + + use anise::constants::frames::IAU_EARTH_FRAME; + use indexmap::{IndexMap, IndexSet}; + + use crate::io::ConfigRepr; + use crate::od::prelude::*; + + #[test] + fn test_load_single() { + use std::env; + use std::path::PathBuf; + + use hifitime::TimeUnits; + + let test_data: PathBuf = [ + env::var("CARGO_MANIFEST_DIR").unwrap(), + "data".to_string(), + "tests".to_string(), + "config".to_string(), + "one_ground_station.yaml".to_string(), + ] + .iter() + .collect(); + + assert!(test_data.exists(), "Could not find the test data"); + + let gs = GroundStation::load(test_data).unwrap(); + + dbg!(&gs); + + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert( + MeasurementType::Range, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), + ..Default::default() + }, + ); + stochastics.insert( + MeasurementType::Doppler, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), + ..Default::default() + }, + ); + + let expected_gs = GroundStation { + name: "Demo ground station".to_string(), + frame: IAU_EARTH_FRAME, + measurement_types, + elevation_mask_deg: 5.0, + stochastic_noises: Some(stochastics), + latitude_deg: 2.3522, + longitude_deg: 48.8566, + height_km: 0.4, + light_time_correction: false, + timestamp_noise_s: None, + integration_time: Some(60 * Unit::Second), + }; + + println!("{}", serde_yml::to_string(&expected_gs).unwrap()); + + assert_eq!(expected_gs, gs); + } + + #[test] + fn test_load_many() { + use hifitime::TimeUnits; + use std::env; + use std::path::PathBuf; + + let test_file: PathBuf = [ + env::var("CARGO_MANIFEST_DIR").unwrap(), + "data".to_string(), + "tests".to_string(), + "config".to_string(), + "many_ground_stations.yaml".to_string(), + ] + .iter() + .collect(); + + let stations = GroundStation::load_many(test_file).unwrap(); + + dbg!(&stations); + + let mut measurement_types = IndexSet::new(); + measurement_types.insert(MeasurementType::Range); + measurement_types.insert(MeasurementType::Doppler); + + let mut stochastics = IndexMap::new(); + stochastics.insert( + MeasurementType::Range, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-3).unwrap()), + ..Default::default() + }, + ); + stochastics.insert( + MeasurementType::Doppler, + StochasticNoise { + bias: Some(GaussMarkov::new(1.days(), 5e-5).unwrap()), + ..Default::default() + }, + ); + + let expected = vec![ + GroundStation { + name: "Demo ground station".to_string(), + frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), + measurement_types: measurement_types.clone(), + elevation_mask_deg: 5.0, + stochastic_noises: Some(stochastics.clone()), + latitude_deg: 2.3522, + longitude_deg: 48.8566, + height_km: 0.4, + light_time_correction: false, + timestamp_noise_s: None, + integration_time: None, + }, + GroundStation { + name: "Canberra".to_string(), + frame: IAU_EARTH_FRAME.with_mu_km3_s2(398600.435436096), + measurement_types, + elevation_mask_deg: 5.0, + stochastic_noises: Some(stochastics), + latitude_deg: -35.398333, + longitude_deg: 148.981944, + height_km: 0.691750, + light_time_correction: false, + timestamp_noise_s: None, + integration_time: None, + }, + ]; + + assert_eq!(expected, stations); + + // Serialize back + let reser = serde_yml::to_string(&expected).unwrap(); + dbg!(reser); + } +} diff --git a/src/od/ground_station/trk_device.rs b/src/od/ground_station/trk_device.rs new file mode 100644 index 00000000..b2b31e05 --- /dev/null +++ b/src/od/ground_station/trk_device.rs @@ -0,0 +1,169 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDevice}; +use crate::md::prelude::{Interpolatable, Traj}; +use crate::od::msr::measurement::Measurement; +use crate::od::msr::MeasurementType; +use crate::time::Epoch; +use crate::Spacecraft; +use anise::errors::AlmanacResult; +use anise::frames::Frame; +use anise::prelude::{Almanac, Orbit}; +use hifitime::TimeUnits; +use indexmap::IndexSet; +use rand_pcg::Pcg64Mcg; +use snafu::ResultExt; +use std::sync::Arc; + +use super::GroundStation; + +impl TrackingDevice for GroundStation { + fn measurement_types(&self) -> &IndexSet { + &self.measurement_types + } + + /// Perform a measurement from the ground station to the receiver (rx). + fn measure( + &mut self, + epoch: Epoch, + traj: &Traj, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError> { + match self.integration_time { + Some(integration_time) => { + // If out of traj bounds, return None, else the whole strand is rejected. + let rx_0 = match traj.at(epoch - integration_time) { + Ok(rx) => rx, + Err(_) => return Ok(None), + }; + + let rx_1 = match traj.at(epoch).context(ODTrajSnafu) { + Ok(rx) => rx, + Err(_) => return Ok(None), + }; + + let obstructing_body = if !self.frame.ephem_origin_match(rx_0.frame()) { + Some(rx_0.frame()) + } else { + None + }; + + let aer_t0 = self + .azimuth_elevation_of(rx_0.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + let aer_t1 = self + .azimuth_elevation_of(rx_1.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + + if aer_t0.elevation_deg < self.elevation_mask_deg + || aer_t1.elevation_deg < self.elevation_mask_deg + { + debug!( + "{} (el. mask {:.3} deg) but object moves from {:.3} to {:.3} deg -- no measurement", + self.name, self.elevation_mask_deg, aer_t0.elevation_deg, aer_t1.elevation_deg + ); + return Ok(None); + } + + // Noises are computed at the midpoint of the integration time. + let noises = self.noises(epoch - integration_time * 0.5, rng)?; + + let mut msr = Measurement::new(self.name.clone(), epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_two_way(aer_t0, aer_t1, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) + } + None => self.measure_instantaneous(traj.at(epoch).context(ODTrajSnafu)?, rng, almanac), + } + } + + fn name(&self) -> String { + self.name.clone() + } + + fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult { + almanac.transform_to(self.to_orbit(epoch, &almanac).unwrap(), frame, None) + } + + fn measure_instantaneous( + &mut self, + rx: Spacecraft, + rng: Option<&mut Pcg64Mcg>, + almanac: Arc, + ) -> Result, ODError> { + let obstructing_body = if !self.frame.ephem_origin_match(rx.frame()) { + Some(rx.frame()) + } else { + None + }; + + let aer = self + .azimuth_elevation_of(rx.orbit, obstructing_body, &almanac) + .context(ODAlmanacSnafu { + action: "computing AER", + })?; + + if aer.elevation_deg >= self.elevation_mask_deg { + // Only update the noises if the measurement is valid. + let noises = self.noises(rx.orbit.epoch, rng)?; + + let mut msr = Measurement::new(self.name.clone(), rx.orbit.epoch + noises[0].seconds()); + + for (ii, msr_type) in self.measurement_types.iter().enumerate() { + let msr_value = msr_type.compute_one_way(aer, noises[ii + 1])?; + msr.push(*msr_type, msr_value); + } + + Ok(Some(msr)) + } else { + debug!( + "{} {} (el. mask {:.3} deg), object at {:.3} deg -- no measurement", + self.name, rx.orbit.epoch, self.elevation_mask_deg, aer.elevation_deg + ); + Ok(None) + } + } + + /// Returns the measurement noise of this ground station. + /// + /// # Methodology + /// Noises are modeled using a [StochasticNoise] process, defined by the sigma on the turn-on bias and on the steady state noise. + /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is + /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The + /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. + fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result { + let stochastics = self.stochastic_noises.as_ref().unwrap(); + + Ok(stochastics + .get(&msr_type) + .ok_or(ODError::NoiseNotConfigured { + kind: format!("{msr_type:?}"), + })? + .covariance(epoch)) + } +} diff --git a/src/od/mod.rs b/src/od/mod.rs index c017b587..a28ef042 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -19,12 +19,10 @@ use crate::dynamics::DynamicsError; pub use crate::dynamics::{Dynamics, NyxError}; use crate::io::{ConfigError, InputOutputError}; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector}; +use crate::linalg::OVector; use crate::md::trajectory::TrajError; use crate::propagators::PropagationError; use crate::time::Epoch; -use crate::Orbit; pub use crate::{State, TimeTagged}; use anise::almanac::planetary::PlanetaryDataError; use anise::errors::AlmanacError; @@ -54,8 +52,7 @@ pub mod simulator; /// Provides the interfaces to the orbit determination process pub mod process; -use arrow::datatypes::Field; -pub use simulator::TrackingDeviceSim; +pub use simulator::TrackingDevice; /// Provides all state noise compensation functionality pub mod snc; @@ -64,10 +61,10 @@ pub mod snc; pub type SpacecraftODProcess<'a> = self::process::ODProcess< 'a, crate::md::prelude::SpacecraftDynamics, - msr::RangeDoppler, + nalgebra::Const<2>, nalgebra::Const<3>, - crate::Spacecraft, filter::kalman::KF, nalgebra::Const<2>>, + GroundStation, >; #[allow(unused_imports)] @@ -86,88 +83,88 @@ pub mod prelude { pub use crate::time::{Duration, Epoch, TimeUnits, Unit}; } -/// A trait defining a measurement that can be used in the orbit determination process. -pub trait Measurement: Copy + TimeTagged { - /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2). - type MeasurementSize: DimName; - - /// Returns the fields for this kind of measurement. - /// The metadata must include a `unit` field with the unit. - fn fields() -> Vec; - - /// Initializes a new measurement from the provided data. - fn from_observation(epoch: Epoch, obs: OVector) -> Self - where - DefaultAllocator: Allocator; - - /// Returns the measurement/observation as a vector. - fn observation(&self) -> OVector - where - DefaultAllocator: Allocator; -} - -/// The Estimate trait defines the interface that is the opposite of a `SolveFor`. -/// For example, `impl EstimateFrom for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`. -/// -/// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and -/// the ground station bias information. Then, the `impl EstimateFrom for OrbitAndBias` will be added, where `OrbitAndBias` is the -/// new State that includes the orbit and the bias of one ground station. -pub trait EstimateFrom -where - Self: State, - DefaultAllocator: Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator, -{ - /// From the state extract the state to be estimated - fn extract(from: O) -> Self; - - /// Returns the measurement sensitivity (often referred to as H tilde). - /// - /// # Limitations - /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero. - /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use. - /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator. - fn sensitivity( - msr: &M, - receiver: Self, - transmitter: Orbit, - ) -> OMatrix - where - DefaultAllocator: Allocator; -} - -/// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state. -/// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame. -impl EstimateFrom for O -where - O: State + Measurement, - Self: State, - DefaultAllocator: Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator, -{ - fn extract(from: O) -> Self { - from - } - - fn sensitivity( - _full_state_msr: &O, - _receiver: Self, - _transmitter: Orbit, - ) -> OMatrix::MeasurementSize, Self::Size> - where - DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, - { - OMatrix::::identity() - } -} +// /// A trait defining a measurement that can be used in the orbit determination process. +// pub trait Measurement: Copy + TimeTagged { +// /// Defines how much data is measured. For example, if measuring range and range rate, this should be of size 2 (nalgebra::U2). +// type MeasurementSize: DimName; + +// /// Returns the fields for this kind of measurement. +// /// The metadata must include a `unit` field with the unit. +// fn fields() -> Vec; + +// /// Initializes a new measurement from the provided data. +// fn from_observation(epoch: Epoch, obs: OVector) -> Self +// where +// DefaultAllocator: Allocator; + +// /// Returns the measurement/observation as a vector. +// fn observation(&self) -> OVector +// where +// DefaultAllocator: Allocator; +// } + +// /// The Estimate trait defines the interface that is the opposite of a `SolveFor`. +// /// For example, `impl EstimateFrom for Orbit` means that the `Orbit` can be estimated (i.e. "solved for") from a `Spacecraft`. +// /// +// /// In the future, there will be a way to estimate ground station biases, for example. This will need a new State that includes both the Spacecraft and +// /// the ground station bias information. Then, the `impl EstimateFrom for OrbitAndBias` will be added, where `OrbitAndBias` is the +// /// new State that includes the orbit and the bias of one ground station. +// pub trait EstimateFrom +// where +// Self: State, +// DefaultAllocator: Allocator<::Size> +// + Allocator<::VecLength> +// + Allocator<::Size, ::Size> +// + Allocator +// + Allocator +// + Allocator, +// { +// /// From the state extract the state to be estimated +// fn extract(from: O) -> Self; + +// /// Returns the measurement sensitivity (often referred to as H tilde). +// /// +// /// # Limitations +// /// The transmitter is necessarily an Orbit. This implies that any non-orbit parameter in the estimation vector must be a zero-bias estimator, i.e. it must be assumed that the parameter should be zero. +// /// This is a limitation of the current implementation. It could be fixed by specifying another State like trait in the EstimateFrom trait, significantly adding complexity with little practical use. +// /// To solve for non zero bias parameters, one ought to be able to estimate the _delta_ of that parameter and want that delta to return to zero, thereby becoming a zero-bias estimator. +// fn sensitivity( +// msr: &M, +// receiver: Self, +// transmitter: Orbit, +// ) -> OMatrix +// where +// DefaultAllocator: Allocator; +// } + +// /// A generic implementation of EstimateFrom for any State that is also a Measurement, e.g. if there is a direct observation of the full state. +// /// WARNING: The frame of the full state measurement is _not_ checked to match that of `Self` or of the filtering frame. +// impl EstimateFrom for O +// where +// O: State + Measurement, +// Self: State, +// DefaultAllocator: Allocator<::Size> +// + Allocator<::VecLength> +// + Allocator<::Size, ::Size> +// + Allocator +// + Allocator +// + Allocator, +// { +// fn extract(from: O) -> Self { +// from +// } + +// fn sensitivity( +// _full_state_msr: &O, +// _receiver: Self, +// _transmitter: Orbit, +// ) -> OMatrix::MeasurementSize, Self::Size> +// where +// DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, +// { +// OMatrix::::identity() +// } +// } #[derive(Debug, PartialEq, Snafu)] #[snafu(visibility(pub(crate)))] @@ -193,7 +190,9 @@ pub enum ODError { #[snafu(display("Noise matrix is singular"))] SingularNoiseRk, #[snafu(display("{kind} noise not configured"))] - NoiseNotConfigured { kind: &'static str }, + NoiseNotConfigured { kind: String }, + #[snafu(display("measurement sim error: {details}"))] + MeasurementSimError { details: String }, #[snafu(display("during an OD encountered {source}"))] ODTrajError { source: TrajError }, #[snafu(display("OD failed because {source}"))] diff --git a/src/od/msr/arc.rs b/src/od/msr/arc.rs deleted file mode 100644 index 3c259431..00000000 --- a/src/od/msr/arc.rs +++ /dev/null @@ -1,322 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use std::collections::{BTreeMap, HashMap, HashSet}; -use std::error::Error; -use std::fmt::{Debug, Display}; -use std::fs::File; -use std::ops::RangeBounds; -use std::path::{Path, PathBuf}; -use std::sync::Arc; - -use crate::io::watermark::pq_writer; -use crate::io::{ConfigError, ExportCfg}; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, DimName}; -use crate::md::trajectory::Interpolatable; -use crate::od::prelude::TrkConfig; -use crate::od::{Measurement, TrackingDeviceSim}; -use crate::State; -use arrow::array::{Array, Float64Builder, StringBuilder}; -use arrow::datatypes::{DataType, Field, Schema}; -use arrow::record_batch::RecordBatch; -use hifitime::prelude::{Duration, Epoch, Unit}; -use hifitime::TimeScale; -use parquet::arrow::ArrowWriter; - -/// Tracking arc contains the tracking data generated by the tracking devices defined in this structure. -/// This structure is shared between both simulated and real tracking arcs. -#[derive(Clone, Default, Debug)] -pub struct TrackingArc -where - Msr: Measurement, - DefaultAllocator: Allocator, -{ - /// The YAML configuration to set up these devices - pub device_cfg: String, - /// A chronological list of the measurements to the devices used to generate these measurements. If the name of the device does not appear in the list of devices, then the measurement will be ignored. - pub measurements: Vec<(String, Msr)>, -} - -impl Display for TrackingArc -where - Msr: Measurement, - DefaultAllocator: - Allocator + Allocator, -{ - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!( - f, - "{} measurements from {:?}", - self.measurements.len(), - self.device_names() - ) - } -} - -impl TrackingArc -where - Msr: Measurement, - DefaultAllocator: - Allocator + Allocator, -{ - /// Store this tracking arc to a parquet file. - pub fn to_parquet_simple + Debug>( - &self, - path: P, - ) -> Result> { - self.to_parquet(path, ExportCfg::default()) - } - - /// Store this tracking arc to a parquet file, with optional metadata and a timestamp appended to the filename. - pub fn to_parquet + Debug>( - &self, - path: P, - cfg: ExportCfg, - ) -> Result> { - let path_buf = cfg.actual_path(path); - - if cfg.step.is_some() { - warn!("The `step` parameter in the export is not supported for tracking arcs."); - } - - if cfg.fields.is_some() { - warn!("The `fields` parameter in the export is not supported for tracking arcs."); - } - - // Build the schema - let mut hdrs = vec![ - Field::new("Epoch (UTC)", DataType::Utf8, false), - Field::new("Tracking device", DataType::Utf8, false), - ]; - - let mut msr_fields = Msr::fields(); - - hdrs.append(&mut msr_fields); - - // Build the schema - let schema = Arc::new(Schema::new(hdrs)); - let mut record: Vec> = Vec::new(); - - // Build the measurement iterator - - let measurements = - if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { - let start = cfg - .start_epoch - .unwrap_or_else(|| self.measurements.first().unwrap().1.epoch()); - let end = cfg - .end_epoch - .unwrap_or_else(|| self.measurements.last().unwrap().1.epoch()); - - info!("Exporting measurements from {start} to {end}."); - - self.measurements - .iter() - .filter(|msr| msr.1.epoch() >= start && msr.1.epoch() <= end) - .cloned() - .collect() - } else { - self.measurements.to_vec() - }; - - // Build all of the records - - // Epochs - let mut utc_epoch = StringBuilder::new(); - for m in &measurements { - utc_epoch.append_value(m.1.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); - } - record.push(Arc::new(utc_epoch.finish())); - - // Device names - let mut device_names = StringBuilder::new(); - for m in &measurements { - device_names.append_value(m.0.clone()); - } - record.push(Arc::new(device_names.finish())); - - // Measurement data - for obs_no in 0..Msr::MeasurementSize::USIZE { - let mut data_builder = Float64Builder::new(); - - for m in &measurements { - data_builder.append_value(m.1.observation()[obs_no]); - } - record.push(Arc::new(data_builder.finish())); - } - - // Serialize all of the devices and add that to the parquet file too. - let mut metadata = HashMap::new(); - metadata.insert("devices".to_string(), self.device_cfg.clone()); - metadata.insert("Purpose".to_string(), "Tracking Arc Data".to_string()); - if let Some(add_meta) = cfg.metadata { - for (k, v) in add_meta { - metadata.insert(k, v); - } - } - - let props = pq_writer(Some(metadata)); - - let file = File::create(&path_buf)?; - - let mut writer = ArrowWriter::try_new(file, schema.clone(), props).unwrap(); - - let batch = RecordBatch::try_new(schema, record)?; - writer.write(&batch)?; - writer.close()?; - - info!("Serialized {self} to {}", path_buf.display()); - - // Return the path this was written to - Ok(path_buf) - } - - /// Returns the set of devices from which measurements were taken. This accounts for the availability of measurements, so if a device was not available, it will not appear in this set. - pub fn device_names(&self) -> HashSet<&String> { - let mut set = HashSet::new(); - self.measurements.iter().for_each(|(name, _msr)| { - set.insert(name); - }); - set - } - - /// Returns the minimum duration between two subsequent measurements. - /// This is important to correctly set up the propagator and not miss any measurement. - pub fn min_duration_sep(&self) -> Option { - if self.measurements.is_empty() { - None - } else { - let mut windows = self.measurements.windows(2); - let first_window = windows.next()?; - // Ensure that the first interval isn't zero - let mut min_interval = - (first_window[1].1.epoch() - first_window[0].1.epoch()).max(2 * Unit::Second); - for window in windows { - let interval = window[1].1.epoch() - window[0].1.epoch(); - if interval > Duration::ZERO && interval < min_interval { - min_interval = interval; - } - } - Some(min_interval) - } - } - - /// If this tracking arc has devices that can be used to generate simulated measurements, - /// then this function can be used to rebuild said measurement devices - pub fn rebuild_devices(&self) -> Result, ConfigError> - where - MsrIn: Interpolatable, - D: TrackingDeviceSim, - DefaultAllocator: Allocator<::Size> - + Allocator<::Size, ::Size> - + Allocator<::VecLength>, - { - let devices = D::loads_named(&self.device_cfg)?; - - for device in devices.keys() { - if !self.device_names().contains(device) { - info!("no measurements from {device} in loaded arc"); - continue; - } - } - - Ok(devices) - } - - /// Returns a new tracking arc that only contains measurements that fall within the given epoch range. - pub fn filter_by_epoch>(&self, bound: R) -> Self { - let mut measurements = Vec::new(); - for (name, msr) in &self.measurements { - if bound.contains(&msr.epoch()) { - measurements.push((name.clone(), *msr)); - } - } - - Self { - measurements, - device_cfg: self.device_cfg.clone(), - } - } - - /// Returns a new tracking arc that only contains measurements that fall within the given offset from the first epoch - pub fn filter_by_offset>(&self, bound: R) -> Self { - if self.measurements.is_empty() { - return Self { - device_cfg: self.device_cfg.clone(), - measurements: Vec::new(), - }; - } - let ref_epoch = self.measurements[0].1.epoch(); - let mut measurements = Vec::new(); - for (name, msr) in &self.measurements { - if bound.contains(&(msr.epoch() - ref_epoch)) { - measurements.push((name.clone(), *msr)); - } - } - - Self { - measurements, - device_cfg: self.device_cfg.clone(), - } - } - - /// If this tracking arc has devices that can be used to generate simulated measurements, - /// then this function can be used to rebuild said measurement devices - pub fn set_devices( - &mut self, - devices: Vec, - configs: BTreeMap, - ) -> Result<(), Box> - where - MsrIn: Interpolatable, - D: TrackingDeviceSim, - DefaultAllocator: Allocator<::Size> - + Allocator<::Size, ::Size> - + Allocator<::VecLength>, - { - let mut devices_map = BTreeMap::new(); - let mut sampling_rates_ns = Vec::with_capacity(devices.len()); - for device in devices { - if let Some(cfg) = configs.get(&device.name()) { - if let Err(e) = cfg.sanity_check() { - warn!("Ignoring device {}: {e}", device.name()); - continue; - } - sampling_rates_ns.push(cfg.sampling.truncated_nanoseconds()); - } else { - warn!( - "Ignoring device {}: no associated tracking configuration", - device.name() - ); - continue; - } - devices_map.insert(device.name(), device); - } - - if devices_map.is_empty() { - return Err(Box::new(ConfigError::InvalidConfig { - msg: "None of the devices are properly configured".to_string(), - })); - } - - self.device_cfg = serde_yaml::to_string(&devices_map).unwrap(); - - Ok(()) - } -} diff --git a/src/od/msr/data_arc.rs b/src/od/msr/data_arc.rs new file mode 100644 index 00000000..031aef5c --- /dev/null +++ b/src/od/msr/data_arc.rs @@ -0,0 +1,471 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ +use super::{measurement::Measurement, MeasurementType}; +use crate::io::watermark::pq_writer; +use crate::io::{ArrowSnafu, InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu}; +use crate::io::{EmptyDatasetSnafu, ExportCfg}; +use arrow::array::{Array, Float64Builder, StringBuilder}; +use arrow::datatypes::{DataType, Field, Schema}; +use arrow::record_batch::RecordBatch; +use arrow::{ + array::{Float64Array, PrimitiveArray, StringArray}, + datatypes, + record_batch::RecordBatchReader, +}; +use core::fmt; +use hifitime::prelude::{Duration, Epoch}; +use hifitime::TimeScale; +use indexmap::IndexSet; +use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; +use parquet::arrow::ArrowWriter; +use snafu::{ensure, ResultExt}; +use std::collections::{BTreeMap, HashMap}; +use std::error::Error; +use std::fs::File; +use std::ops::Bound::{Excluded, Included, Unbounded}; +use std::ops::RangeBounds; +use std::path::{Path, PathBuf}; +use std::sync::Arc; + +/// Tracking data storing all of measurements as a B-Tree. +#[derive(Clone, Default)] +pub struct TrackingDataArc { + /// All measurements in this data arc + pub measurements: BTreeMap, + /// Source file if loaded from a file or saved to a file. + pub source: Option, +} + +impl TrackingDataArc { + /// Loads a tracking arc from its serialization in parquet. + pub fn from_parquet>(path: P) -> Result { + // Read the file since we closed it earlier + let file = File::open(&path).context(StdIOSnafu { + action: "opening file for tracking arc", + })?; + let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap(); + + let reader = builder.build().context(ParquetSnafu { + action: "reading tracking arc", + })?; + + // Check the schema + let mut has_epoch = false; + let mut has_tracking_dev = false; + let mut range_avail = false; + let mut doppler_avail = false; + let mut az_avail = false; + let mut el_avail = false; + for field in &reader.schema().fields { + match field.name().as_str() { + "Epoch (UTC)" => has_epoch = true, + "Tracking device" => has_tracking_dev = true, + "Range (km)" => range_avail = true, + "Doppler (km/s)" => doppler_avail = true, + "Azimuth (deg)" => az_avail = true, + "Elevation (deg)" => el_avail = true, + _ => {} + } + } + + ensure!( + has_epoch, + MissingDataSnafu { + which: "Epoch (UTC)" + } + ); + + ensure!( + has_tracking_dev, + MissingDataSnafu { + which: "Tracking device" + } + ); + + ensure!( + range_avail || doppler_avail || az_avail || el_avail, + MissingDataSnafu { + which: "`Range (km)` or `Doppler (km/s)` or `Azimuth (deg)` or `Elevation (deg)`" + } + ); + + let mut measurements = BTreeMap::new(); + + // We can safely unwrap the columns since we've checked for their existance just before. + for maybe_batch in reader { + let batch = maybe_batch.context(ArrowSnafu { + action: "reading batch of tracking data", + })?; + + let tracking_device = batch + .column_by_name("Tracking device") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let epochs = batch + .column_by_name("Epoch (UTC)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(); + + let range_data: Option<&PrimitiveArray> = if range_avail { + Some( + batch + .column_by_name("Range (km)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + let doppler_data: Option<&PrimitiveArray> = if doppler_avail { + Some( + batch + .column_by_name("Doppler (km/s)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + let azimuth_data: Option<&PrimitiveArray> = if az_avail { + Some( + batch + .column_by_name("Azimuth (deg)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + let elevation_data: Option<&PrimitiveArray> = if el_avail { + Some( + batch + .column_by_name("Elevation (deg)") + .unwrap() + .as_any() + .downcast_ref::() + .unwrap(), + ) + } else { + None + }; + + // Set the measurements in the tracking arc + for i in 0..batch.num_rows() { + let epoch = Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?; + + let mut measurement = Measurement { + epoch, + tracker: tracking_device.value(i).to_string(), + data: HashMap::new(), + }; + + if range_avail { + measurement + .data + .insert(MeasurementType::Range, range_data.unwrap().value(i)); + } + + if doppler_avail { + measurement + .data + .insert(MeasurementType::Doppler, doppler_data.unwrap().value(i)); + } + + if az_avail { + measurement + .data + .insert(MeasurementType::Azimuth, azimuth_data.unwrap().value(i)); + } + + if el_avail { + measurement + .data + .insert(MeasurementType::Elevation, elevation_data.unwrap().value(i)); + } + + measurements.insert(epoch, measurement); + } + } + + Ok(Self { + measurements, + source: Some(path.as_ref().to_path_buf().display().to_string()), + }) + } + + /// Returns the unique list of aliases in this tracking data arc + pub fn unique_aliases(&self) -> IndexSet { + self.unique().0 + } + + /// Returns the unique measurement types in this tracking data arc + pub fn unique_types(&self) -> IndexSet { + self.unique().1 + } + + /// Returns the unique trackers and unique measurement types in this data arc + pub fn unique(&self) -> (IndexSet, IndexSet) { + let mut aliases = IndexSet::new(); + let mut types = IndexSet::new(); + for msr in self.measurements.values() { + aliases.insert(msr.tracker.clone()); + for k in msr.data.keys() { + types.insert(*k); + } + } + (aliases, types) + } + + /// Returns the start epoch of this tracking arc + pub fn start_epoch(&self) -> Option { + self.measurements.first_key_value().map(|(k, _)| *k) + } + + /// Returns the end epoch of this tracking arc + pub fn end_epoch(&self) -> Option { + self.measurements.last_key_value().map(|(k, _)| *k) + } + + /// Returns the number of measurements in this data arc + pub fn len(&self) -> usize { + self.measurements.len() + } + + /// Returns whether this arc has no measurements. + pub fn is_empty(&self) -> bool { + self.measurements.is_empty() + } + + /// Returns the minimum duration between two subsequent measurements. + /// This is important to correctly set up the propagator and not miss any measurement. + pub fn min_duration_sep(&self) -> Option { + if self.is_empty() { + None + } else { + let mut min_sep = Duration::MAX; + let mut prev_epoch = self.start_epoch().unwrap(); + for (epoch, _) in self.measurements.iter().skip(1) { + let this_sep = *epoch - prev_epoch; + min_sep = min_sep.min(this_sep); + prev_epoch = *epoch; + } + Some(min_sep) + } + } + + /// Returns a new tracking arc that only contains measurements that fall within the given epoch range. + pub fn filter_by_epoch>(mut self, bound: R) -> Self { + self.measurements = self + .measurements + .range(bound) + .map(|(epoch, msr)| (*epoch, msr.clone())) + .collect::>(); + self + } + + /// Returns a new tracking arc that only contains measurements that fall within the given offset from the first epoch + pub fn filter_by_offset>(self, bound: R) -> Self { + if self.is_empty() { + return self; + } + // Rebuild an epoch bound. + let start = match bound.start_bound() { + Unbounded => self.start_epoch().unwrap(), + Included(offset) | Excluded(offset) => self.start_epoch().unwrap() + *offset, + }; + + let end = match bound.end_bound() { + Unbounded => self.end_epoch().unwrap(), + Included(offset) | Excluded(offset) => self.end_epoch().unwrap() - *offset, + }; + + self.filter_by_epoch(start..end) + } + + /// Store this tracking arc to a parquet file. + pub fn to_parquet_simple>(&self, path: P) -> Result> { + self.to_parquet(path, ExportCfg::default()) + } + + /// Store this tracking arc to a parquet file, with optional metadata and a timestamp appended to the filename. + pub fn to_parquet>( + &self, + path: P, + cfg: ExportCfg, + ) -> Result> { + ensure!( + !self.is_empty(), + EmptyDatasetSnafu { + action: "exporting tracking data arc" + } + ); + + let path_buf = cfg.actual_path(path); + + if cfg.step.is_some() { + warn!("The `step` parameter in the export is not supported for tracking arcs."); + } + + if cfg.fields.is_some() { + warn!("The `fields` parameter in the export is not supported for tracking arcs."); + } + + // Build the schema + let mut hdrs = vec![ + Field::new("Epoch (UTC)", DataType::Utf8, false), + Field::new("Tracking device", DataType::Utf8, false), + ]; + + let msr_types = self.unique_types(); + let mut msr_fields = msr_types + .iter() + .map(|msr_type| msr_type.to_field()) + .collect::>(); + + hdrs.append(&mut msr_fields); + + // Build the schema + let schema = Arc::new(Schema::new(hdrs)); + let mut record: Vec> = Vec::new(); + + // Build the measurement iterator + + let measurements = + if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { + let start = cfg + .start_epoch + .unwrap_or_else(|| self.start_epoch().unwrap()); + let end = cfg.end_epoch.unwrap_or_else(|| self.end_epoch().unwrap()); + + info!("Exporting measurements from {start} to {end}."); + + self.measurements + .range(start..end) + .map(|(k, v)| (*k, v.clone())) + .collect() + } else { + self.measurements.clone() + }; + + // Build all of the records + + // Epochs + let mut utc_epoch = StringBuilder::new(); + for epoch in measurements.keys() { + utc_epoch.append_value(epoch.to_time_scale(TimeScale::UTC).to_isoformat()); + } + record.push(Arc::new(utc_epoch.finish())); + + // Device names + let mut device_names = StringBuilder::new(); + for m in measurements.values() { + device_names.append_value(m.tracker.clone()); + } + record.push(Arc::new(device_names.finish())); + + // Measurement data, column by column + for msr_type in msr_types { + let mut data_builder = Float64Builder::new(); + + for m in measurements.values() { + match m.data.get(&msr_type) { + Some(value) => data_builder.append_value(*value), + None => data_builder.append_null(), + }; + } + record.push(Arc::new(data_builder.finish())); + } + + // Serialize all of the devices and add that to the parquet file too. + let mut metadata = HashMap::new(); + metadata.insert("Purpose".to_string(), "Tracking Arc Data".to_string()); + if let Some(add_meta) = cfg.metadata { + for (k, v) in add_meta { + metadata.insert(k, v); + } + } + + let props = pq_writer(Some(metadata)); + + let file = File::create(&path_buf)?; + + let mut writer = ArrowWriter::try_new(file, schema.clone(), props).unwrap(); + + let batch = RecordBatch::try_new(schema, record)?; + writer.write(&batch)?; + writer.close()?; + + info!("Serialized {self} to {}", path_buf.display()); + + // Return the path this was written to + Ok(path_buf) + } +} + +impl fmt::Display for TrackingDataArc { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + if self.is_empty() { + write!(f, "Empty tracking arc") + } else { + let start = self.start_epoch().unwrap(); + let end = self.end_epoch().unwrap(); + let src = match &self.source { + Some(src) => format!(" (source: {src})"), + None => String::new(), + }; + write!( + f, + "Tracking arc with {} measurements of type {:?} over {} (from {start} to {end}) with trackers {:?}{src}", + self.len(), + self.unique_types(), + end - start, + self.unique_aliases() + ) + } + } +} + +impl fmt::Debug for TrackingDataArc { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{self} @ {self:p}") + } +} + +impl PartialEq for TrackingDataArc { + fn eq(&self, other: &Self) -> bool { + self.measurements == other.measurements + } +} diff --git a/src/od/msr/measurement.rs b/src/od/msr/measurement.rs new file mode 100644 index 00000000..9a03792f --- /dev/null +++ b/src/od/msr/measurement.rs @@ -0,0 +1,94 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use super::MeasurementType; +use hifitime::Epoch; +use indexmap::IndexSet; +use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OVector}; +use std::collections::HashMap; +use std::fmt; + +/// A type-agnostic simultaneous measurement storage structure. Allows storing any number of simultaneous measurement of a given taker. +#[derive(Clone, Debug, PartialEq)] +pub struct Measurement { + /// Tracker alias which made this measurement + pub tracker: String, + /// Epoch of the measurement + pub epoch: Epoch, + /// All measurements made simultaneously + pub data: HashMap, +} + +impl Measurement { + pub fn new(tracker: String, epoch: Epoch) -> Self { + Self { + tracker, + epoch, + data: HashMap::new(), + } + } + + pub fn push(&mut self, msr_type: MeasurementType, msr_value: f64) { + self.data.insert(msr_type, msr_value); + } + + pub fn with(mut self, msr_type: MeasurementType, msr_value: f64) -> Self { + self.push(msr_type, msr_value); + self + } + + /// Builds an observation vector for this measurement provided a set of measurement types. + /// If the requested measurement type is not available, then that specific row is set to zero. + /// The caller must set the appropriate sensitivity matrix rows to zero. + pub fn observation(&self, types: &IndexSet) -> OVector + where + DefaultAllocator: Allocator, + { + let mut obs = OVector::zeros(); + for (i, t) in types.iter().enumerate() { + if let Some(msr_value) = self.data.get(t) { + obs[i] = *msr_value; + } + } + obs + } + + /// Returns a vector specifying which measurement types are available. + pub fn availability(&self, types: &IndexSet) -> Vec { + let mut rtn = Vec::with_capacity(types.len()); + for (i, t) in types.iter().enumerate() { + if self.data.contains_key(t) { + rtn[i] = true; + } + } + rtn + } +} + +impl fmt::Display for Measurement { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let msrs = self + .data + .iter() + .map(|(msr_type, msr_value)| format!("{msr_type:?} = {msr_value} {}", msr_type.unit())) + .collect::>() + .join(", "); + + write!(f, "{} measured {} on {}", self.tracker, msrs, self.epoch) + } +} diff --git a/src/od/msr/mod.rs b/src/od/msr/mod.rs index 8b216d60..61d2414f 100644 --- a/src/od/msr/mod.rs +++ b/src/od/msr/mod.rs @@ -16,12 +16,11 @@ along with this program. If not, see . */ -mod arc; -mod range; -mod range_doppler; -mod rangerate; +mod data_arc; +pub mod measurement; +pub mod sensitivity; +mod types; -pub use arc::TrackingArc; -pub use range::RangeMsr; -pub use range_doppler::RangeDoppler; -pub use rangerate::RangeRate; +pub use data_arc::TrackingDataArc; +pub use measurement::Measurement; +pub use types::MeasurementType; diff --git a/src/od/msr/range.rs b/src/od/msr/range.rs deleted file mode 100644 index 1828f61e..00000000 --- a/src/od/msr/range.rs +++ /dev/null @@ -1,131 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::{DimName, Matrix1x6, OVector, Vector1, U1, U6, U7}; -use crate::od::Measurement; -use crate::time::Epoch; -use crate::TimeTagged; -use arrow::datatypes::{DataType, Field}; -use hyperdual::linalg::norm; -use hyperdual::{hyperspace_from_vector, OHyperdual}; -use serde::ser::SerializeSeq; -use serde::{Serialize, Serializer}; -use std::collections::HashMap; - -/// Stores a standard measurement of range (km) -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeMsr { - pub dt: Epoch, - pub obs: Vector1, - visible: bool, - h_tilde: Matrix1x6, -} - -impl RangeMsr { - pub fn range(&self) -> f64 { - self.obs[(0, 0)] - } - - fn compute_sensitivity( - state: &OVector, U6>, - ) -> (Vector1, Matrix1x6) { - // Extract data from hyperspace - let range_vec = state.fixed_rows::<3>(0).into_owned(); - - // Code up math as usual - let range = norm(&range_vec); - - // Extract result into Vector2 and Matrix2x6 - let fx = Vector1::new(range.real()); - let mut pmat = Matrix1x6::zeros(); - - for j in 1..U7::dim() { - pmat[j - 1] = range[j]; - } - - (fx, pmat) - } - - pub fn new(tx: Orbit, rx: Orbit, visible: bool) -> RangeMsr { - assert_eq!(tx.frame, rx.frame, "tx and rx in different frames"); - assert_eq!(tx.epoch, rx.epoch, "tx and rx states have different times"); - - let dt = tx.epoch; - let hyperstate = - hyperspace_from_vector(&(rx.to_cartesian_pos_vel() - tx.to_cartesian_pos_vel())); - let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate); - - RangeMsr { - dt, - obs, - visible, - h_tilde, - } - } -} - -impl Measurement for RangeMsr { - type MeasurementSize = U1; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km - fn observation(&self) -> Vector1 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km".to_string()); - vec![Field::new("Range (km)", DataType::Float64, false).with_metadata(meta)] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { - dt: epoch, - obs, - visible: true, - h_tilde: Matrix1x6::zeros(), - } - } -} - -impl TimeTagged for RangeMsr { - fn epoch(&self) -> Epoch { - self.dt - } - - fn set_epoch(&mut self, dt: Epoch) { - self.dt = dt - } -} - -impl Serialize for RangeMsr { - /// Serializes the observation vector at the given time. - fn serialize(&self, serializer: S) -> Result - where - S: Serializer, - { - let mut seq = serializer.serialize_seq(Some(3))?; - seq.serialize_element(&self.dt.to_mjd_tai_days())?; - let obs = self.observation(); - seq.serialize_element(&obs[(0, 0)])?; - seq.end() - } -} diff --git a/src/od/msr/range_doppler.rs b/src/od/msr/range_doppler.rs deleted file mode 100644 index aa57f53b..00000000 --- a/src/od/msr/range_doppler.rs +++ /dev/null @@ -1,175 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, OMatrix, OVector, Vector2, U2}; -use crate::od::msr::RangeMsr; -use crate::od::{EstimateFrom, Measurement}; -use crate::{Spacecraft, TimeTagged}; -use anise::astro::AzElRange; -use arrow::datatypes::{DataType, Field}; -use hifitime::{Epoch, Unit}; -use std::collections::HashMap; - -/// A simultaneous range and Doppler measurement in units of km and km/s, available both in one way and two way measurement. -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeDoppler { - /// Epoch of the observation - pub epoch: Epoch, - /// Observation vector in km and km/s - pub obs: Vector2, -} - -impl RangeDoppler { - /// Initialize a new one-way range and Doppler measurement from the provided states and the effective noises. - /// - /// # Panics - /// + If the epochs of the two states differ. - /// + If the frames of the two states differ. - pub fn one_way( - aer: AzElRange, - timestamp_noise_s: f64, - range_noise_km: f64, - doppler_noise_km_s: f64, - ) -> Self { - Self { - epoch: aer.epoch + timestamp_noise_s * Unit::Second, - obs: Vector2::new( - aer.range_km + range_noise_km, - aer.range_rate_km_s + doppler_noise_km_s, - ), - } - } - - /// Initialize a new two-way range and Doppler measurement from the provided states as times t_1 and t_2 and the effective noises. - /// - /// The measurement is time-tagged at realization, i.e. at the end of the integration time (plus timestamp noise). - /// - /// # Noise - /// The measurements are not considered to be independent distributed variables. As such, the noises are reduced by a factor of sqrt(2). - /// - /// # Panics - /// + If the epochs of the two states differ. - /// + If the frames of the two states differ. - /// + If both epochs are identical. - pub fn two_way( - aer_t0: AzElRange, - aer_t1: AzElRange, - timestamp_noise_s: f64, - range_noise_km: f64, - doppler_noise_km_s: f64, - ) -> Self { - if aer_t0.epoch == aer_t1.epoch { - return Self::one_way( - aer_t1, - timestamp_noise_s, - range_noise_km, - doppler_noise_km_s, - ); - } - - let range_km = (aer_t1.range_km + aer_t0.range_km) * 0.5; - let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; - - // Time tagged at the realization of this measurement, i.e. at the end of the integration time. - let epoch = aer_t1.epoch + timestamp_noise_s * Unit::Second; - - let obs = Vector2::new( - range_km + range_noise_km / 2.0_f64.sqrt(), - doppler_km_s + doppler_noise_km_s / 2.0_f64.sqrt(), - ); - - debug!( - "two way msr @ {epoch}:\naer_t0 = {}\naer_t1 = {}{obs}", - aer_t0, aer_t1 - ); - - Self { epoch, obs } - } -} - -impl TimeTagged for RangeDoppler { - fn epoch(&self) -> Epoch { - self.epoch - } - - fn set_epoch(&mut self, epoch: Epoch) { - self.epoch = epoch - } -} - -impl Measurement for RangeDoppler { - type MeasurementSize = U2; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km, km/s - fn observation(&self) -> Vector2 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km/s".to_string()); - - vec![ - RangeMsr::fields()[0].clone(), - Field::new("Doppler (km/s)", DataType::Float64, false).with_metadata(meta), - ] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { epoch, obs } - } -} - -impl EstimateFrom for Spacecraft { - fn extract(from: Spacecraft) -> Self { - from - } - - fn sensitivity( - msr: &RangeDoppler, - receiver: Self, - transmitter: Orbit, - ) -> OMatrix::MeasurementSize, Self::Size> - where - DefaultAllocator: Allocator<::MeasurementSize, Self::Size>, - { - let delta_r = receiver.orbit.radius_km - transmitter.radius_km; - let delta_v = receiver.orbit.velocity_km_s - transmitter.velocity_km_s; - let ρ = msr.observation()[0]; - let ρ_dot = msr.observation()[1]; - let m11 = delta_r.x / ρ; - let m12 = delta_r.y / ρ; - let m13 = delta_r.z / ρ; - let m21 = delta_v.x / ρ - ρ_dot * delta_r.x / ρ.powi(2); - let m22 = delta_v.y / ρ - ρ_dot * delta_r.y / ρ.powi(2); - let m23 = delta_v.z / ρ - ρ_dot * delta_r.z / ρ.powi(2); - - let items = &[ - m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, m21, m22, m23, m11, m12, m13, 0.0, 0.0, - 0.0, - ]; - - OMatrix::::MeasurementSize, Self::Size>::from_row_slice( - items, - ) - } -} diff --git a/src/od/msr/rangerate.rs b/src/od/msr/rangerate.rs deleted file mode 100644 index e2fbaaa4..00000000 --- a/src/od/msr/rangerate.rs +++ /dev/null @@ -1,132 +0,0 @@ -/* - Nyx, blazing fast astrodynamics - Copyright (C) 2018-onwards Christopher Rabotin - - 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 . -*/ - -use crate::cosmic::Orbit; -use crate::linalg::{DimName, Matrix1x6, OVector, Vector1, U1, U6, U7}; -use crate::od::Measurement; -use crate::time::Epoch; -use crate::TimeTagged; -use arrow::datatypes::{DataType, Field}; -use hyperdual::linalg::norm; -use hyperdual::{hyperspace_from_vector, OHyperdual}; -use serde::ser::SerializeSeq; -use serde::{Serialize, Serializer}; -use std::collections::HashMap; - -/// Stores a standard measurement of range (km) -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct RangeRate { - pub dt: Epoch, - pub obs: Vector1, - visible: bool, - h_tilde: Matrix1x6, -} - -impl RangeRate { - pub fn range_rate(&self) -> f64 { - self.obs[(0, 0)] - } - - fn compute_sensitivity( - state: &OVector, U6>, - ) -> (Vector1, Matrix1x6) { - // Extract data from hyperspace - let range_vec = state.fixed_rows::<3>(0).into_owned(); - let velocity_vec = state.fixed_rows::<3>(3).into_owned(); - - // Code up math as usual - let delta_v_vec = velocity_vec / norm(&range_vec); - let range_rate = range_vec.dot(&delta_v_vec); - - // Extract result into Vector2 and Matrix2x6 - let fx = Vector1::new(range_rate.real()); - let mut pmat = Matrix1x6::zeros(); - - for j in 1..U7::dim() { - pmat[j - 1] = range_rate[j]; - } - (fx, pmat) - } - - pub fn new(tx: Orbit, rx: Orbit, visible: bool) -> RangeRate { - assert_eq!(tx.frame, rx.frame, "tx and rx in different frames"); - assert_eq!(tx.epoch, rx.epoch, "tx and rx states have different times"); - - let dt = tx.epoch; - let hyperstate = - hyperspace_from_vector(&(rx.to_cartesian_pos_vel() - tx.to_cartesian_pos_vel())); - let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate); - - RangeRate { - dt, - obs, - visible, - h_tilde, - } - } -} - -impl Measurement for RangeRate { - type MeasurementSize = U1; - - /// Returns this measurement as a vector of Range and Range Rate - /// - /// **Units:** km/s - fn observation(&self) -> Vector1 { - self.obs - } - - fn fields() -> Vec { - let mut meta = HashMap::new(); - meta.insert("unit".to_string(), "km/s".to_string()); - vec![Field::new("Doppler (km/s)", DataType::Float64, false).with_metadata(meta)] - } - - fn from_observation(epoch: Epoch, obs: OVector) -> Self { - Self { - dt: epoch, - obs, - visible: true, - h_tilde: Matrix1x6::zeros(), - } - } -} - -impl TimeTagged for RangeRate { - fn epoch(&self) -> Epoch { - self.dt - } - - fn set_epoch(&mut self, dt: Epoch) { - self.dt = dt - } -} - -impl Serialize for RangeRate { - /// Serializes the observation vector at the given time. - fn serialize(&self, serializer: S) -> Result - where - S: Serializer, - { - let mut seq = serializer.serialize_seq(Some(3))?; - seq.serialize_element(&self.dt.to_mjd_tai_days())?; - let obs = self.observation(); - seq.serialize_element(&obs[(0, 0)])?; - seq.end() - } -} diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs new file mode 100644 index 00000000..d42688da --- /dev/null +++ b/src/od/msr/sensitivity.rs @@ -0,0 +1,234 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use crate::linalg::allocator::Allocator; +use crate::linalg::DefaultAllocator; +use crate::md::prelude::Interpolatable; +use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice}; +use crate::{Spacecraft, State}; +use anise::prelude::Almanac; +use indexmap::IndexSet; +use nalgebra::{DimName, OMatrix, U1}; +use snafu::ResultExt; +use std::marker::PhantomData; +use std::sync::Arc; + +use super::measurement::Measurement; +use super::MeasurementType; + +trait ScalarSensitivityT +where + Self: Sized, + DefaultAllocator: Allocator + + Allocator + + Allocator, +{ + fn new( + msr_type: MeasurementType, + msr: &Measurement, + rx: &Rx, + tx: &Tx, + almanac: Arc, + ) -> Result; +} + +/// Trait required to build a triplet of a solve-for state, a receiver, and a transmitter. +pub trait TrackerSensitivity: TrackingDevice +where + Self: Sized, + DefaultAllocator: Allocator + + Allocator + + Allocator, +{ + /// Returns the sensitivity matrix of size MxS where M is the number of simultaneous measurements + /// and S is the size of the state being solved for. + fn h_tilde( + &self, + msr: &Measurement, + msr_types: &IndexSet, // Consider switching to array + rx: &Rx, + almanac: Arc, + ) -> Result, ODError> + where + DefaultAllocator: Allocator + Allocator; +} + +struct ScalarSensitivity +where + DefaultAllocator: Allocator + + Allocator + + Allocator + + Allocator, +{ + sensitivity_row: OMatrix, + _rx: PhantomData, + _tx: PhantomData, +} + +impl TrackerSensitivity for GroundStation +where + DefaultAllocator: Allocator<::Size> + + Allocator<::VecLength> + + Allocator<::Size, ::Size>, +{ + fn h_tilde( + &self, + msr: &Measurement, + msr_types: &IndexSet, + rx: &Spacecraft, + almanac: Arc, + ) -> Result::Size>, ODError> + where + DefaultAllocator: Allocator + Allocator::Size>, + { + // Rebuild each row of the scalar sensitivities. + let mut mat = OMatrix::::Size>::identity(); + for (ith_row, msr_type) in msr_types.iter().enumerate() { + if !msr.data.contains_key(msr_type) { + // Skip computation, this row is zero anyway. + continue; + } + let scalar_h = + as ScalarSensitivityT< + Spacecraft, + Spacecraft, + GroundStation, + >>::new(*msr_type, msr, rx, self, almanac.clone())?; + + mat.set_row(ith_row, &scalar_h.sensitivity_row); + } + Ok(mat) + } +} + +impl ScalarSensitivityT + for ScalarSensitivity +{ + fn new( + msr_type: MeasurementType, + msr: &Measurement, + rx: &Spacecraft, + tx: &GroundStation, + almanac: Arc, + ) -> Result { + let receiver = rx.orbit; + + // Compute the device location in the receiver frame because we compute the sensitivity in that frame. + // This frame is required because the scalar measurements are frame independent, but the sensitivity + // must be in the estimation frame. + let transmitter = tx + .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone()) + .context(ODAlmanacSnafu { + action: "computing transmitter location when computing sensitivity matrix", + })?; + + let delta_r = receiver.radius_km - transmitter.radius_km; + let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s; + + match msr_type { + MeasurementType::Doppler => { + // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range. + let ρ_km = match msr.data.get(&MeasurementType::Range) { + Some(range_km) => *range_km, + None => { + tx.azimuth_elevation_of(receiver, None, &almanac) + .context(ODAlmanacSnafu { + action: "computing range for Doppler measurement", + })? + .range_km + } + }; + + let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).unwrap(); + let m11 = delta_r.x / ρ_km; + let m12 = delta_r.y / ρ_km; + let m13 = delta_r.z / ρ_km; + let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2); + let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2); + let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2); + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + MeasurementType::Range => { + let ρ_km = msr.data.get(&MeasurementType::Range).unwrap(); + let m11 = delta_r.x / ρ_km; + let m12 = delta_r.y / ρ_km; + let m13 = delta_r.z / ρ_km; + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + MeasurementType::Azimuth => { + let denom = delta_r.x.powi(2) + delta_r.y.powi(2); + let m11 = -delta_r.y / denom; + let m12 = delta_r.x / denom; + let m13 = 0.0; + + // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + MeasurementType::Elevation => { + let r2 = delta_r.norm().powi(2); + let z2 = delta_r.z.powi(2); + + // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame. + let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - z2).sqrt()); + let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - z2).sqrt()); + let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2; + + let sensitivity_row = + OMatrix::::Size>::from_row_slice(&[ + m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + ]); + + Ok(Self { + sensitivity_row, + _rx: PhantomData::<_>, + _tx: PhantomData::<_>, + }) + } + } + } +} diff --git a/src/od/msr/types.rs b/src/od/msr/types.rs new file mode 100644 index 00000000..79c46059 --- /dev/null +++ b/src/od/msr/types.rs @@ -0,0 +1,99 @@ +/* + Nyx, blazing fast astrodynamics + Copyright (C) 2018-onwards Christopher Rabotin + + 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 . +*/ + +use anise::astro::AzElRange; +use arrow::datatypes::{DataType, Field}; +use serde_derive::{Deserialize, Serialize}; +use std::collections::HashMap; + +use crate::od::ODError; + +#[derive(Copy, Clone, Debug, Hash, Serialize, Deserialize, PartialEq, Eq)] +pub enum MeasurementType { + #[serde(rename = "range_km")] + Range, + #[serde(rename = "doppler_km_s")] + Doppler, + #[serde(rename = "azimuth_deg")] + Azimuth, + #[serde(rename = "elevation_deg")] + Elevation, +} + +impl MeasurementType { + /// Returns the expected unit of this measurement type + pub fn unit(self) -> &'static str { + match self { + Self::Range => "km", + Self::Doppler => "km/s", + Self::Azimuth | Self::Elevation => "deg", + } + } + + /// Returns the fields for this kind of measurement. The metadata includes a `unit` field with the unit. + /// Column is nullable in case there is no such measurement at a given epoch. + pub fn to_field(&self) -> Field { + let mut meta = HashMap::new(); + meta.insert("unit".to_string(), self.unit().to_string()); + + Field::new( + format!("{self:?} ({})", self.unit()), + DataType::Float64, + true, + ) + .with_metadata(meta) + } + + /// Computes the one way measurement from an AER object and the noise of this measurement type, returned in the units of this measurement type. + pub fn compute_one_way(self, aer: AzElRange, noise: f64) -> Result { + match self { + Self::Range => Ok(aer.range_km + noise), + Self::Doppler => Ok(aer.range_rate_km_s + noise), + Self::Azimuth => Ok(aer.azimuth_deg + noise), + Self::Elevation => Ok(aer.elevation_deg + noise), + } + } + + /// Computes the two way measurement from two AER values and the noise of this measurement type, returned in the units of this measurement type. + /// Two way is modeled by averaging the measurement in between both times, and adding the noise divided by sqrt(2). + pub fn compute_two_way( + self, + aer_t0: AzElRange, + aer_t1: AzElRange, + noise: f64, + ) -> Result { + match self { + Self::Range => { + let range_km = (aer_t1.range_km + aer_t0.range_km) * 0.5; + Ok(range_km + noise / 2.0_f64.sqrt()) + } + Self::Doppler => { + let doppler_km_s = (aer_t1.range_rate_km_s + aer_t0.range_rate_km_s) * 0.5; + Ok(doppler_km_s + noise / 2.0_f64.sqrt()) + } + Self::Azimuth => { + let az_deg = (aer_t1.azimuth_deg + aer_t0.azimuth_deg) * 0.5; + Ok(az_deg + noise / 2.0_f64.sqrt()) + } + Self::Elevation => { + let el_deg = (aer_t1.elevation_deg + aer_t0.elevation_deg) * 0.5; + Ok(el_deg + noise / 2.0_f64.sqrt()) + } + } + } +} diff --git a/src/od/noise/gauss_markov.rs b/src/od/noise/gauss_markov.rs index 1957e126..a004253a 100644 --- a/src/od/noise/gauss_markov.rs +++ b/src/od/noise/gauss_markov.rs @@ -291,7 +291,7 @@ impl GaussMarkov { for item in as_list.iter() { // Check that the item is a dictionary let next: Self = - serde_yaml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; + serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; selves.push(next); } Ok(selves) @@ -307,7 +307,7 @@ impl GaussMarkov { // Try to convert the underlying data match pyany_to_value(v_any) { Ok(value) => { - match serde_yaml::from_value(value) { + match serde_yml::from_value(value) { Ok(next) => selves.push(next), Err(_) => { // Maybe this was to be parsed in full as a single item @@ -412,15 +412,15 @@ mod ut_gm { #[test] fn serde_test() { - use serde_yaml; + use serde_yml; use std::env; use std::path::PathBuf; // Note that we set the initial bias to zero because it is not serialized. let gm = GaussMarkov::new(Duration::MAX, 0.1).unwrap(); - let serialized = serde_yaml::to_string(&gm).unwrap(); + let serialized = serde_yml::to_string(&gm).unwrap(); println!("{serialized}"); - let gm_deser: GaussMarkov = serde_yaml::from_str(&serialized).unwrap(); + let gm_deser: GaussMarkov = serde_yml::from_str(&serialized).unwrap(); assert_eq!(gm_deser, gm); let test_data: PathBuf = [ diff --git a/src/od/noise/mod.rs b/src/od/noise/mod.rs index 5d02eed9..b32d7332 100644 --- a/src/od/noise/mod.rs +++ b/src/od/noise/mod.rs @@ -93,6 +93,18 @@ impl StochasticNoise { } } + /// Default stochastic process for an angle measurement (azimuth or elevation) + /// Using the instrument bias as the white noise value, zero constant bias. + pub fn default_angle_deg() -> Self { + Self { + white_noise: Some(WhiteNoise { + sigma: 1.0e-2, // 0.01 deg + ..Default::default() + }), + bias: Some(GaussMarkov::default_range_km()), + } + } + /// Sample these stochastics pub fn sample(&mut self, epoch: Epoch, rng: &mut R) -> f64 { let mut sample = 0.0; diff --git a/src/od/process/export.rs b/src/od/process/export.rs index abff5ab0..92001e9c 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -16,6 +16,7 @@ along with this program. If not, see . */ +use crate::dynamics::SpacecraftDynamics; use crate::io::watermark::pq_writer; use crate::io::{ArrowSnafu, ExportCfg, ParquetSnafu, StdIOSnafu}; use crate::linalg::allocator::Allocator; @@ -30,48 +31,43 @@ use arrow::datatypes::{DataType, Field, Schema}; use arrow::record_batch::RecordBatch; use filter::kalman::KF; use hifitime::TimeScale; +use msr::sensitivity::TrackerSensitivity; +use msr::TrackingDataArc; use nalgebra::Const; use parquet::arrow::ArrowWriter; use snafu::prelude::*; use std::collections::HashMap; use std::fs::File; -use std::ops::Add; use std::path::{Path, PathBuf}; use super::ODProcess; -impl<'a, D: Dynamics, Msr: Measurement, A: DimName> - ODProcess<'a, D, Msr, A, Spacecraft, KF> +impl<'a, MsrSize: DimName, Accel: DimName, Trk: TrackerSensitivity> + ODProcess<'a, SpacecraftDynamics, MsrSize, Accel, KF, Trk> where - D::StateType: - Interpolatable + Add::Size>, Output = D::StateType>, - ::VecLength>>::Buffer: Send, - DefaultAllocator: Allocator<::Size> - + Allocator - + Allocator::Size> - + Allocator, Msr::MeasurementSize> + DefaultAllocator: Allocator + + Allocator::Size> + + Allocator, MsrSize> + Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator - + Allocator::Size> - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size> + + Allocator + + Allocator::Size> + + Allocator<::Size, MsrSize> + + Allocator + + Allocator + Allocator<::Size> + Allocator<::VecLength> + Allocator<::Size, ::Size> - + Allocator<::Size, A> - + Allocator::Size>, - Spacecraft: EstimateFrom, + + Allocator<::Size, Accel> + + Allocator::Size>, { /// Store the estimates and residuals in a parquet file - pub fn to_parquet>(&self, path: P, cfg: ExportCfg) -> Result { + pub fn to_parquet>( + &self, + arc: &TrackingDataArc, + path: P, + cfg: ExportCfg, + ) -> Result { ensure!( !self.estimates.is_empty(), TooFewMeasurementsSnafu { @@ -221,25 +217,25 @@ where // Add the fields of the residuals let mut msr_fields = Vec::new(); - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Prefit residual: {}", f.name())), + .with_name(format!("Prefit residual: {f:?} ({})", f.unit())), ); } - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Postfit residual: {}", f.name())), + .with_name(format!("Postfit residual: {f:?} ({})", f.unit())), ); } - for f in Msr::fields() { + for f in arc.unique_types() { msr_fields.push( - f.clone() + f.to_field() .with_nullable(true) - .with_name(format!("Measurement noise: {}", f.name())), + .with_name(format!("Measurement noise: {f:?} ({})", f.unit())), ); } @@ -264,7 +260,7 @@ where .end_epoch .unwrap_or_else(|| self.estimates.last().unwrap().state().epoch()); - let mut residuals: Vec>> = + let mut residuals: Vec>> = Vec::with_capacity(self.residuals.len()); let mut estimates = Vec::with_capacity(self.estimates.len()); @@ -358,11 +354,14 @@ where // Finally, add the residuals. // Prefits - for i in 0..Msr::MeasurementSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.prefit[i]); + match resid.prefit(msr_type) { + Some(prefit) => data.append_value(prefit), + None => data.append_null(), + }; } else { data.append_null(); } @@ -370,11 +369,14 @@ where record.push(Arc::new(data.finish())); } // Postfit - for i in 0..Msr::MeasurementSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.postfit[i]); + match resid.postfit(msr_type) { + Some(postfit) => data.append_value(postfit), + None => data.append_null(), + }; } else { data.append_null(); } @@ -382,11 +384,14 @@ where record.push(Arc::new(data.finish())); } // Measurement noise - for i in 0..Msr::MeasurementSize::dim() { + for msr_type in arc.unique_types() { let mut data = Float64Builder::new(); for resid_opt in &residuals { if let Some(resid) = resid_opt { - data.append_value(resid.tracker_msr_noise[i]); + match resid.trk_noise(msr_type) { + Some(noise) => data.append_value(noise), + None => data.append_null(), + }; } else { data.append_null(); } diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 6d75c4a9..48feef2a 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -26,13 +26,15 @@ pub use crate::od::*; use crate::propagators::PropInstance; pub use crate::time::{Duration, Unit}; use anise::prelude::Almanac; +use indexmap::IndexSet; +use msr::sensitivity::TrackerSensitivity; use snafu::prelude::*; mod conf; pub use conf::{IterationConf, SmoothingArc}; mod trigger; pub use trigger::EkfTrigger; mod rejectcrit; -use self::msr::TrackingArc; +use self::msr::TrackingDataArc; pub use self::rejectcrit::ResidRejectCrit; use std::collections::BTreeMap; use std::marker::PhantomData; @@ -44,84 +46,71 @@ mod export; pub struct ODProcess< 'a, D: Dynamics, - Msr: Measurement, - A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, > where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> - + Allocator<::Size> - + Allocator<::VecLength> + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { /// PropInstance used for the estimation pub prop: PropInstance<'a, D>, /// Kalman filter itself pub kf: K, + /// Tracking devices + pub devices: BTreeMap, /// Vector of estimates available after a pass pub estimates: Vec, /// Vector of residuals available after a pass - pub residuals: Vec>>, + pub residuals: Vec>>, pub ekf_trigger: Option, /// Residual rejection criteria allows preventing bad measurements from affecting the estimation. pub resid_crit: Option, pub almanac: Arc, init_state: D::StateType, - _marker: PhantomData, + _marker: PhantomData, } impl< 'a, D: Dynamics, - Msr: Measurement, - A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, - > ODProcess<'a, D, Msr, A, S, K> + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, + > ODProcess<'a, D, MsrSize, Accel, K, Trk> where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, ::Size> + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator::Size> + + Allocator + + Allocator<::Size, ::Size> + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { /// Initialize a new orbit determination process with an optional trigger to switch from a CKF to an EKF. pub fn new( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, ekf_trigger: Option, resid_crit: Option, almanac: Arc, @@ -130,13 +119,14 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), ekf_trigger, resid_crit, almanac, init_state, - _marker: PhantomData::, + _marker: PhantomData::, } } @@ -144,6 +134,7 @@ where pub fn ekf( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, trigger: EkfTrigger, resid_crit: Option, almanac: Arc, @@ -152,13 +143,14 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), ekf_trigger: Some(trigger), resid_crit, almanac, init_state, - _marker: PhantomData::, + _marker: PhantomData::, } } @@ -273,18 +265,11 @@ where } /// Allows iterating on the filter solution. Requires specifying a smoothing condition to know where to stop the smoothing. - pub fn iterate( + pub fn iterate_arc( &mut self, - measurements: &[(String, Msr)], - devices: &mut BTreeMap, - step_size: Duration, + arc: &TrackingDataArc, config: IterationConf, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - // TODO: Add ExportCfg to iterate and to process so the data can be exported as we process it. Consider a thread writing with channel for faster serialization. - + ) -> Result<(), ODError> { let mut best_rms = self.rms_residual_ratios(); let mut previous_rms = best_rms; let mut divergence_cnt = 0; @@ -318,12 +303,12 @@ where // Reset the propagator self.prop.state = self.init_state; // Empty the estimates and add the first smoothed estimate as the initial estimate - self.estimates = Vec::with_capacity(measurements.len().max(self.estimates.len())); - self.residuals = Vec::with_capacity(measurements.len().max(self.estimates.len())); + self.estimates = Vec::with_capacity(arc.measurements.len().max(self.estimates.len())); + self.residuals = Vec::with_capacity(arc.measurements.len().max(self.estimates.len())); self.kf.set_previous_estimate(&smoothed[0]); // And re-run the filter - self.process::(measurements, devices, step_size)?; + self.process_arc(arc)?; // Compute the new RMS let new_rms = self.rms_residual_ratios(); @@ -407,53 +392,6 @@ where Ok(()) } - /// Allows iterating on the filter solution. Requires specifying a smoothing condition to know where to stop the smoothing. - pub fn iterate_arc( - &mut self, - arc: &TrackingArc, - config: IterationConf, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - let mut devices = arc.rebuild_devices::().context(ODConfigSnafu)?; - - let measurements = &arc.measurements; - let step_size = match arc.min_duration_sep() { - Some(step_size) => step_size, - None => { - return Err(ODError::TooFewMeasurements { - action: "determine the minimum step step", - need: 2, - }) - } - }; - - self.iterate(measurements, &mut devices, step_size, config) - } - - /// Process the provided tracking arc for this orbit determination process. - #[allow(clippy::erasing_op)] - pub fn process_arc(&mut self, arc: &TrackingArc) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { - let mut devices = arc.rebuild_devices::().context(ODConfigSnafu)?; - - let measurements = &arc.measurements; - let step_size = match arc.min_duration_sep() { - Some(step_size) => step_size, - None => { - return Err(ODError::TooFewMeasurements { - action: "determining the minimum step size", - need: 2, - }) - } - }; - - self.process(measurements, &mut devices, step_size) - } - /// Process the provided measurements for this orbit determination process given the associated devices. /// /// # Argument details @@ -461,15 +399,8 @@ where /// + The name of all measurement devices must be present in the provided devices, i.e. the key set of `devices` must be a superset of the measurement device names present in the list. /// + The maximum step size to ensure we don't skip any measurements. #[allow(clippy::erasing_op)] - pub fn process( - &mut self, - measurements: &[(String, Msr)], - devices: &mut BTreeMap, - max_step: Duration, - ) -> Result<(), ODError> - where - Dev: TrackingDeviceSim, - { + pub fn process_arc(&mut self, arc: &TrackingDataArc) -> Result<(), ODError> { + let measurements = &arc.measurements; ensure!( measurements.len() >= 2, TooFewMeasurementsSnafu { @@ -478,12 +409,22 @@ where } ); + let max_step = match arc.min_duration_sep() { + Some(step_size) => step_size, + None => { + return Err(ODError::TooFewMeasurements { + action: "determining the minimum step size", + need: 2, + }) + } + }; + ensure!( !max_step.is_negative() && max_step != Duration::ZERO, StepSizeSnafu { step: max_step } ); - // Start by propagating the estimator (on the same thread). + // Start by propagating the estimator. let num_msrs = measurements.len(); // Update the step size of the navigation propagator if it isn't already fixed step @@ -491,7 +432,8 @@ where self.prop.set_step(max_step, false); } - let prop_time = measurements[num_msrs - 1].1.epoch() - self.kf.previous_estimate().epoch(); + // let prop_time = measurements[num_msrs - 1].1.epoch() - self.kf.previous_estimate().epoch(); + let prop_time = arc.end_epoch().unwrap() - self.kf.previous_estimate().epoch(); info!("Navigation propagating for a total of {prop_time} with step size {max_step}"); let mut epoch = self.prop.state.epoch(); @@ -501,23 +443,13 @@ where info!("Processing {num_msrs} measurements with covariance mapping"); // We'll build a trajectory of the estimated states. This will be used to compute the measurements. - let mut traj: Traj = Traj::new(); + let mut traj: Traj = Traj::new(); let mut msr_accepted_cnt: usize = 0; let tick = Epoch::now().unwrap(); - for (msr_cnt, (device_name, msr)) in measurements.iter().enumerate() { - let next_msr_epoch = msr.epoch(); - - for val in msr.observation().iter() { - ensure!( - val.is_finite(), - InvalidMeasurementSnafu { - epoch: next_msr_epoch, - val: *val - } - ); - } + for (msr_cnt, (epoch_ref, msr)) in measurements.iter().enumerate() { + let next_msr_epoch = *epoch_ref; // Advance the propagator loop { @@ -544,28 +476,27 @@ where .context(ODPropSnafu)?; for state in traj_covar.states { - traj.states.push(S::extract(state)); + // NOTE: At the time being, only spacecraft estimation is possible, and the trajectory will always be the exact state + // that was propagated. Even once ground station biases are estimated, these won't go through the propagator. + traj.states.push(state); } // Now that we've advanced the propagator, let's see whether we're at the time of the next measurement. // Extract the state and update the STM in the filter. - let nominal_state = S::extract(self.prop.state); + let nominal_state = self.prop.state; // Get the datetime and info needed to compute the theoretical measurement according to the model epoch = nominal_state.epoch(); // Perform a measurement update if nominal_state.epoch() == next_msr_epoch { // Get the computed observations - match devices.get_mut(device_name) { + match self.devices.get_mut(&msr.tracker) { Some(device) => { if let Some(computed_meas) = device.measure(epoch, &traj, None, self.almanac.clone())? { - // Grab the device location - let device_loc = device - .location(epoch, nominal_state.frame(), self.almanac.clone()) - .unwrap(); + let msr_types = device.measurement_types(); // Switch back from extended if necessary if let Some(trigger) = &mut self.ekf_trigger { @@ -575,61 +506,108 @@ where } } - let h_tilde = S::sensitivity(msr, nominal_state, device_loc); - - self.kf.update_h_tilde(h_tilde); + // Perform several measurement updates to ensure the desired dimensionality. + let windows = msr_types.len() / MsrSize::USIZE; + let mut msr_rejected = false; + for wno in 0..=windows { + let mut cur_msr_types = IndexSet::new(); + for msr_type in msr_types + .iter() + .copied() + .skip(wno * MsrSize::USIZE) + .take(MsrSize::USIZE) + { + cur_msr_types.insert(msr_type); + } - match self.kf.measurement_update( - nominal_state, - &msr.observation(), - &computed_meas.observation(), - device.measurement_covar(epoch)?, - self.resid_crit, - ) { - Ok((estimate, mut residual)) => { - debug!("processed msr #{msr_cnt} @ {epoch}"); + if cur_msr_types.is_empty() { + // We've processed all measurements. + break; + } - residual.tracker = Some(device.name()); + // Check that the observation is valid. + for val in + msr.observation::(&cur_msr_types).iter().copied() + { + ensure!( + val.is_finite(), + InvalidMeasurementSnafu { + epoch: next_msr_epoch, + val + } + ); + } - if !residual.rejected { - msr_accepted_cnt += 1; - } + let h_tilde = device + .h_tilde::( + msr, + &cur_msr_types, + &nominal_state, + self.almanac.clone(), + ) + .unwrap(); + + self.kf.update_h_tilde(h_tilde); + + match self.kf.measurement_update( + nominal_state, + &msr.observation(&cur_msr_types), + &computed_meas.observation(&cur_msr_types), + device.measurement_covar_matrix(&cur_msr_types, epoch)?, + self.resid_crit, + ) { + Ok((estimate, mut residual)) => { + debug!("processed measurement #{msr_cnt} for {cur_msr_types:?} @ {epoch} from {}", device.name()); + + residual.tracker = Some(device.name()); + residual.msr_types = cur_msr_types; + + if residual.rejected { + msr_rejected = true; + } - // Switch to EKF if necessary, and update the dynamics and such - // Note: we call enable_ekf first to ensure that the trigger gets - // called in case it needs to save some information (e.g. the - // StdEkfTrigger needs to store the time of the previous measurement). - - if let Some(trigger) = &mut self.ekf_trigger { - if trigger.enable_ekf(&estimate) - && !self.kf.is_extended() - { - self.kf.set_extended(true); - if !estimate.within_3sigma() { - warn!("EKF enabled @ {epoch} but filter DIVERGING"); - } else { - info!("EKF enabled @ {epoch}"); + // Switch to EKF if necessary, and update the dynamics and such + // Note: we call enable_ekf first to ensure that the trigger gets + // called in case it needs to save some information (e.g. the + // StdEkfTrigger needs to store the time of the previous measurement). + + if let Some(trigger) = &mut self.ekf_trigger { + if trigger.enable_ekf(&estimate) + && !self.kf.is_extended() + { + self.kf.set_extended(true); + if !estimate.within_3sigma() { + warn!("EKF enabled @ {epoch} but filter DIVERGING"); + } else { + info!("EKF enabled @ {epoch}"); + } + } + if self.kf.is_extended() { + self.prop.state = self.prop.state + + estimate.state_deviation(); } } - if self.kf.is_extended() { - self.prop.state = - self.prop.state + estimate.state_deviation(); - } - } - self.prop.state.reset_stm(); + self.prop.state.reset_stm(); - self.estimates.push(estimate); - self.residuals.push(Some(residual)); + self.estimates.push(estimate); + self.residuals.push(Some(residual)); + } + Err(e) => return Err(e), } - Err(e) => return Err(e), + } + if !msr_rejected { + msr_accepted_cnt += 1; } } else { - warn!("Real observation exists @ {epoch} but simulated {device_name} does not see it -- ignoring measurement"); + warn!("Ignoring observation @ {epoch} because simulated {} does not expect it", msr.tracker); } } None => { - error!("Tracking arc references {device_name} which is not in the list of configured devices") + error!( + "Tracker {} is not in the list of configured devices", + msr.tracker + ) } } @@ -695,8 +673,7 @@ where // Perform time update // Extract the state and update the STM in the filter. - let prop_state = self.prop.state; - let nominal_state = S::extract(prop_state); + let nominal_state = self.prop.state; // Get the datetime and info needed to compute the theoretical measurement according to the model epoch = nominal_state.epoch(); // No measurement can be used here, let's just do a time update @@ -726,10 +703,9 @@ where } /// Builds the navigation trajectory for the estimated state only - pub fn to_traj(&self) -> Result, NyxError> + pub fn to_traj(&self) -> Result, NyxError> where - DefaultAllocator: Allocator<::VecLength>, - S: Interpolatable, + DefaultAllocator: Allocator<::VecLength>, { if self.estimates.is_empty() { Err(NyxError::NoStateData { @@ -751,38 +727,30 @@ where impl< 'a, D: Dynamics, - Msr: Measurement, - A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, - > ODProcess<'a, D, Msr, A, S, K> + MsrSize: DimName, + Accel: DimName, + K: Filter, + Trk: TrackerSensitivity, + > ODProcess<'a, D, MsrSize, Accel, K, Trk> where - D::StateType: Interpolatable + Add::Size>, Output = D::StateType>, + D::StateType: + Interpolatable + Add::Size>, Output = D::StateType>, ::VecLength>>::Buffer: Send, DefaultAllocator: Allocator<::Size> + Allocator<::VecLength> - + Allocator - + Allocator - + Allocator - + Allocator - + Allocator::Size> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator<::Size, Msr::MeasurementSize> - + Allocator::Size> + + Allocator + + Allocator::Size> + + Allocator + Allocator<::Size, ::Size> - + Allocator<::Size> - + Allocator<::VecLength> - + Allocator<::Size, ::Size> - + Allocator - + Allocator - + Allocator<::Size, A> - + Allocator::Size> - + Allocator<::Size, A> - + Allocator::Size>, + + Allocator + + Allocator + + Allocator<::Size, Accel> + + Allocator::Size>, { pub fn ckf( prop: PropInstance<'a, D>, kf: K, + devices: BTreeMap, resid_crit: Option, almanac: Arc, ) -> Self { @@ -790,13 +758,14 @@ where Self { prop: prop.quiet(), kf, + devices, estimates: Vec::with_capacity(10_000), residuals: Vec::with_capacity(10_000), resid_crit, ekf_trigger: None, init_state, almanac, - _marker: PhantomData::, + _marker: PhantomData::, } } } diff --git a/src/od/process/trigger.rs b/src/od/process/trigger.rs index e559db5a..f7c21ab8 100644 --- a/src/od/process/trigger.rs +++ b/src/od/process/trigger.rs @@ -24,6 +24,7 @@ use crate::linalg::DefaultAllocator; use crate::time::Duration; use crate::State; +#[derive(Copy, Clone, Debug)] /// An EkfTrigger on the number of measurements processed and a time between measurements. pub struct EkfTrigger { pub num_msrs: usize, diff --git a/src/od/simulator/arc.rs b/src/od/simulator/arc.rs index 6302ee33..caa59a60 100644 --- a/src/od/simulator/arc.rs +++ b/src/od/simulator/arc.rs @@ -25,13 +25,13 @@ use rand_pcg::Pcg64Mcg; use crate::dynamics::NyxError; use crate::io::ConfigError; use crate::md::trajectory::Interpolatable; -use crate::od::msr::{RangeDoppler, TrackingArc}; +use crate::od::msr::TrackingDataArc; use crate::od::prelude::Strand; use crate::od::simulator::Cadence; -use crate::od::{GroundStation, Measurement}; +use crate::od::GroundStation; use crate::Spacecraft; use crate::State; -use crate::{linalg::allocator::Allocator, od::TrackingDeviceSim}; +use crate::{linalg::allocator::Allocator, od::TrackingDevice}; use crate::{linalg::DefaultAllocator, md::prelude::Traj}; use std::collections::BTreeMap; use std::fmt::Display; @@ -41,18 +41,14 @@ use std::sync::Arc; use super::{Handoff, TrkConfig}; #[derive(Clone)] -pub struct TrackingArcSim +pub struct TrackingArcSim where - D: TrackingDeviceSim, + D: TrackingDevice, MsrIn: State, - Msr: Measurement, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { /// Map of devices from their names. pub devices: BTreeMap, @@ -65,51 +61,41 @@ where /// Greatest common denominator time series that allows this arc to meet all of the conditions. time_series: TimeSeries, _msr_in: PhantomData, - _msr: PhantomData, } -impl TrackingArcSim +impl TrackingArcSim where - D: TrackingDeviceSim, + D: TrackingDevice, MsrIn: State, - Msr: Measurement, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { /// Build a new tracking arc simulator using the provided seeded random number generator. pub fn with_rng( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, rng: Pcg64Mcg, ) -> Result { // Check that each device has an associated configurations. // We don't care if there are more configurations than chosen devices. - let mut devices_map = BTreeMap::new(); let mut sampling_rates_ns = Vec::with_capacity(devices.len()); - for device in devices { - if let Some(cfg) = configs.get(&device.name()) { + for name in devices.keys() { + if let Some(cfg) = configs.get(name) { if let Err(e) = cfg.sanity_check() { - warn!("Ignoring device {}: {e}", device.name()); + warn!("Ignoring device {name}: {e}"); continue; } sampling_rates_ns.push(cfg.sampling.truncated_nanoseconds()); } else { - warn!( - "Ignoring device {}: no associated tracking configuration", - device.name() - ); + warn!("Ignoring device {name}: no associated tracking configuration",); continue; } - devices_map.insert(device.name(), device); } - if devices_map.is_empty() { + if sampling_rates_ns.is_empty() { return Err(ConfigError::InvalidConfig { msg: "None of the devices are properly configured".to_string(), }); @@ -128,13 +114,12 @@ where ); let me = Self { - devices: devices_map, + devices, trajectory, configs, rng, time_series, _msr_in: PhantomData, - _msr: PhantomData, }; info!("{me}"); @@ -144,7 +129,7 @@ where /// Build a new tracking arc simulator using the provided seed to initialize the random number generator. pub fn with_seed( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, seed: u64, @@ -156,7 +141,7 @@ where /// Build a new tracking arc simulator using the system entropy to seed the random number generator. pub fn new( - devices: Vec, + devices: BTreeMap, trajectory: Traj, configs: BTreeMap, ) -> Result { @@ -182,93 +167,89 @@ where pub fn generate_measurements( &mut self, almanac: Arc, - ) -> Result, NyxError> { - let mut measurements = Vec::new(); + ) -> Result { + let mut measurements = BTreeMap::new(); for (name, device) in self.devices.iter_mut() { - let cfg = &self.configs[name]; - if cfg.scheduler.is_some() { - if cfg.strands.is_none() { - return Err(NyxError::CustomError { - msg: format!( - "schedule for {name} must be built before generating measurements" - ), - }); - } else { - warn!("scheduler for {name} is ignored, using the defined tracking strands instead") + if let Some(cfg) = self.configs.get(name) { + if cfg.scheduler.is_some() { + if cfg.strands.is_none() { + return Err(NyxError::CustomError { + msg: format!( + "schedule for {name} must be built before generating measurements" + ), + }); + } else { + warn!("scheduler for {name} is ignored, using the defined tracking strands instead") + } } - } - let init_msr_count = measurements.len(); - let tick = Epoch::now().unwrap(); - - match cfg.strands.as_ref() { - Some(strands) => { - // Strands are defined at this point - 'strands: for (ii, strand) in strands.iter().enumerate() { - // Build the time series for this strand, sampling at the correct rate - for epoch in TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) { - match device.measure( - epoch, - &self.trajectory, - Some(&mut self.rng), - almanac.clone(), - ) { - Ok(msr_opt) => { - if let Some(msr) = msr_opt { - measurements.push((name.clone(), msr)); + let init_msr_count = measurements.len(); + let tick = Epoch::now().unwrap(); + + match cfg.strands.as_ref() { + Some(strands) => { + // Strands are defined at this point + 'strands: for (ii, strand) in strands.iter().enumerate() { + // Build the time series for this strand, sampling at the correct rate + for epoch in + TimeSeries::inclusive(strand.start, strand.end, cfg.sampling) + { + match device.measure( + epoch, + &self.trajectory, + Some(&mut self.rng), + almanac.clone(), + ) { + Ok(msr_opt) => { + if let Some(msr) = msr_opt { + measurements.insert(epoch, msr); + } } - } - Err(e) => { - if epoch != strand.end { - warn!( + Err(e) => { + if epoch != strand.end { + warn!( "Skipping the remaining strand #{ii} ending on {}: {e}", strand.end ); + } + continue 'strands; } - continue 'strands; } } } - } - info!( - "Simulated {} measurements for {name} for {} tracking strands in {}", - measurements.len() - init_msr_count, - strands.len(), - (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) - ); - } - None => { - warn!("No tracking strands defined for {name}, skipping"); + info!( + "Simulated {} measurements for {name} for {} tracking strands in {}", + measurements.len() - init_msr_count, + strands.len(), + (Epoch::now().unwrap() - tick).round(1.0_f64.milliseconds()) + ); + } + None => { + warn!("No tracking strands defined for {name}, skipping"); + } } } } - // Reorder the measurements - measurements.sort_by_key(|(_name, msr)| msr.epoch()); - // Build the tracking arc. - let trk = TrackingArc { - device_cfg: serde_yaml::to_string(&self.devices).unwrap(), + let trk_data = TrackingDataArc { measurements, + source: None, }; - Ok(trk) + Ok(trk_data) } } -impl Display for TrackingArcSim +impl Display for TrackingArcSim where - D: TrackingDeviceSim, - Msr: Measurement, + D: TrackingDevice, MsrIn: Interpolatable, DefaultAllocator: Allocator<::Size> + Allocator<::Size, ::Size> - + Allocator<::VecLength> - + Allocator::Size> - + Allocator - + Allocator, + + Allocator<::VecLength>, { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { write!( @@ -282,7 +263,7 @@ where } // Literally the same as above, but can't make it generic =( -impl TrackingArcSim { +impl TrackingArcSim { /// Builds the schedule provided the config. Requires the tracker to be a ground station. /// /// # Algorithm @@ -302,78 +283,81 @@ impl TrackingArcSim { ) -> Result, NyxError> { // Consider using find_all via the heuristic let mut built_cfg = self.configs.clone(); + for (name, device) in self.devices.iter() { - let cfg = &self.configs[name]; - if let Some(scheduler) = cfg.scheduler { - info!("Building schedule for {name}"); - built_cfg.get_mut(name).unwrap().scheduler = None; - built_cfg.get_mut(name).unwrap().strands = Some(Vec::new()); - // Convert the trajectory into the ground station frame. - let traj = self.trajectory.to_frame(device.frame, almanac.clone())?; - - match traj.find_arcs(&device, almanac.clone()) { - Err(_) => info!("No measurements from {name}"), - Ok(elevation_arcs) => { - for arc in elevation_arcs { - let strand_start = arc.rise.state.epoch(); - let strand_end = arc.fall.state.epoch(); - - if strand_end - strand_start - < cfg.sampling * i64::from(scheduler.min_samples) - { - info!( + if let Some(cfg) = self.configs.get(name) { + if let Some(scheduler) = cfg.scheduler { + info!("Building schedule for {name}"); + built_cfg.get_mut(name).unwrap().scheduler = None; + built_cfg.get_mut(name).unwrap().strands = Some(Vec::new()); + + // Convert the trajectory into the ground station frame. + let traj = self.trajectory.to_frame(device.frame, almanac.clone())?; + + match traj.find_arcs(&device, almanac.clone()) { + Err(_) => info!("No measurements from {name}"), + Ok(elevation_arcs) => { + for arc in elevation_arcs { + let strand_start = arc.rise.state.epoch(); + let strand_end = arc.fall.state.epoch(); + + if strand_end - strand_start + < cfg.sampling * i64::from(scheduler.min_samples) + { + info!( "Too few samples from {name} opportunity from {strand_start} to {strand_end}, discarding strand", ); - continue; - } + continue; + } - let mut strand_range = Strand { - start: strand_start, - end: strand_end, - }; + let mut strand_range = Strand { + start: strand_start, + end: strand_end, + }; - // If there is an alignment, apply it - if let Some(alignment) = scheduler.sample_alignment { - strand_range.start = strand_range.start.round(alignment); - strand_range.end = strand_range.end.round(alignment); - } + // If there is an alignment, apply it + if let Some(alignment) = scheduler.sample_alignment { + strand_range.start = strand_range.start.round(alignment); + strand_range.end = strand_range.end.round(alignment); + } - if let Cadence::Intermittent { on, off } = scheduler.cadence { - // Check that the next start time is within the allocated time - if let Some(prev_strand) = - built_cfg[name].strands.as_ref().unwrap().last() - { - if prev_strand.end + off > strand_range.start { - // We're turning on the tracking sooner than the schedule allows, so let's fix that. - strand_range.start = prev_strand.end + off; - // Check that we didn't eat into the whole tracking opportunity - if strand_range.start > strand_end { - // Lost this whole opportunity. - info!("Discarding {name} opportunity from {strand_start} to {strand_end} due to cadence {:?}", scheduler.cadence); - continue; + if let Cadence::Intermittent { on, off } = scheduler.cadence { + // Check that the next start time is within the allocated time + if let Some(prev_strand) = + built_cfg[name].strands.as_ref().unwrap().last() + { + if prev_strand.end + off > strand_range.start { + // We're turning on the tracking sooner than the schedule allows, so let's fix that. + strand_range.start = prev_strand.end + off; + // Check that we didn't eat into the whole tracking opportunity + if strand_range.start > strand_end { + // Lost this whole opportunity. + info!("Discarding {name} opportunity from {strand_start} to {strand_end} due to cadence {:?}", scheduler.cadence); + continue; + } } } + // Check that we aren't tracking for longer than configured + if strand_range.end - strand_range.start > on { + strand_range.end = strand_range.start + on; + } } - // Check that we aren't tracking for longer than configured - if strand_range.end - strand_range.start > on { - strand_range.end = strand_range.start + on; - } + + // We've found when the spacecraft is below the horizon, so this is a new strand. + built_cfg + .get_mut(name) + .unwrap() + .strands + .as_mut() + .unwrap() + .push(strand_range); } - // We've found when the spacecraft is below the horizon, so this is a new strand. - built_cfg - .get_mut(name) - .unwrap() - .strands - .as_mut() - .unwrap() - .push(strand_range); + info!( + "Built {} tracking strands for {name}", + built_cfg[name].strands.as_ref().unwrap().len() + ); } - - info!( - "Built {} tracking strands for {name}", - built_cfg[name].strands.as_ref().unwrap().len() - ); } } } @@ -381,8 +365,10 @@ impl TrackingArcSim { // Build all of the strands, remembering which tracker they come from. let mut cfg_as_vec = Vec::new(); for (name, cfg) in &built_cfg { - for (ii, strand) in cfg.strands.as_ref().unwrap().iter().enumerate() { - cfg_as_vec.push((name.clone(), ii, *strand)); + if let Some(strands) = &cfg.strands { + for (ii, strand) in strands.iter().enumerate() { + cfg_as_vec.push((name.clone(), ii, *strand)); + } } } // Iterate through the strands by chronological order. Cannot use maps because we change types. diff --git a/src/od/simulator/mod.rs b/src/od/simulator/mod.rs index 2e294fc4..453f2a92 100644 --- a/src/od/simulator/mod.rs +++ b/src/od/simulator/mod.rs @@ -23,6 +23,6 @@ pub use arc::TrackingArcSim; mod scheduler; pub use scheduler::{Cadence, Handoff, Scheduler}; mod trackdata; -pub use trackdata::TrackingDeviceSim; +pub use trackdata::TrackingDevice; mod trkconfig; pub use trkconfig::{Strand, TrkConfig}; diff --git a/src/od/simulator/scheduler.rs b/src/od/simulator/scheduler.rs index 117ff745..5444269c 100644 --- a/src/od/simulator/scheduler.rs +++ b/src/od/simulator/scheduler.rs @@ -122,13 +122,13 @@ mod scheduler_ut { #[test] fn serde_cadence() { use hifitime::TimeUnits; - use serde_yaml; + use serde_yml; - let cont: Cadence = serde_yaml::from_str("!Continuous").unwrap(); + let cont: Cadence = serde_yml::from_str("!Continuous").unwrap(); assert_eq!(cont, Cadence::Continuous); let int: Cadence = - serde_yaml::from_str("!Intermittent {on: 1 h 35 min, off: 15 h 02 min 3 s}").unwrap(); + serde_yml::from_str("!Intermittent {on: 1 h 35 min, off: 15 h 02 min 3 s}").unwrap(); assert_eq!( int, Cadence::Intermittent { @@ -141,23 +141,23 @@ mod scheduler_ut { r#"Intermittent { on: "1 h 35 min", off: "15 h 2 min 3 s" }"# ); - let serialized = serde_yaml::to_string(&int).unwrap(); - let deserd: Cadence = serde_yaml::from_str(&serialized).unwrap(); + let serialized = serde_yml::to_string(&int).unwrap(); + let deserd: Cadence = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, int); } #[test] fn api_and_serde_scheduler() { use hifitime::TimeUnits; - use serde_yaml; + use serde_yml; let scheduler = Scheduler::default(); - let serialized = serde_yaml::to_string(&scheduler).unwrap(); + let serialized = serde_yml::to_string(&scheduler).unwrap(); assert_eq!( serialized, "handoff: Eager\ncadence: Continuous\nmin_samples: 0\nsample_alignment: null\n" ); - let deserd: Scheduler = serde_yaml::from_str(&serialized).unwrap(); + let deserd: Scheduler = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, scheduler); let scheduler = Scheduler::builder() @@ -168,12 +168,12 @@ mod scheduler_ut { }) .build(); - let serialized = serde_yaml::to_string(&scheduler).unwrap(); + let serialized = serde_yml::to_string(&scheduler).unwrap(); assert_eq!( serialized, - "handoff: Eager\ncadence: !Intermittent\n on: 12 min\n off: 17 h 5 min\nmin_samples: 10\nsample_alignment: 1 s\n" + "handoff: Eager\ncadence: !Intermittent\n 'on': '12 min'\n 'off': '17 h 5 min'\nmin_samples: 10\nsample_alignment: '1 s'\n" ); - let deserd: Scheduler = serde_yaml::from_str(&serialized).unwrap(); + let deserd: Scheduler = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, scheduler); } diff --git a/src/od/simulator/trackdata.rs b/src/od/simulator/trackdata.rs index 06b8f28c..d7699adf 100644 --- a/src/od/simulator/trackdata.rs +++ b/src/od/simulator/trackdata.rs @@ -21,30 +21,33 @@ use std::sync::Arc; use anise::almanac::Almanac; use anise::errors::AlmanacResult; use hifitime::Epoch; +use indexmap::IndexSet; +use nalgebra::{DimName, OMatrix}; use rand_pcg::Pcg64Mcg; use crate::io::ConfigRepr; use crate::linalg::allocator::Allocator; -use crate::linalg::{DefaultAllocator, OMatrix}; +use crate::linalg::DefaultAllocator; use crate::md::prelude::{Frame, Traj}; use crate::md::trajectory::Interpolatable; -use crate::od::{Measurement, ODError}; +use crate::od::msr::measurement::Measurement as NewMeasurement; +use crate::od::msr::MeasurementType; +use crate::od::ODError; use crate::Orbit; /// Tracking device simulator. -pub trait TrackingDeviceSim: ConfigRepr +pub trait TrackingDevice: ConfigRepr where MsrIn: Interpolatable, - Msr: Measurement, - DefaultAllocator: Allocator - + Allocator - + Allocator - + Allocator - + Allocator, + DefaultAllocator: + Allocator + Allocator + Allocator, { /// Returns the name of this tracking data simulator fn name(&self) -> String; + /// Returns the _enabled_ measurement types for thie device. + fn measurement_types(&self) -> &IndexSet; + /// Performs a measurement of the input trajectory at the provided epoch (with integration times if relevant), and returns a measurement from itself to the input state. Returns None of the object is not visible. /// This trait function takes in a trajectory and epoch so it can properly simulate integration times for the measurements. /// If the random number generator is provided, it shall be used to add noise to the measurement. @@ -61,7 +64,7 @@ where traj: &Traj, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError>; + ) -> Result, ODError>; /// Returns the device location at the given epoch and in the given frame. fn location(&self, epoch: Epoch, frame: Frame, almanac: Arc) -> AlmanacResult; @@ -72,11 +75,28 @@ where rx: MsrIn, rng: Option<&mut Pcg64Mcg>, almanac: Arc, - ) -> Result, ODError>; + ) -> Result, ODError>; - // Return the noise statistics of this tracking device at the requested epoch. - fn measurement_covar( - &mut self, + // Return the noise statistics of this tracking device for the provided measurement type at the requested epoch. + fn measurement_covar(&self, msr_type: MeasurementType, epoch: Epoch) -> Result; + + fn measurement_covar_matrix( + &self, + msr_types: &IndexSet, epoch: Epoch, - ) -> Result, ODError>; + ) -> Result, ODError> + where + DefaultAllocator: Allocator, + { + // Rebuild the R matrix of the measurement noise. + let mut r_mat = OMatrix::::zeros(); + + for (i, msr_type) in msr_types.iter().enumerate() { + if self.measurement_types().contains(msr_type) { + r_mat[(i, i)] = self.measurement_covar(*msr_type, epoch)?; + } + } + + Ok(r_mat) + } } diff --git a/src/od/simulator/trkconfig.rs b/src/od/simulator/trkconfig.rs index e6570243..7ee43ed5 100644 --- a/src/od/simulator/trkconfig.rs +++ b/src/od/simulator/trkconfig.rs @@ -60,7 +60,7 @@ impl FromStr for TrkConfig { type Err = ConfigError; fn from_str(s: &str) -> Result { - serde_yaml::from_str(s).map_err(|source| ConfigError::ParseError { source }) + serde_yml::from_str(s).map_err(|source| ConfigError::ParseError { source }) } } @@ -93,7 +93,11 @@ impl TrkConfig { for (ii, strand) in strands.iter().enumerate() { if strand.duration() < self.sampling { return Err(ConfigError::InvalidConfig { - msg: format!("Strand #{ii} is shorter than sampling time"), + msg: format!( + "Strand #{ii} lasts {} which is shorter than sampling time of {}", + strand.duration(), + self.sampling + ), }); } if strand.duration().is_negative() { @@ -206,13 +210,13 @@ mod trkconfig_ut { #[test] fn serde_trkconfig() { - use serde_yaml; + use serde_yml; // Test the default config let cfg = TrkConfig::default(); - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); assert_eq!( cfg.scheduler.unwrap(), @@ -234,9 +238,9 @@ mod trkconfig_ut { sampling: 45.2.seconds(), ..Default::default() }; - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); } @@ -263,16 +267,16 @@ mod trkconfig_ut { #[test] fn api_trk_config() { - use serde_yaml; + use serde_yml; let cfg = TrkConfig::builder() .sampling(15.seconds()) .scheduler(Scheduler::builder().handoff(Handoff::Overlap).build()) .build(); - let serialized = serde_yaml::to_string(&cfg).unwrap(); + let serialized = serde_yml::to_string(&cfg).unwrap(); println!("{serialized}"); - let deserd: TrkConfig = serde_yaml::from_str(&serialized).unwrap(); + let deserd: TrkConfig = serde_yml::from_str(&serialized).unwrap(); assert_eq!(deserd, cfg); let cfg = TrkConfig::builder() diff --git a/src/python/orbit_determination/ground_station.rs b/src/python/orbit_determination/ground_station.rs index 03f99c35..c85d2610 100644 --- a/src/python/orbit_determination/ground_station.rs +++ b/src/python/orbit_determination/ground_station.rs @@ -126,7 +126,7 @@ impl GroundStation { for item in as_list.iter() { // Check that the item is a dictionary let next: Self = - serde_yaml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; + serde_yml::from_value(pyany_to_value(item)?).context(ParseSnafu)?; as_map.insert(next.name.clone(), next); } Ok(as_map) @@ -148,12 +148,12 @@ impl GroundStation { // Try to convert the underlying data match pyany_to_value(v_any) { Ok(value) => { - let next: Self = serde_yaml::from_value(value).context(ParseSnafu)?; + let next: Self = serde_yml::from_value(value).context(ParseSnafu)?; as_map.insert(as_str, next); } Err(_) => { // Maybe this was to be parsed in full as a single item - let me: Self = serde_yaml::from_value(pyany_to_value(as_dict)?) + let me: Self = serde_yml::from_value(pyany_to_value(as_dict)?) .context(ParseSnafu)?; as_map.clear(); as_map.insert(me.name.clone(), me); diff --git a/src/python/orbit_determination/trkconfig.rs b/src/python/orbit_determination/trkconfig.rs index b28a146a..b0fee72e 100644 --- a/src/python/orbit_determination/trkconfig.rs +++ b/src/python/orbit_determination/trkconfig.rs @@ -71,7 +71,7 @@ impl TrkConfig { } fn __repr__(&self) -> String { - serde_yaml::to_string(&self).unwrap() + serde_yml::to_string(&self).unwrap() } fn __str__(&self) -> String { diff --git a/src/python/pyo3utils/mod.rs b/src/python/pyo3utils/mod.rs index 6aede635..146abeb1 100644 --- a/src/python/pyo3utils/mod.rs +++ b/src/python/pyo3utils/mod.rs @@ -18,7 +18,7 @@ use crate::io::ConfigError; use pyo3::types::{PyAny, PyDict, PyList}; -use serde_yaml::{Mapping, Value}; +use serde_yml::{Mapping, Value}; /// Try to convert the provided PyAny into a SerDe YAML Value pub fn pyany_to_value(any: &PyAny) -> Result { diff --git a/tests/mission_design/multishoot/mod.rs b/tests/mission_design/multishoot/mod.rs index a6d70b9b..c7511a56 100644 --- a/tests/mission_design/multishoot/mod.rs +++ b/tests/mission_design/multishoot/mod.rs @@ -23,7 +23,7 @@ fn almanac() -> Arc { } #[rstest] -fn alt_orbit_raising(almanac: Arc) { +fn alt_orbit_raising_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); @@ -408,7 +408,7 @@ fn vmag_orbit_raising(almanac: Arc) { .enumerate() { traj.to_parquet_with_step( - &format!("multishoot_to_node_{}.parquet", i), + format!("multishoot_to_node_{}.parquet", i), 2 * Unit::Second, almanac.clone(), ) diff --git a/tests/mission_design/targeter/b_plane.rs b/tests/mission_design/targeter/b_plane.rs index 51435b57..c1358895 100644 --- a/tests/mission_design/targeter/b_plane.rs +++ b/tests/mission_design/targeter/b_plane.rs @@ -1,11 +1,8 @@ extern crate nyx_space as nyx; -use anise::constants::celestial_objects::JUPITER_BARYCENTER; -use anise::constants::celestial_objects::MOON; -use anise::constants::celestial_objects::SUN; +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}; use anise::constants::frames::MOON_J2000; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; diff --git a/tests/mission_design/targeter/finite_burns.rs b/tests/mission_design/targeter/finite_burns.rs index 8ef6f91f..e330e3d0 100644 --- a/tests/mission_design/targeter/finite_burns.rs +++ b/tests/mission_design/targeter/finite_burns.rs @@ -5,7 +5,6 @@ use hifitime::TimeUnits; use nyx::dynamics::guidance::{LocalFrame, Mnvr, Thruster}; use nyx::linalg::Vector3; use nyx::md::prelude::*; -use nyx::md::targeter::*; use crate::propagation::GMAT_EARTH_GM; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; @@ -115,7 +114,7 @@ fn thrust_dir_rate_tgt_sma_aop_raan(almanac: Arc) { #[ignore] #[rstest] -fn thrust_profile_tgt_sma_aop_raan(almanac: Arc) { +fn thrust_profile_tgt_sma_aop_raan_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/mod.rs b/tests/mission_design/targeter/mod.rs index 2f88163d..06936ac9 100644 --- a/tests/mission_design/targeter/mod.rs +++ b/tests/mission_design/targeter/mod.rs @@ -4,6 +4,4 @@ mod b_plane; mod finite_burns; mod multi_oe; mod multi_oe_vnc; -#[cfg(feature = "broken-donotuse")] -mod opti_levenberg; mod single_oe; diff --git a/tests/mission_design/targeter/multi_oe.rs b/tests/mission_design/targeter/multi_oe.rs index ac5b11fd..c9229fcc 100644 --- a/tests/mission_design/targeter/multi_oe.rs +++ b/tests/mission_design/targeter/multi_oe.rs @@ -1,9 +1,7 @@ extern crate nyx_space as nyx; -// use nyx::dynamics::guidance::Mnvr; use nyx::dynamics::guidance::Thruster; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; @@ -187,7 +185,7 @@ fn conv_tgt_sma_ecc(almanac: Arc) { } #[rstest] -fn tgt_hd_sma_ecc(almanac: Arc) { +fn tgt_hd_sma_ecc_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/multi_oe_vnc.rs b/tests/mission_design/targeter/multi_oe_vnc.rs index 7397b602..3632b3f3 100644 --- a/tests/mission_design/targeter/multi_oe_vnc.rs +++ b/tests/mission_design/targeter/multi_oe_vnc.rs @@ -1,7 +1,6 @@ extern crate nyx_space as nyx; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; @@ -14,7 +13,7 @@ fn almanac() -> Arc { } #[rstest] -fn tgt_vnc_c3_decl(almanac: Arc) { +fn tgt_vnc_c3_decl_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); @@ -62,7 +61,7 @@ fn tgt_vnc_c3_decl(almanac: Arc) { } #[rstest] -fn tgt_vnc_sma_ecc(almanac: Arc) { +fn tgt_vnc_sma_ecc_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/mission_design/targeter/single_oe.rs b/tests/mission_design/targeter/single_oe.rs index 33603a63..8c73c590 100644 --- a/tests/mission_design/targeter/single_oe.rs +++ b/tests/mission_design/targeter/single_oe.rs @@ -1,10 +1,7 @@ extern crate nyx_space as nyx; -use anise::constants::celestial_objects::JUPITER_BARYCENTER; -use anise::constants::celestial_objects::MOON; -use anise::constants::celestial_objects::SUN; +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}; use nyx::md::prelude::*; -use nyx::md::targeter::*; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; @@ -573,7 +570,7 @@ fn tgt_aop_from_apo(almanac: Arc) { } #[rstest] -fn tgt_aop_from_peri(almanac: Arc) { +fn tgt_aop_from_peri_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/monte_carlo/framework.rs b/tests/monte_carlo/framework.rs index 6455829e..190e3b93 100644 --- a/tests/monte_carlo/framework.rs +++ b/tests/monte_carlo/framework.rs @@ -19,7 +19,7 @@ fn almanac() -> Arc { } #[rstest] -fn test_monte_carlo_epoch(almanac: Arc) { +fn test_monte_carlo_epoch_cov_test(almanac: Arc) { extern crate pretty_env_logger; let _ = pretty_env_logger::try_init(); @@ -33,7 +33,7 @@ fn test_monte_carlo_epoch(almanac: Arc) { // 5% error on SMA and 5% on Eccentricity let nominal_state = Spacecraft::from(state); - let random_state = MultivariateNormal::new( + let random_state = MvnSpacecraft::new( nominal_state, vec![ StateDispersion::zero_mean(StateParameter::SMA, 0.05), diff --git a/tests/orbit_determination/measurements.rs b/tests/orbit_determination/measurements.rs index 0317ddd6..dc5033a6 100644 --- a/tests/orbit_determination/measurements.rs +++ b/tests/orbit_determination/measurements.rs @@ -3,6 +3,8 @@ extern crate nyx_space as nyx; use anise::constants::celestial_objects::{EARTH, MOON, SUN}; use anise::constants::frames::IAU_EARTH_FRAME; use anise::constants::usual_planetary_constants::MEAN_EARTH_ANGULAR_VELOCITY_DEG_S; +use indexmap::{IndexMap, IndexSet}; +use nalgebra::{Const, U2}; use nyx::cosmic::Orbit; use nyx::dynamics::SpacecraftDynamics; use nyx::od::prelude::*; @@ -14,6 +16,7 @@ use rand_pcg::Pcg64Mcg; use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; use rstest::*; +use sensitivity::TrackerSensitivity; use std::sync::Arc; #[fixture] @@ -34,6 +37,10 @@ fn nil_measurement(almanac: Arc) { let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let mut stochastics = IndexMap::new(); + stochastics.insert(MeasurementType::Range, StochasticNoise::MIN); + stochastics.insert(MeasurementType::Doppler, StochasticNoise::MIN); + let mut station = GroundStation { name: "nil".to_string(), latitude_deg: lat, @@ -42,10 +49,10 @@ fn nil_measurement(almanac: Arc) { frame: eme2k, elevation_mask_deg: 0.0, timestamp_noise_s: None, - range_noise_km: Some(StochasticNoise::MIN), - doppler_noise_km_s: Some(StochasticNoise::MIN), + stochastic_noises: Some(stochastics), integration_time: None, light_time_correction: false, + ..Default::default() }; let at_station = Orbit::try_latlongalt( @@ -81,6 +88,10 @@ fn val_measurements_topo(almanac: Arc) { use self::nyx::od::prelude::*; use std::str::FromStr; + let mut msr_types = IndexSet::new(); + msr_types.insert(MeasurementType::Range); + msr_types.insert(MeasurementType::Doppler); + let cislunar1 = Orbit::cartesian( -6252.59501113, 1728.23921802, @@ -194,7 +205,7 @@ fn val_measurements_topo(almanac: Arc) { .unwrap() .unwrap(); - let obs = meas.observation(); + let obs = meas.observation::(&msr_types); println!( "range difference {:e}\t range rate difference: {:e}", (obs[0] - truth.range).abs(), @@ -238,12 +249,12 @@ fn val_measurements_topo(almanac: Arc) { // Now iterate the trajectory to count the measurements. for state in traj2.every(1 * Unit::Minute) { - if dss65_madrid + if let Some(msr) = dss65_madrid .measure(state.epoch(), &traj2, Some(&mut rng), almanac.clone()) .unwrap() - .is_some() { traj2_msr_cnt += 1; + println!("{msr}"); } } @@ -261,7 +272,7 @@ fn val_measurements_topo(almanac: Arc) { .measure(state.epoch(), &traj2, Some(&mut rng), almanac.clone()) .unwrap() .unwrap(); - let obs = meas.observation(); + let obs = meas.observation::(&msr_types); println!( "range difference {:e}\t range rate difference: {:e}", (obs[0] - truth.range).abs(), @@ -269,3 +280,85 @@ fn val_measurements_topo(almanac: Arc) { ); } } + +/// Verifies that the sensitivity matrix is reasonably well. +#[allow(clippy::identity_op)] +#[rstest] +fn verif_sensitivity_mat(almanac: Arc) { + use self::nyx::cosmic::Orbit; + use self::nyx::md::prelude::*; + use self::nyx::od::prelude::*; + use std::str::FromStr; + + let cislunar1 = Orbit::cartesian( + 58643.769540, + -61696.435624, + -36178.745722, + 2.148654, + -1.202489, + -0.714016, + Epoch::from_str("2022-11-16T13:35:31.0 UTC").unwrap(), + almanac.frame_from_uid(EARTH_J2000).unwrap(), + ); + + let cislunar_sc: Spacecraft = cislunar1.into(); + + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + + let mut dss65_madrid = + GroundStation::dss65_madrid(0.0, StochasticNoise::ZERO, StochasticNoise::ZERO, iau_earth) + .with_msr_type(MeasurementType::Azimuth, StochasticNoise::ZERO) + .with_msr_type(MeasurementType::Elevation, StochasticNoise::ZERO); + + let mut cislunar_sc_pert = cislunar_sc; + cislunar_sc_pert.orbit.radius_km.x += 1.0; + cislunar_sc_pert.orbit.radius_km.y -= 1.0; + cislunar_sc_pert.orbit.radius_km.z += 1.0; + cislunar_sc_pert.orbit.velocity_km_s.x += 1.0e-3; + cislunar_sc_pert.orbit.velocity_km_s.y -= 1.0e-3; + cislunar_sc_pert.orbit.velocity_km_s.z += 1.0e-3; + + let truth_meas = dss65_madrid + .measure_instantaneous(cislunar_sc, None, almanac.clone()) + .expect("successful measurement") + .expect("a measurement"); + + let pert_meas = dss65_madrid + .measure_instantaneous(cislunar_sc_pert, None, almanac.clone()) + .expect("successful measurement") + .expect("a measurement"); + + for msr_type in [ + MeasurementType::Range, + MeasurementType::Doppler, + MeasurementType::Elevation, + MeasurementType::Azimuth, + ] { + let mut msr_types = IndexSet::new(); + msr_types.insert(msr_type); + + let truth_obs = truth_meas.observation::>(&msr_types); + + let pert_obs = pert_meas.observation::>(&msr_types); + + // Given this observation, feed it to the sensitivity matrix, and we should find the original state. + + let h_tilde = dss65_madrid + .h_tilde::>(&truth_meas, &msr_types, &cislunar_sc, almanac.clone()) + .expect("sensitivity should not fail"); + + let delta_state = cislunar_sc.to_vector().fixed_rows::<9>(0) + - cislunar_sc_pert.to_vector().fixed_rows::<9>(0); + + let delta_obs = h_tilde * delta_state; + let computed_obs = truth_obs - delta_obs; + + let sensitivity_error = (pert_obs - computed_obs)[0]; + println!( + "{msr_type:?} error = {sensitivity_error:.3e} {}", + msr_type.unit() + ); + + assert!(sensitivity_error.abs() < 1e-3); + } +} diff --git a/tests/orbit_determination/mod.rs b/tests/orbit_determination/mod.rs index d2b6fdb3..e3145bd0 100644 --- a/tests/orbit_determination/mod.rs +++ b/tests/orbit_determination/mod.rs @@ -11,6 +11,7 @@ mod measurements; mod multi_body; mod resid_reject; mod robust; +mod robust_az_el; mod simulator; mod spacecraft; mod trackingarc; diff --git a/tests/orbit_determination/multi_body.rs b/tests/orbit_determination/multi_body.rs index 97a5ba65..a9b8e1c9 100644 --- a/tests/orbit_determination/multi_body.rs +++ b/tests/orbit_determination/multi_body.rs @@ -4,6 +4,7 @@ use anise::constants::celestial_objects::JUPITER_BARYCENTER; use anise::constants::celestial_objects::MOON; use anise::constants::celestial_objects::SUN; use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::U2; use nyx::od::simulator::TrackingArcSim; use nyx::od::simulator::TrkConfig; use nyx_space::propagators::IntegratorMethod; @@ -26,7 +27,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -48,12 +49,17 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -75,29 +81,29 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices } #[allow(clippy::identity_op)] #[rstest] fn od_val_multi_body_ckf_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); // Define the tracking configurations let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert( - device.name.clone(), - TrkConfig::from_sample_rate(10.seconds()), - ); + for name in sim_devices.keys() { + configs.insert(name.clone(), TrkConfig::from_sample_rate(10.seconds())); } - let all_stations = sim_devices; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -120,11 +126,11 @@ fn od_val_multi_body_ckf_perfect_stations( let (final_truth, traj) = prop.for_duration_with_traj(prop_time).unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); + arc.to_parquet_simple("multi_body.parquet").unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -149,9 +155,9 @@ fn od_val_multi_body_ckf_perfect_stations( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::<_, RangeDoppler, _, _, _>::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let mut last_est = None; for (no, est) in odp.estimates.iter().enumerate() { @@ -202,15 +208,15 @@ fn od_val_multi_body_ckf_perfect_stations( #[allow(clippy::identity_op)] #[rstest] -fn multi_body_ckf_covar_map( +fn multi_body_ckf_covar_map_cov_test( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + mut sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // For this test, we're only enabling one station so we can check that the covariance inflates between visibility passes. let _ = pretty_env_logger::try_init(); - let dss13_goldstone = sim_devices[2].clone(); + let dss13_goldstone = sim_devices.get("Goldstone").unwrap(); // Define the tracking configurations let mut configs = BTreeMap::new(); @@ -222,7 +228,9 @@ fn multi_body_ckf_covar_map( .build(), ); - let all_stations = vec![dss13_goldstone]; + // Remove all but Goldstone + sim_devices.remove("Canberra").unwrap(); + sim_devices.remove("Madrid").unwrap(); // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -247,12 +255,10 @@ fn multi_body_ckf_covar_map( let (_, traj) = prop.for_duration_with_traj(prop_time).unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(vec![proc_devices[2].clone()], configs) - .unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -277,9 +283,10 @@ fn multi_body_ckf_covar_map( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac.clone()); + let mut odp = + ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac.clone()); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let mut num_pred = 0_u32; for est in odp.estimates.iter() { diff --git a/tests/orbit_determination/resid_reject.rs b/tests/orbit_determination/resid_reject.rs index b6a4b076..ed7ed806 100644 --- a/tests/orbit_determination/resid_reject.rs +++ b/tests/orbit_determination/resid_reject.rs @@ -1,5 +1,6 @@ use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; use anise::constants::frames::{EARTH_J2000, IAU_EARTH_FRAME}; +use nalgebra::U2; use nyx_space::dynamics::guidance::LocalFrame; use pretty_env_logger::try_init; @@ -64,7 +65,7 @@ fn traj(epoch: Epoch, almanac: Arc) -> Traj { fn devices_n_configs( epoch: Epoch, almanac: Arc, -) -> (Vec, BTreeMap) { +) -> (BTreeMap, BTreeMap) { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; @@ -107,15 +108,19 @@ fn devices_n_configs( .build(), ); - (vec![dss65_madrid, dss34_canberra], configs) + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + + (devices, configs) } #[fixture] fn tracking_arc( traj: Traj, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, -) -> TrackingArc { +) -> TrackingDataArc { let (devices, configs) = devices_n_configs; // Simulate tracking data @@ -177,9 +182,9 @@ fn initial_estimate(traj: Traj) -> KfEstimate { #[rstest] fn od_resid_reject_inflated_snc_ckf_two_way( traj: Traj, - tracking_arc: TrackingArc, + tracking_arc: TrackingDataArc, initial_estimate: KfEstimate, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, ) { let (devices, _configs) = devices_n_configs; @@ -207,29 +212,15 @@ fn od_resid_reject_inflated_snc_ckf_two_way( // the measurements are accepted. // So we end up with an excellent estimate but an unusably high covariance. - let mut odp = ODProcess::ckf( + let mut odp = ODProcess::<_, U2, _, _, _>::ckf( prop_est, kf, + devices, Some(ResidRejectCrit { num_sigmas: 3.0 }), almanac, ); - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we're stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the hashmap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); - - odp.process( - &tracking_arc.measurements, - &mut devices_map, - tracking_arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&tracking_arc).unwrap(); let path: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -238,7 +229,9 @@ fn od_resid_reject_inflated_snc_ckf_two_way( ] .iter() .collect(); - odp.to_parquet(path, ExportCfg::timestamped()).unwrap(); + + odp.to_parquet(&tracking_arc, path, ExportCfg::timestamped()) + .unwrap(); let num_rejections = odp .residuals @@ -291,11 +284,11 @@ fn od_resid_reject_inflated_snc_ckf_two_way( } #[rstest] -fn od_resid_reject_default_ckf_two_way( +fn od_resid_reject_default_ckf_two_way_cov_test( traj: Traj, - tracking_arc: TrackingArc, + tracking_arc: TrackingDataArc, initial_estimate: KfEstimate, - devices_n_configs: (Vec, BTreeMap), + devices_n_configs: (BTreeMap, BTreeMap), almanac: Arc, ) { let (devices, _configs) = devices_n_configs; @@ -319,24 +312,15 @@ fn od_resid_reject_default_ckf_two_way( let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, kf, Some(ResidRejectCrit::default()), almanac); - - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we're stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the hashmap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf( + prop_est, + kf, + devices, + Some(ResidRejectCrit::default()), + almanac, + ); - odp.process( - &tracking_arc.measurements, - &mut devices_map, - tracking_arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&tracking_arc).unwrap(); // Save this result before the asserts for analysis let path: PathBuf = [ @@ -346,7 +330,8 @@ fn od_resid_reject_default_ckf_two_way( ] .iter() .collect(); - odp.to_parquet(path, ExportCfg::timestamped()).unwrap(); + odp.to_parquet(&tracking_arc, path, ExportCfg::timestamped()) + .unwrap(); let num_rejections = odp .residuals diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 119ff775..67102a5f 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -39,7 +39,7 @@ fn almanac() -> Arc { #[allow(clippy::identity_op)] #[rstest] -fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { +fn od_robust_test_ekf_realistic_one_way_cov_test(almanac: Arc) { let _ = pretty_env_logger::try_init(); let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); @@ -75,7 +75,9 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { ]); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. - let all_stations = vec![dss65_madrid, dss34_canberra]; + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -131,7 +133,7 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -147,8 +149,13 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { arc.to_parquet_simple(&path).unwrap(); - // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. - // We expect the estimated orbit to be _nearly_ perfect because we"ve removed SATURN_BARYCENTER from the estimated trajectory + println!("{arc}\n{arc:?}"); + // Reload + let reloaded = TrackingDataArc::from_parquet(&path).unwrap(); + assert_eq!(reloaded, arc); + + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); let setup = Propagator::new(estimator, IntegratorMethod::RungeKutta4, opts); @@ -162,15 +169,14 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - // Let"s filter and iterate on the initial subset of the arc to refine the initial estimate - let subset = arc.filter_by_offset(..3.hours()); + // Let's filter and iterate on the initial subset of the arc to refine the initial estimate + let subset = arc.clone().filter_by_offset(..3.hours()); let remaining = arc.filter_by_offset(3.hours()..); - odp.process_arc::(&subset).unwrap(); - odp.iterate_arc::(&subset, IterationConf::once()) - .unwrap(); + odp.process_arc(&subset).unwrap(); + odp.iterate_arc(&subset, IterationConf::once()).unwrap(); let (sm_rss_pos_km, sm_rss_vel_km_s) = rss_orbit_errors(&initial_state.orbit, &odp.estimates[0].state().orbit); @@ -182,9 +188,10 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { (initial_state.orbit - odp.estimates[0].state().orbit).unwrap() ); - odp.process_arc::(&remaining).unwrap(); + odp.process_arc(&remaining).unwrap(); odp.to_parquet( + &remaining, path.with_file_name("robustness_test_one_way.parquet"), ExportCfg::timestamped(), ) @@ -280,7 +287,10 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { ]); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. - let devices = vec![dss65_madrid, dss34_canberra]; + // Note that we do not have Goldstone so we can test enabling and disabling the EKF. + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); // Disperse the initial state based on some orbital elements errors given from ULA Atlas 5 user guide, table 2.3.3-1 // This assumes that the errors are ONE TENTH of the values given in the table. It assumes that the launch provider has provided an initial state vector, whose error is lower than the injection errors. @@ -338,8 +348,8 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { println!("{arc}"); - // Now that we have the truth data, let"s start an OD with no noise at all and compute the estimates. - // We expect the estimated orbit to be _nearly_ perfect because we"ve removed SATURN_BARYCENTER from the estimated trajectory + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory let bodies = vec![MOON, SUN, JUPITER_BARYCENTER]; let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); let setup = Propagator::default(estimator); @@ -353,32 +363,18 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); - - // TODO: Fix the deserialization of the measurements such that they also deserialize the integration time. - // Without it, we"re stuck having to rebuild them from scratch. - // https://github.com/nyx-space/nyx/issues/140 - - // Build the BTreeMap of devices from the vector using their names - let mut devices_map = devices - .into_iter() - .map(|dev| (dev.name.clone(), dev)) - .collect::>(); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); // Check that exporting an empty set returns an error. assert!(odp .to_parquet( + &arc, path.with_file_name("robustness_test_two_way.parquet"), ExportCfg::timestamped(), ) .is_err()); - odp.process( - &arc.measurements, - &mut devices_map, - arc.min_duration_sep().unwrap(), - ) - .unwrap(); + odp.process_arc(&arc).unwrap(); let mut num_residual_none = 0; let mut num_residual_some = 0; @@ -392,6 +388,7 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { // Export as Parquet let timestamped_path = odp .to_parquet( + &arc, path.with_file_name("robustness_test_two_way.parquet"), ExportCfg::timestamped(), ) diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs new file mode 100644 index 00000000..67c9361b --- /dev/null +++ b/tests/orbit_determination/robust_az_el.rs @@ -0,0 +1,355 @@ +extern crate nyx_space as nyx; +extern crate pretty_env_logger; + +use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN}; +use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::Const; +use nyx::cosmic::Orbit; +use nyx::dynamics::orbital::OrbitalDynamics; +use nyx::dynamics::SpacecraftDynamics; +use nyx::io::ExportCfg; +use nyx::md::StateParameter; +use nyx::od::prelude::*; +use nyx::propagators::{IntegratorOptions, Propagator}; +use nyx::time::{Epoch, TimeUnits, Unit}; +use nyx::utils::rss_orbit_errors; +use nyx::Spacecraft; +use nyx_space::mc::StateDispersion; +use nyx_space::propagators::IntegratorMethod; +use std::collections::BTreeMap; +use std::env; +use std::path::PathBuf; + +use anise::{constants::frames::EARTH_J2000, prelude::Almanac}; +use rstest::*; +use std::sync::Arc; + +#[fixture] +fn almanac() -> Arc { + use crate::test_almanac_arcd; + test_almanac_arcd() +} + +/// Using identically configured ground stations for all tests. +#[fixture] +fn devices(almanac: Arc) -> BTreeMap { + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + let elevation_mask = 10.0; + let integration_time = None; //Some(60 * Unit::Second); + + let dss65_madrid = GroundStation::dss65_madrid( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) + .with_integration_time(integration_time); + + let dss34_canberra = GroundStation::dss34_canberra( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) + .with_integration_time(integration_time); + + let dss13_goldstone = GroundStation::dss13_goldstone( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ) + .with_msr_type( + MeasurementType::Azimuth, + StochasticNoise::default_angle_deg(), + ) + .with_msr_type( + MeasurementType::Elevation, + StochasticNoise::default_angle_deg(), + ) + .with_integration_time(integration_time); + + let mut devices = BTreeMap::new(); + devices.insert(dss65_madrid.name(), dss65_madrid); + devices.insert(dss34_canberra.name(), dss34_canberra); + devices.insert(dss13_goldstone.name(), dss13_goldstone); + + devices +} + +#[fixture] +fn initial_state(almanac: Arc) -> Spacecraft { + // Define state information. + let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let dt = Epoch::from_gregorian_utc_at_midnight(2020, 1, 1); + Spacecraft::from(Orbit::keplerian( + 22000.0, 0.01, 30.0, 80.0, 40.0, 0.0, dt, eme2k, + )) +} + +#[fixture] +fn initial_estimate(initial_state: Spacecraft) -> KfEstimate { + // Disperse the initial state based on some orbital elements errors given from ULA Atlas 5 user guide, table 2.3.3-1 + // This assumes that the errors are ONE TENTH of the values given in the table. It assumes that the launch provider has provided an initial state vector, whose error is lower than the injection errors. + // The initial covariance is computed based on the realized dispersions. + let initial_estimate = KfEstimate::disperse_from_diag( + initial_state, + 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); + + initial_estimate +} + +#[fixture] +fn truth_setup() -> Propagator { + let step_size = 60.0 * Unit::Second; + let opts = IntegratorOptions::with_max_step(step_size); + + let bodies = vec![MOON, SUN, JUPITER_BARYCENTER, SATURN_BARYCENTER]; + let orbital_dyn = OrbitalDynamics::point_masses(bodies); + Propagator::new( + SpacecraftDynamics::new(orbital_dyn), + IntegratorMethod::DormandPrince78, + opts, + ) +} + +#[fixture] +fn estimator_setup() -> Propagator { + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be _nearly_ perfect because we've removed SATURN_BARYCENTER from the estimated trajectory + + let step_size = 60.0 * Unit::Second; + let opts = IntegratorOptions::with_max_step(step_size); + + let bodies = vec![MOON, SUN, JUPITER_BARYCENTER, SATURN_BARYCENTER]; + let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies)); + + Propagator::new(estimator, IntegratorMethod::DormandPrince78, opts) +} + +/* + * These tests check that if we start with a state deviation in the estimate, the filter will eventually converge back. + * These tests do NOT check that the filter will converge if the initial state in the propagator has that state deviation. + * The latter would require iteration and smoothing before playing with an EKF. This will be handled in a subsequent version. +**/ + +#[allow(clippy::identity_op)] +#[rstest] +fn od_robust_test_ekf_rng_dop_az_el( + almanac: Arc, + devices: BTreeMap, + initial_state: Spacecraft, + initial_estimate: KfEstimate, + truth_setup: Propagator, + estimator_setup: Propagator, +) { + let _ = pretty_env_logger::try_init(); + + let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); + // Define the ground stations. + let ekf_num_meas = 10; + // Set the disable time to be very low to test enable/disable sequence + let ekf_disable_time = 3 * Unit::Minute; + let elevation_mask = 0.0; + + let dss65_madrid = GroundStation::dss65_madrid( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ); + let dss34_canberra = GroundStation::dss34_canberra( + elevation_mask, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), + iau_earth, + ); + + // Define the tracking configurations + let configs = BTreeMap::from([ + ( + dss65_madrid.name.clone(), + TrkConfig::from_sample_rate(60.seconds()), + ), + ( + dss34_canberra.name.clone(), + TrkConfig::from_sample_rate(60.seconds()), + ), + ]); + + // Define the propagator information. + let prop_time = 1.1 * initial_state.orbit.period().unwrap(); + + let initial_state_dev = initial_estimate.nominal_state; + let (init_rss_pos_km, init_rss_vel_km_s) = + rss_orbit_errors(&initial_state.orbit, &initial_state_dev.orbit); + + 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{:.3} m\t{:.3} m/s\n{}", + init_rss_pos_km * 1e3, + init_rss_vel_km_s * 1e3, + (initial_state.orbit - initial_state_dev.orbit).unwrap() + ); + + let (_, traj) = truth_setup + .with(initial_state, almanac.clone()) + .for_duration_with_traj(prop_time) + .unwrap(); + + // Simulate tracking data + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); + arc_sim.build_schedule(almanac.clone()).unwrap(); + + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); + + // And serialize to disk + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "output_data", + "ekf_rng_dpl_az_el_arc.parquet", + ] + .iter() + .collect(); + + arc.to_parquet_simple(&path).unwrap(); + + println!("{arc}\n{arc:?}"); + // Reload + let reloaded = TrackingDataArc::from_parquet(&path).unwrap(); + assert_eq!(reloaded, arc); + + let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); + + // Define the process noise to assume an unmodeled acceleration on X, Y and Z in the EME2000 frame + let sigma_q = 5e-10_f64.powi(2); + let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); + + let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); + + // Run with all data simultaneously + let mut odp_simul = ODProcess::< + SpacecraftDynamics, + Const<4>, + Const<3>, + KF, Const<4>>, + GroundStation, + >::ekf( + prop_est, + KF::new(initial_estimate, process_noise.clone()), + devices.clone(), + trig, + None, + almanac.clone(), + ); + + odp_simul.process_arc(&arc).unwrap(); + + odp_simul + .to_parquet( + &arc, + path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"), + ExportCfg::default(), + ) + .unwrap(); + + // Check that the covariance deflated + let est = &odp_simul.estimates[odp_simul.estimates.len() - 1]; + let final_truth_state = traj.at(est.epoch()).unwrap(); + + println!("Estimate:\n{}", est); + println!("Truth:\n{}", final_truth_state); + println!( + "Delta state with truth (epoch match: {}):\n{}", + final_truth_state.epoch() == est.epoch(), + (final_truth_state.orbit - est.state().orbit).unwrap() + ); + + for i in 0..6 { + if est.covar[(i, i)] < 0.0 { + println!( + "covar diagonal element negative @ [{}, {}] = {:.3e}-- issue #164", + i, + i, + est.covar[(i, i)], + ); + } + } + + assert_eq!( + final_truth_state.epoch(), + est.epoch(), + "time of final EST and TRUTH epochs differ" + ); + let delta = (est.state().orbit - final_truth_state.orbit).unwrap(); + println!( + "RMAG error = {:.6} m\tVMAG error = {:.6} m/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e3 + ); + + assert!( + delta.rmag_km() < 0.01, + "Position error should be less than 10 meters" + ); + assert!( + delta.vmag_km_s() < 2e-4, + "Velocity error should be on centimeter level" + ); + + // We get the best results with all data simultaneously, let's rerun with then two-by-two. + let prop_est = estimator_setup.with(initial_state_dev.with_stm(), almanac.clone()); + let mut odp_2by2 = SpacecraftODProcess::ekf( + prop_est, + KF::new(initial_estimate, process_noise.clone()), + devices, + trig, + None, + almanac, + ); + + odp_2by2.process_arc(&arc).unwrap(); + let est_2by2 = &odp_2by2.estimates[odp_2by2.estimates.len() - 1]; + + let delta_2by2 = (est.state().orbit - est_2by2.state().orbit).unwrap(); + println!( + "RMAG diff = {:.6} m\tVMAG diff = {:.6} m/s", + delta_2by2.rmag_km() * 1e3, + delta_2by2.vmag_km_s() * 1e3 + ); + + assert!( + delta_2by2.rmag_km() < 0.1, + "Position error should be less than 100 meters" + ); + assert!( + delta_2by2.vmag_km_s() < 1e-1, + "Velocity error should be on decimeter level" + ); +} diff --git a/tests/orbit_determination/simulator.rs b/tests/orbit_determination/simulator.rs index 0abe7346..20234444 100644 --- a/tests/orbit_determination/simulator.rs +++ b/tests/orbit_determination/simulator.rs @@ -1,8 +1,6 @@ -use nyx_space::io::tracking_data::DynamicTrackingArc; use nyx_space::io::ConfigRepr; use nyx_space::md::prelude::*; use nyx_space::md::trajectory::ExportCfg; -use nyx_space::od::msr::RangeDoppler; use nyx_space::od::prelude::*; use nyx_space::od::simulator::TrackingArcSim; use nyx_space::od::simulator::TrkConfig; @@ -78,9 +76,10 @@ fn continuous_tracking(almanac: Arc) { .iter() .collect(); - let devices = GroundStation::load_many(ground_station_file).unwrap(); - - // dbg!(&devices); + let mut devices = BTreeMap::new(); + for gs in GroundStation::load_many(ground_station_file).unwrap() { + devices.insert(gs.name.clone(), gs); + } // Load the tracking configuration from the test data. let trkconfg_yaml: PathBuf = [ @@ -98,10 +97,9 @@ fn continuous_tracking(almanac: Arc) { dbg!(&configs); // Build the tracking arc simulation to generate a "standard measurement". - let mut trk = TrackingArcSim::::with_seed( - devices, trajectory, configs, 12345, - ) - .unwrap(); + let mut trk = + TrackingArcSim::::with_seed(devices, trajectory, configs, 12345) + .unwrap(); trk.build_schedule(almanac.clone()).unwrap(); let arc = trk.generate_measurements(almanac).unwrap(); @@ -119,17 +117,12 @@ fn continuous_tracking(almanac: Arc) { println!("[{}] {arc}", output_fn.to_string_lossy()); // Now read this file back in. - let dyn_arc = DynamicTrackingArc::from_parquet(output_fn).unwrap(); - // And convert to the same tracking arc as earlier - let arc_concrete = dyn_arc.to_tracking_arc::().unwrap(); + let arc_concrete = TrackingDataArc::from_parquet(output_fn).unwrap(); println!("{arc_concrete}"); assert_eq!(arc.measurements.len(), 116); // Check that we've loaded all of the measurements assert_eq!(arc_concrete.measurements.len(), arc.measurements.len()); - // Check that we find the same device names too - assert_eq!(arc_concrete.device_names(), arc.device_names()); - // Check that we've copied over the device configurations as well - assert_eq!(arc_concrete.device_cfg, arc.device_cfg); + assert_eq!(arc_concrete.unique(), arc.unique()); } diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index 778b9162..751e3680 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -29,7 +29,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -51,12 +51,16 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -78,15 +82,19 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + devices } #[allow(clippy::identity_op)] #[rstest] fn od_val_sc_mb_srp_reals_duals_models( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { /* * This tests that the state transition matrix computation is correct when multiple celestial gravities and solar radiation pressure @@ -116,12 +124,10 @@ fn od_val_sc_mb_srp_reals_duals_models( }]) .build(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } - let all_stations = sim_devices; - // Define the propagator information. let step_size = 10.0 * Unit::Second; let opts = IntegratorOptions::with_fixed_step(step_size); @@ -178,11 +184,10 @@ fn od_val_sc_mb_srp_reals_duals_models( .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(sim_devices, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.to_parquet_simple(path.with_file_name("sc_msr_arc.parquet")) .unwrap(); @@ -214,11 +219,12 @@ fn od_val_sc_mb_srp_reals_duals_models( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); odp.to_parquet( + &arc, path.with_file_name("spacecraft_od_results.parquet"), ExportCfg::timestamped(), ) @@ -281,10 +287,10 @@ fn od_val_sc_mb_srp_reals_duals_models( #[allow(clippy::identity_op)] #[rstest] -fn od_val_sc_srp_estimation( +fn od_val_sc_srp_estimation_cov_test( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { /* * This tests that we can correctly estimate the solar radiation pressure. @@ -357,8 +363,8 @@ fn od_val_sc_srp_estimation( }]) .build(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -368,8 +374,7 @@ fn od_val_sc_srp_estimation( TrackingArcSim::with_seed(all_stations, traj.clone(), configs.clone(), 120).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); arc.to_parquet_simple(path.with_file_name("sc_srp_msr_arc.parquet")) .unwrap(); @@ -407,17 +412,19 @@ fn od_val_sc_srp_estimation( SNC3::from_diagonal(2 * Unit::Minute, &[1e-15, 1e-15, 1e-15]), ); - let mut odp = ODProcess::ekf( + let mut odp = SpacecraftODProcess::ekf( prop_est, ckf, + proc_devices, EkfTrigger::new(30, Unit::Minute * 2), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); odp.to_parquet( + &arc, path.with_file_name("sc_od_with_srp.parquet"), ExportCfg::default(), ) @@ -442,8 +449,8 @@ fn od_val_sc_srp_estimation( delta.vmag_km_s() * 1e6 ); - assert!(delta.rmag_km() < 1e-3, "More than 1 meter error"); - assert!(delta.vmag_km_s() < 1e-6, "More than 1 millimeter/s error"); + assert!(delta.rmag_km() < 2e-3, "More than 2 meter error"); + assert!(delta.vmag_km_s() < 2e-6, "More than 2 millimeter/s error"); assert!( (est.state().srp.cr - truth_cr).abs() < (1.5 - truth_cr), "Cr estimation did not improve" diff --git a/tests/orbit_determination/trackingarc.rs b/tests/orbit_determination/trackingarc.rs index ef0d6860..111baabe 100644 --- a/tests/orbit_determination/trackingarc.rs +++ b/tests/orbit_determination/trackingarc.rs @@ -1,8 +1,6 @@ use anise::constants::frames::{EARTH_J2000, IAU_EARTH_FRAME}; -use nyx_space::io::tracking_data::DynamicTrackingArc; use nyx_space::io::ConfigRepr; use nyx_space::md::prelude::*; -use nyx_space::od::msr::RangeDoppler; use nyx_space::od::prelude::*; use nyx_space::od::simulator::TrackingArcSim; use nyx_space::od::simulator::{Cadence, Strand, TrkConfig}; @@ -47,7 +45,7 @@ fn traj(almanac: Arc) -> Traj { } #[fixture] -fn devices() -> Vec { +fn devices() -> BTreeMap { // Load the ground stations from the test data. let ground_station_file: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -59,11 +57,20 @@ fn devices() -> Vec { .iter() .collect(); - GroundStation::load_many(ground_station_file).unwrap() + let mut devices = BTreeMap::new(); + for gs in GroundStation::load_many(ground_station_file).unwrap() { + devices.insert(gs.name.clone(), gs); + } + + devices } #[rstest] -fn trk_simple(traj: Traj, devices: Vec, almanac: Arc) { +fn trk_simple( + traj: Traj, + devices: BTreeMap, + almanac: Arc, +) { // Path to output data let path: PathBuf = [ env!("CARGO_MANIFEST_DIR"), @@ -104,7 +111,7 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< // Build the tracking arc simulation to generate a "standard measurement". let mut trk = - TrackingArcSim::::with_seed(devices, traj, configs, 12345) + TrackingArcSim::::with_seed(devices, traj, configs, 12345) .unwrap(); // Test that building the schedule is deterministic @@ -121,47 +128,6 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< let arc = trk.generate_measurements(almanac).unwrap(); - // Test filtering by epoch - let start_epoch = arc.measurements[0].1.epoch() + 1.minutes(); - for (_, msr) in arc.filter_by_epoch(start_epoch..).measurements { - assert!(msr.epoch() >= start_epoch); - } - - for (_, msr) in arc.filter_by_epoch(..=start_epoch).measurements { - assert!(msr.epoch() <= start_epoch); - } - - for (_, msr) in arc.filter_by_epoch(..start_epoch).measurements { - assert!(msr.epoch() < start_epoch); - } - - assert_eq!( - arc.filter_by_epoch(start_epoch..start_epoch) - .measurements - .len(), - 0 - ); - - // Test filtering by duration offset - for (_, msr) in arc.filter_by_offset(1.minutes()..).measurements { - assert!(msr.epoch() >= start_epoch); - } - - for (_, msr) in arc.filter_by_offset(..=1.minutes()).measurements { - assert!(msr.epoch() <= start_epoch); - } - - for (_, msr) in arc.filter_by_offset(..1.minutes()).measurements { - assert!(msr.epoch() < start_epoch); - } - - assert_eq!( - arc.filter_by_offset(1.minutes()..1.minutes()) - .measurements - .len(), - 0 - ); - // Regression assert_eq!(arc.measurements.len(), 197); @@ -178,25 +144,20 @@ fn trk_simple(traj: Traj, devices: Vec, almanac: Arc< println!("[{}] {arc}", output_fn.to_string_lossy()); // Now read this file back in. - let dyn_arc = DynamicTrackingArc::from_parquet(output_fn).unwrap(); - // And convert to the same tracking arc as earlier - let arc_concrete = dyn_arc.to_tracking_arc::().unwrap(); + let arc_concrete = TrackingDataArc::from_parquet(output_fn).unwrap(); println!("{arc_concrete}"); // Check that we've loaded all of the measurements assert_eq!(arc_concrete.measurements.len(), arc.measurements.len()); - // Check that we find the same device names too - assert_eq!(arc_concrete.device_names(), arc.device_names()); - // Check that we've copied over the device configurations as well - assert_eq!(arc_concrete.device_cfg, arc.device_cfg); + assert_eq!(arc_concrete.unique(), arc.unique()); } /// Tests that inclusion epochs work #[rstest] fn trkconfig_zero_inclusion( traj: Traj, - devices: Vec, + devices: BTreeMap, almanac: Arc, ) { // Build a tracking config that should always see this vehicle. @@ -209,10 +170,9 @@ fn trkconfig_zero_inclusion( // Build the configs map, where we only have one of the two stations configured let mut configs = BTreeMap::new(); - configs.insert(devices[1].name.clone(), trkcfg_always); + configs.insert("Canberra".to_string(), trkcfg_always); - let mut trk = - TrackingArcSim::::new(devices, traj, configs).unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); @@ -222,7 +182,7 @@ fn trkconfig_zero_inclusion( assert_eq!(arc.measurements.len(), 113); assert_eq!( - arc.device_names().len(), + arc.unique_aliases().len(), 1, "only one device should have measurements" ); @@ -230,7 +190,7 @@ fn trkconfig_zero_inclusion( /// Test invalid tracking configurations #[rstest] -fn trkconfig_invalid(traj: Traj, devices: Vec) { +fn trkconfig_invalid(traj: Traj, devices: BTreeMap) { // Build a tracking config where the exclusion range is less than the sampling rate let trkcfg = TrkConfig::builder() .strands(vec![Strand { @@ -241,18 +201,18 @@ fn trkconfig_invalid(traj: Traj, devices: Vec) { // Build the configs map let mut configs = BTreeMap::new(); - for device in &devices { - configs.insert(device.name.clone(), trkcfg.clone()); + for name in devices.keys() { + configs.insert(name.clone(), trkcfg.clone()); } - assert!(TrackingArcSim::::new(devices, traj, configs).is_err()); + assert!(TrackingArcSim::::new(devices, traj, configs).is_err()); } /// Test a delayed start of the configuration #[rstest] -fn trkconfig_delayed_start( +fn trkconfig_delayed_start_cov_test( traj: Traj, - devices: Vec, + mut devices: BTreeMap, almanac: Arc, ) { let trkcfg = TrkConfig::builder() @@ -263,14 +223,13 @@ fn trkconfig_delayed_start( .sampling(1.26.minutes()) .build(); + devices.remove("Canberra").unwrap(); + // Build the configs map with a single ground station let mut configs = BTreeMap::new(); + configs.insert("Demo ground station".to_string(), trkcfg); - configs.insert(devices[0].name.clone(), trkcfg); - - let mut trk = - TrackingArcSim::::new(vec![devices[0].clone()], traj, configs) - .unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); @@ -289,12 +248,16 @@ fn trkconfig_delayed_start( /// Test different cadences and availabilities #[rstest] -fn trkconfig_cadence(traj: Traj, devices: Vec, almanac: Arc) { +fn trkconfig_cadence_cov_test( + traj: Traj, + devices: BTreeMap, + almanac: Arc, +) { // Build the configs map with a single ground station let mut configs = BTreeMap::new(); configs.insert( - devices[0].name.clone(), + "Demo ground station".to_string(), TrkConfig::builder() .scheduler( Scheduler::builder() @@ -308,15 +271,14 @@ fn trkconfig_cadence(traj: Traj, devices: Vec, almana ); configs.insert( - devices[1].name.clone(), + "Canberra".to_string(), TrkConfig::builder() .sampling(26.1.seconds()) .scheduler(Scheduler::default()) .build(), ); - let mut trk = - TrackingArcSim::::new(devices, traj, configs).unwrap(); + let mut trk = TrackingArcSim::::new(devices, traj, configs).unwrap(); trk.build_schedule(almanac.clone()).unwrap(); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index fed2091c..96b6aa9e 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -2,6 +2,7 @@ extern crate nyx_space as nyx; extern crate pretty_env_logger; use anise::constants::frames::IAU_EARTH_FRAME; +use nalgebra::U2; use nyx::cosmic::Orbit; use nyx::dynamics::orbital::OrbitalDynamics; use nyx::dynamics::sph_harmonics::Harmonics; @@ -28,7 +29,7 @@ fn almanac() -> Arc { } #[fixture] -fn sim_devices(almanac: Arc) -> Vec { +fn sim_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -50,12 +51,17 @@ fn sim_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + + devices } /// Devices for processing the measurement, noise may not be zero. #[fixture] -fn proc_devices(almanac: Arc) -> Vec { +fn proc_devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( @@ -77,15 +83,20 @@ fn proc_devices(almanac: Arc) -> Vec { iau_earth, ); - vec![dss65_madrid, dss34_canberra, dss13_goldstone] + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices.insert("Goldstone".to_string(), dss13_goldstone); + + devices } #[allow(clippy::identity_op)] #[rstest] fn od_tb_val_ekf_fixed_step_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -108,8 +119,8 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let cfg = TrkConfig::load(trkconfig_yaml).unwrap(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -136,8 +147,7 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -163,15 +173,16 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ekf( + let mut odp = ODProcess::<_, U2, _, _, _>::ekf( prop_est, kf, + proc_devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates[odp.estimates.len() - 1]; @@ -221,8 +232,8 @@ fn od_tb_val_ekf_fixed_step_perfect_stations( #[rstest] fn od_tb_val_with_arc( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -283,8 +294,7 @@ fn od_tb_val_with_arc( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 1).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // And serialize to disk let path: PathBuf = [ @@ -321,15 +331,16 @@ fn od_tb_val_with_arc( let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ekf( + let mut odp = ODProcess::<_, U2, _, _, _>::ekf( prop_est, kf, + proc_devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), Some(ResidRejectCrit::default()), almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates[odp.estimates.len() - 1]; @@ -375,12 +386,22 @@ fn od_tb_val_with_arc( assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); } +#[fixture] +fn cfg() -> TrkConfig { + // Define the tracking configurations + TrkConfig::builder() + .sampling(10.seconds()) + .scheduler(Scheduler::builder().sample_alignment(10.seconds()).build()) + .build() +} + #[allow(clippy::identity_op)] #[rstest] fn od_tb_val_ckf_fixed_step_perfect_stations( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, + cfg: TrkConfig, ) { /* * This tests that the state transition matrix computation is correct with two body dynamics. @@ -397,15 +418,9 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( **/ let _ = pretty_env_logger::try_init(); - // Define the tracking configurations - let cfg = TrkConfig::builder() - .sampling(10.seconds()) - .scheduler(Scheduler::builder().sample_alignment(10.seconds()).build()) - .build(); - let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -432,8 +447,7 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // And serialize to disk let path: PathBuf = [ @@ -471,15 +485,15 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "tb_ckf.parquet"] .iter() .collect(); - odp.to_parquet(path, ExportCfg::default()).unwrap(); + odp.to_parquet(&arc, path, ExportCfg::default()).unwrap(); // Check that there are no duplicates of epochs. let mut prev_epoch = odp.estimates[0].epoch(); @@ -515,6 +529,269 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( ); } + for res in odp.residuals.iter().flatten() { + assert!( + res.prefit.norm() < 1e-12, + "prefit should be zero (perfect dynamics) ({:e})", + res + ); + } + + for res in odp.residuals.iter().flatten() { + assert!( + res.postfit.norm() < 1e-12, + "postfit should be zero (perfect dynamics) ({:e})", + res + ); + } + + let est = odp.estimates.last().unwrap(); + println!("estimate error {:.2e}", est.state_deviation().norm()); + println!("estimate covariance {:.2e}", est.covar.diagonal().norm()); + + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + + assert!( + est.covar.diagonal().norm() < 1e-6, + "estimate covariance norm should be zero (perfect dynamics) ({:e})", + est.covar.diagonal().norm() + ); + + let delta = (est.state().orbit - final_truth.orbit).unwrap(); + println!( + "RMAG error = {:.2e} m\tVMAG error = {:.3e} mm/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e6 + ); + + assert!(delta.rmag_km() < 2e-16, "Position error should be zero"); + assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); + + // Iterate + odp.iterate_arc( + &arc, + IterationConf { + smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), + ..Default::default() + }, + ) + .unwrap(); + + println!( + "N-1 one iteration: \n{}", + odp.estimates[odp.estimates.len() - 1] + ); + + println!( + "Initial state after iteration: \n{:x}", + odp.estimates[0].state() + ); + + // Check the final estimate + let est = odp.estimates.last().unwrap(); + println!("estimate error {:.2e}", est.state_deviation().norm()); + println!("estimate covariance {:.2e}", est.covar.diagonal().norm()); + + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + + // Note we accept a larger covariance diagonal here because smoothing will increase the covariance + assert!( + est.covar.diagonal().norm() < 1e-4, + "estimate covariance norm should be zero (perfect dynamics) ({:e})", + est.covar.diagonal().norm() + ); + + let delta = (est.state().orbit - final_truth.orbit).unwrap(); + println!( + "RMAG error = {:.2e} m\tVMAG error = {:.3e} mm/s", + delta.rmag_km() * 1e3, + delta.vmag_km_s() * 1e6 + ); + + assert!(delta.rmag_km() < 1e-9, "More than 1 micrometer error"); + assert!(delta.vmag_km_s() < 1e-9, "More than 1 micrometer/s error"); +} + +#[allow(clippy::identity_op)] +#[rstest] +fn od_tb_val_az_el_ckf_fixed_step_perfect_stations( + almanac: Arc, + mut sim_devices: BTreeMap, + mut proc_devices: BTreeMap, + cfg: TrkConfig, +) { + /* + * This tests that the state transition matrix computation is correct with two body dynamics. + * + * Specifically, the same dynamics are used for both the measurement generation and for the estimation. + * However, only the estimation generation propagates the STM. When STM propagation is enabled, the code will compute + * the dynamics using a hyperdual representation in 7 dimensions: 1 for the reals, 3 for the position partials, + * 3 for the velocity partials. + * + * Hence, if the filter state estimation is any different from the truth data, then it means that the equations of + * motion computed in hyperdual space differ from the ones computes in the reals. + * + * Thereby, this serves as a validation of the orbital dynamics implementation. + **/ + let _ = pretty_env_logger::try_init(); + + for (_, dev) in sim_devices.iter_mut() { + dev.measurement_types.swap_remove(&MeasurementType::Range); + dev.measurement_types.swap_remove(&MeasurementType::Doppler); + dev.measurement_types.insert(MeasurementType::Azimuth); + dev.measurement_types.insert(MeasurementType::Elevation); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Azimuth, StochasticNoise::ZERO); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Elevation, StochasticNoise::ZERO); + } + + for (_, dev) in proc_devices.iter_mut() { + dev.measurement_types.swap_remove(&MeasurementType::Range); + dev.measurement_types.swap_remove(&MeasurementType::Doppler); + dev.measurement_types.insert(MeasurementType::Azimuth); + dev.measurement_types.insert(MeasurementType::Elevation); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Azimuth, StochasticNoise::MIN); + dev.stochastic_noises + .as_mut() + .unwrap() + .insert(MeasurementType::Elevation, StochasticNoise::MIN); + } + + let mut configs = BTreeMap::new(); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); + } + + let all_stations = sim_devices; + + // Define the propagator information. + let prop_time = 1 * Unit::Day; + let step_size = 10.0 * Unit::Second; + let opts = IntegratorOptions::with_fixed_step(step_size); + + // Define state information. + let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); + let dt = Epoch::from_gregorian_tai_at_midnight(2020, 1, 1); + let initial_state = Orbit::keplerian(22000.0, 0.01, 30.0, 80.0, 40.0, 0.0, dt, eme2k); + + // Generate the truth data on one thread. + let orbital_dyn = SpacecraftDynamics::new(OrbitalDynamics::two_body()); + + let setup = Propagator::new(orbital_dyn, IntegratorMethod::RungeKutta4, opts); + let mut prop = setup.with(initial_state.into(), almanac.clone()); + + let (final_truth, traj) = prop.for_duration_with_traj(prop_time).unwrap(); + + // Simulate tracking data + let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); + arc_sim.build_schedule(almanac.clone()).unwrap(); + + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); + + // And serialize to disk + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "output_data", + "od_tb_val_az_el_ckf_fixed_step_perfect_stations.parquet", + ] + .iter() + .collect(); + + arc.to_parquet_simple(path).unwrap(); + + // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. + // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on + // the measurements, and the same time step. + let initial_state_est = Spacecraft::from(initial_state).with_stm(); + // Use the same setup as earlier + let prop_est = setup.with(initial_state_est, almanac.clone()); + let covar_radius_km = 1.0e-3; + let covar_velocity_km_s = 1.0e-6; + let init_covar = SMatrix::::from_diagonal(&SVector::::from_iterator([ + covar_radius_km, + covar_radius_km, + covar_radius_km, + covar_velocity_km_s, + covar_velocity_km_s, + covar_velocity_km_s, + 0.0, + 0.0, + 0.0, + ])); + + // Define the initial orbit estimate + let initial_estimate = KfEstimate::from_covar(initial_state_est, init_covar); + + let ckf = KF::no_snc(initial_estimate); + + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); + + odp.process_arc(&arc).unwrap(); + + let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "tb_ckf.parquet"] + .iter() + .collect(); + + odp.to_parquet(&arc, path, ExportCfg::default()).unwrap(); + + // Check that there are no duplicates of epochs. + let mut prev_epoch = odp.estimates[0].epoch(); + + for est in odp.estimates.iter().skip(2) { + let this_epoch = est.epoch(); + assert!( + this_epoch > prev_epoch, + "Estimates not continuously going forward: {this_epoch} <= {prev_epoch}" + ); + prev_epoch = this_epoch; + } + + for (no, est) in odp.estimates.iter().enumerate() { + if no == 0 { + // Skip the first estimate which is the initial estimate provided by user + continue; + } + for i in 0..6 { + assert!( + est.covar[(i, i)] >= 0.0, + "covar diagonal element negative @ [{}, {}] = {:e} @ {}", + i, + i, + est.covar[(i, i)], + est.epoch() + ); + } + assert!( + est.state_deviation().norm() < 1e-12, + "estimate error should be zero (perfect dynamics) ({:e})", + est.state_deviation().norm() + ); + } + + for res in odp.residuals.iter().flatten() { + assert!( + res.prefit.norm() < 1e-12, + "prefit should be zero (perfect dynamics) ({:e})", + res + ); + } + for res in odp.residuals.iter().flatten() { assert!( res.postfit.norm() < 1e-12, @@ -550,7 +827,7 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( assert!(delta.vmag_km_s() < 2e-16, "Velocity error should be zero"); // Iterate - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), @@ -602,8 +879,8 @@ fn od_tb_val_ckf_fixed_step_perfect_stations( #[rstest] fn od_tb_ckf_fixed_step_iteration_test( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { let _ = pretty_env_logger::try_init(); @@ -615,8 +892,8 @@ fn od_tb_ckf_fixed_step_iteration_test( let cfg = TrkConfig::from_sample_rate(10.seconds()); let mut configs = BTreeMap::new(); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -641,8 +918,7 @@ fn od_tb_ckf_fixed_step_iteration_test( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -670,9 +946,9 @@ fn od_tb_ckf_fixed_step_iteration_test( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check the final estimate prior to iteration let delta = (odp.estimates.last().unwrap().state().orbit - final_truth.orbit).unwrap(); @@ -692,7 +968,7 @@ fn od_tb_ckf_fixed_step_iteration_test( ); // Iterate, and check that the initial state difference is lower - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::TimeGap(10.0 * Unit::Second), @@ -743,8 +1019,8 @@ fn od_tb_ckf_fixed_step_iteration_test( #[rstest] fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -752,8 +1028,8 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -778,8 +1054,7 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -810,9 +1085,9 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( let ckf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { @@ -902,7 +1177,8 @@ fn od_tb_ckf_map_covar(almanac: Arc) { let ckf = KF::no_snc(initial_estimate); - let mut odp: SpacecraftODProcess = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp: SpacecraftODProcess = + ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, BTreeMap::new(), None, almanac); odp.predict_for(30.seconds(), duration).unwrap(); @@ -934,10 +1210,10 @@ fn od_tb_ckf_map_covar(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn od_tb_val_harmonics_ckf_fixed_step_perfect( +fn od_tb_val_harmonics_ckf_fixed_step_perfect_cov_test( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -945,8 +1221,8 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -974,8 +1250,7 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -1002,9 +1277,9 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( let ckf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { @@ -1044,8 +1319,8 @@ fn od_tb_val_harmonics_ckf_fixed_step_perfect( #[rstest] fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( almanac: Arc, - sim_devices: Vec, - proc_devices: Vec, + sim_devices: BTreeMap, + proc_devices: BTreeMap, ) { // Tests state noise compensation with covariance mapping let _ = pretty_env_logger::try_init(); @@ -1053,8 +1328,8 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( // Define the tracking configurations let mut configs = BTreeMap::new(); let cfg = TrkConfig::from_sample_rate(10.seconds()); - for device in &sim_devices { - configs.insert(device.name.clone(), cfg.clone()); + for name in sim_devices.keys() { + configs.insert(name.clone(), cfg.clone()); } let all_stations = sim_devices; @@ -1078,8 +1353,7 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj, configs.clone(), 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); - let mut arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); - arc.set_devices(proc_devices, configs).unwrap(); + let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); // Now that we have the truth data, let's start an OD with no noise at all and compute the estimates. // We expect the estimated orbit to be perfect since we're using strictly the same dynamics, no noise on @@ -1119,9 +1393,9 @@ fn od_tb_ckf_fixed_step_perfect_stations_several_snc_covar_map( let ckf = KF::with_sncs(initial_estimate, vec![process_noise1, process_noise2]); - let mut odp = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp = ODProcess::<_, U2, _, _, _>::ckf(prop_est, ckf, proc_devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Let's check that the covariance never falls below our sigma squared values for (no, est) in odp.estimates.iter().enumerate() { diff --git a/tests/orbit_determination/xhat_dev.rs b/tests/orbit_determination/xhat_dev.rs index cb8ad9a9..1490d3a6 100644 --- a/tests/orbit_determination/xhat_dev.rs +++ b/tests/orbit_determination/xhat_dev.rs @@ -33,41 +33,46 @@ fn almanac() -> Arc { * The latter would require iteration and smoothing before playing with an EKF. This will be handled in a subsequent version. **/ -#[allow(clippy::identity_op)] -#[rstest] -fn xhat_dev_test_ekf_two_body(almanac: Arc) { - let _ = pretty_env_logger::try_init(); - +#[fixture] +fn devices(almanac: Arc) -> BTreeMap { let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - let elevation_mask = 0.0; let dss65_madrid = GroundStation::dss65_madrid( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ); let dss34_canberra = GroundStation::dss34_canberra( elevation_mask, - StochasticNoise::MIN, - StochasticNoise::MIN, + StochasticNoise::default_range_km(), + StochasticNoise::default_doppler_km_s(), iau_earth, ); + let mut devices = BTreeMap::new(); + devices.insert("Madrid".to_string(), dss65_madrid); + devices.insert("Canberra".to_string(), dss34_canberra); + devices +} + +#[allow(clippy::identity_op)] +#[rstest] +fn xhat_dev_test_ekf_two_body(almanac: Arc, devices: BTreeMap) { + let _ = pretty_env_logger::try_init(); + // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 0.01 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -99,7 +104,7 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { // Simulate tracking data println!("{traj}"); - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -133,12 +138,12 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); let pre_smooth_first_est = odp.estimates[0]; let pre_smooth_num_est = odp.estimates.len(); - odp.iterate_arc::( + odp.iterate_arc( &arc, IterationConf { smoother: SmoothingArc::All, @@ -256,46 +261,29 @@ fn xhat_dev_test_ekf_two_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_multi_body(almanac: Arc) { +fn xhat_dev_test_ekf_multi_body(almanac: Arc, devices: BTreeMap) { // We seed both propagators with the same initial state, but we let a large state deviation in the filter. // This does _not_ impact the prefits, but it impacts the state deviation and therefore the state estimate. // As such, it checks that the filter can return to a nominal state. let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 500; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 10.0 * Unit::Second; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -328,7 +316,7 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -362,10 +350,10 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); - odp.iterate_arc::(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) + odp.process_arc(&arc).unwrap(); + odp.iterate_arc(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) .unwrap(); // Check that the covariance deflated @@ -429,43 +417,26 @@ fn xhat_dev_test_ekf_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_harmonics(almanac: Arc) { +fn xhat_dev_test_ekf_harmonics(almanac: Arc, devices: BTreeMap) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 5000; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 1 * Unit::Minute; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -508,7 +479,7 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -542,9 +513,9 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates.last().unwrap(); @@ -592,43 +563,26 @@ fn xhat_dev_test_ekf_harmonics(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_realistic(almanac: Arc) { +fn xhat_dev_test_ekf_realistic(almanac: Arc, devices: BTreeMap) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - // Define the ground stations. let ekf_num_meas = 500; // Set the disable time to be very low to test enable/disable sequence let ekf_disable_time = 10.0 * Unit::Second; - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; - // Define the propagator information. let prop_time = 1 * Unit::Day; let step_size = 10.0 * Unit::Second; @@ -658,7 +612,7 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -692,9 +646,9 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { let mut trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time); trig.within_sigma = 3.0; - let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); + let mut odp = SpacecraftODProcess::ekf(prop_est, kf, devices, trig, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Check that the covariance deflated let est = &odp.estimates.last().unwrap(); @@ -749,37 +703,23 @@ fn xhat_dev_test_ekf_realistic(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { +fn xhat_dev_test_ckf_smoother_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -812,7 +752,7 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -841,9 +781,9 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Smoother let smoothed_estimates = odp.smooth(SmoothingArc::All).unwrap(); @@ -1019,37 +959,23 @@ fn xhat_dev_test_ckf_smoother_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { +fn xhat_dev_test_ekf_snc_smoother_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 10.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -1082,7 +1008,7 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -1118,15 +1044,16 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { let process_noise = SNC3::from_diagonal(2 * Unit::Minute, &[sigma_q, sigma_q, sigma_q]); let kf = KF::new(initial_estimate, process_noise); - let mut odp = ODProcess::ekf( + let mut odp = SpacecraftODProcess::ekf( prop_est, kf, + devices, EkfTrigger::new(ekf_num_meas, ekf_disable_time), None, almanac, ); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Smoother let smoothed_estimates = odp.smooth(SmoothingArc::All).unwrap(); @@ -1290,37 +1217,23 @@ fn xhat_dev_test_ekf_snc_smoother_multi_body(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { +fn xhat_dev_test_ckf_iteration_multi_body( + almanac: Arc, + devices: BTreeMap, +) { let _ = pretty_env_logger::try_init(); - let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap(); - - let elevation_mask = 0.0; - let dss65_madrid = GroundStation::dss65_madrid( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - let dss34_canberra = GroundStation::dss34_canberra( - elevation_mask, - StochasticNoise::default_range_km(), - StochasticNoise::default_doppler_km_s(), - iau_earth, - ); - // Note that we do not have Goldstone so we can test enabling and disabling the EKF. // Define the tracking configurations let mut configs = BTreeMap::new(); configs.insert( - dss65_madrid.name.clone(), + "Madrid".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); configs.insert( - dss34_canberra.name.clone(), + "Canberra".to_string(), TrkConfig::from_sample_rate(10.seconds()), ); - let all_stations = vec![dss65_madrid, dss34_canberra]; // Define the propagator information. let prop_time = 1 * Unit::Day; @@ -1353,7 +1266,7 @@ fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { .unwrap(); // Simulate tracking data - let mut arc_sim = TrackingArcSim::with_seed(all_stations, traj.clone(), configs, 0).unwrap(); + let mut arc_sim = TrackingArcSim::with_seed(devices.clone(), traj.clone(), configs, 0).unwrap(); arc_sim.build_schedule(almanac.clone()).unwrap(); let arc = arc_sim.generate_measurements(almanac.clone()).unwrap(); @@ -1382,15 +1295,15 @@ fn xhat_dev_test_ckf_iteration_multi_body(almanac: Arc) { let kf = KF::no_snc(initial_estimate); - let mut odp = ODProcess::ckf(prop_est, kf, None, almanac); + let mut odp = SpacecraftODProcess::ckf(prop_est, kf, devices, None, almanac); - odp.process_arc::(&arc).unwrap(); + odp.process_arc(&arc).unwrap(); // Clone the initial estimates let pre_iteration_estimates = odp.estimates.clone(); // Iterate - odp.iterate_arc::(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) + odp.iterate_arc(&arc, IterationConf::try_from(SmoothingArc::All).unwrap()) .unwrap(); let mut rss_pos_avr = 0.0; diff --git a/tests/propagation/stopcond.rs b/tests/propagation/stopcond.rs index c11a7364..a832cdfd 100644 --- a/tests/propagation/stopcond.rs +++ b/tests/propagation/stopcond.rs @@ -28,7 +28,7 @@ fn almanac() -> Arc { } #[rstest] -fn stop_cond_3rd_apo(almanac: Arc) { +fn stop_cond_3rd_apo_cov_test(almanac: Arc) { let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); let start_dt = Epoch::from_mjd_tai(JD_J2000); diff --git a/tests/propagation/trajectory.rs b/tests/propagation/trajectory.rs index 1e3f01de..b5a95b03 100644 --- a/tests/propagation/trajectory.rs +++ b/tests/propagation/trajectory.rs @@ -428,7 +428,7 @@ fn traj_spacecraft(almanac: Arc) { #[allow(clippy::identity_op)] #[rstest] -fn traj_ephem_backward(almanac: Arc) { +fn traj_ephem_backward_cov_test(almanac: Arc) { // Test that we can correctly interpolate a spacecraft orbit let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/propulsion/closedloop_multi_oe_ruggiero.rs b/tests/propulsion/closedloop_multi_oe_ruggiero.rs index 042dc8df..88e6c190 100644 --- a/tests/propulsion/closedloop_multi_oe_ruggiero.rs +++ b/tests/propulsion/closedloop_multi_oe_ruggiero.rs @@ -154,7 +154,7 @@ fn qlaw_as_ruggiero_case_b(almanac: Arc) { } #[rstest] -fn qlaw_as_ruggiero_case_c(almanac: Arc) { +fn qlaw_as_ruggiero_case_c_cov_test(almanac: Arc) { // Source: AAS-2004-5089 let eme2k = almanac.frame_from_uid(EARTH_J2000).unwrap(); diff --git a/tests/propulsion/schedule.rs b/tests/propulsion/schedule.rs index 5e2aa1d8..b276917c 100644 --- a/tests/propulsion/schedule.rs +++ b/tests/propulsion/schedule.rs @@ -116,7 +116,7 @@ fn val_transfer_schedule_no_depl(almanac: Arc) { } #[rstest] -fn val_transfer_schedule_depl(almanac: Arc) { +fn val_transfer_schedule_depl_cov_test(almanac: Arc) { let eme2k = almanac .frame_from_uid(EARTH_J2000) .unwrap() @@ -242,7 +242,7 @@ fn val_transfer_schedule_depl(almanac: Arc) { } #[rstest] -fn val_transfer_single_maneuver_depl(almanac: Arc) { +fn val_transfer_single_maneuver_depl_cov_test(almanac: Arc) { /* This is the same test as val_transfer_schedule_depl but uses the maneuver directly as the guidance law. It should work in the same way. */ let eme2k = almanac