diff --git a/.github/ISSUE_TEMPLATE/documentation.md b/.github/ISSUE_TEMPLATE/documentation.md index c730e0f0..564c62ea 100644 --- a/.github/ISSUE_TEMPLATE/documentation.md +++ b/.github/ISSUE_TEMPLATE/documentation.md @@ -7,37 +7,21 @@ assignees: '' --- -## Description +## Description Please describe the issue or enhancement you want to report. Provide as much detail as possible to help us understand and address it. -## Documentation type +## Documentation type Tell us which of the four Diátaxis documentation types this issue or enhancement relates to: -- Tutorial -- How-to guide -- Technical reference / Math Spec -- Explanation - -## Affected area(s) - -For the selected documentation type, tell us what specific area(s) this issue relates to. Provide titles, page numbers, URLs or other locators. +- [ ] Tutorial +- [ ] How-to guide +- [ ] Technical reference / Math Spec +- [ ] Explanation ## Expected or desired behavior -Tell us what you expected to see in the documentation, or how you think it could be improved. For enhancements, describe the improvement you think could be made. - -## Additional context - -Provide any other context or screenshots that would help us understand the issue or enhancement. - -## Possible solutions (optional) - -If you have any suggestions for how to address the issue or implement the enhancement, provide them here. We appreciate any insights you have! - -## Who should review this? - -Tag any individuals, teams, or roles that would likely need to review or address this issue for the specified documentation type. +Tell us what you expected to see in the documentation, or how you think it could be improved. For enhancements, describe the improvement you think could be made, including URLs to the documentation if available. -We will do our best to direct this to the appropriate people. Thank you for your feedback! + diff --git a/.github/workflows/rust.yaml b/.github/workflows/rust.yaml index 677d11e5..b5f49de6 100644 --- a/.github/workflows/rust.yaml +++ b/.github/workflows/rust.yaml @@ -81,6 +81,11 @@ jobs: - name: Doc Test run: cargo test --doc + - name: Run examples + run: | + cargo run --example 01_orbit_prop --release + cargo run --example 02_jwst --release + lints: name: Lints runs-on: ubuntu-latest diff --git a/.gitignore b/.gitignore index cf0dbb76..7e77bdaf 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,7 @@ node_modules/ *.venv* dist/ *.parquet +*.oem *.profraw lcov.txt .DS_Store diff --git a/Cargo.toml b/Cargo.toml index 145841b5..2cba990c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "nyx-space" build = "build.rs" -version = "2.0.0-beta.1" +version = "2.0.0-rc" edition = "2021" authors = ["Christopher Rabotin "] description = "A high-fidelity space mission toolkit, with orbit propagation, estimation and some systems engineering" @@ -26,6 +26,7 @@ exclude = [ "Dockerfile*", "rustfmt.toml", "tests/GMAT_scripts/*", + "*.png", ] [badges] @@ -96,3 +97,11 @@ name = "nyx_space" # For flamegraph -- https://github.com/flamegraph-rs/flamegraph linker = "/usr/bin/clang" rustflags = ["-Clink-arg=-fuse-ld=lld", "-Clink-arg=-Wl,--no-rosegment"] + +[[example]] +name = "01_orbit_prop" +path = "examples/01_orbit_prop/main.rs" + +[[example]] +name = "02_jwst" +path = "examples/02_jwst_covar_monte_carlo/main.rs" diff --git a/README.md b/README.md index 54fdcbf3..8dba08fc 100644 --- a/README.md +++ b/README.md @@ -1,18 +1,48 @@ -# Nyx: Revolutionizing Flight Dynamics +# Nyx: Comprehensive Spaceflight Dynamics -**Blazing fast from mission concept to operations, and automation.** -- [https://nyxspace.com/](https://nyxspace.com/) +[**Empowering flight dynamics engineers with open-source software**](https://nyxspace.com/) -Nyx is provided under the AGPLv3 License. By using this software, you assume responsibility for adhering to the license. Refer to [the pricing page](https://nyxspace.com/pricing/) for an FAQ on the AGPLv3 license. +Nyx is revolutionizing the field of flight dynamics engineering as a powerful, open-source tool for mission design and orbit determination. From trajectory optimization to orbit estimation, Nyx is built for speed, automation, and scalability. It dramatically reduces simulation time compared to commercial products, and integrates seamlessly into automated workflows across various platforms. + +**Nyx has proven mission-critical reliability, already contributing to the success of three lunar missions.** + +![Static Badge](https://img.shields.io/badge/Nyx_Space-Website-orange?link=https%3A%2F%2Fnyxspace.com%2F) +![Contact Form](https://img.shields.io/badge/Nyx_Space-Contact-orange?link=https%3A%2F%2F7ug5imdtt8v.typeform.com%2Fto%2FneFvVW3p) [![nyx-space on crates.io][cratesio-image]][cratesio] [![nyx-space on docs.rs][docsrs-image]][docsrs] -[![LoC](https://tokei.rs/b1/github/nyx-space/nyx?category=lines)](https://github.com/nyx-space/nyx). [![codecov](https://codecov.io/gh/nyx-space/nyx/graph/badge.svg?token=gEiAvwzwh5)](https://codecov.io/gh/nyx-space/nyx) +# Documentation + +The documentation is currently being updated. If you have specific use cases you would like to see documented, please [open a Github issue](https://github.com/nyx-space/nyx/issues/new?assignees=&labels=Documentation&projects=&template=documentation.md&title=) or [use the contact form](https://7ug5imdtt8v.typeform.com/to/neFvVW3p). + +## Quick start + +### Rust + +To install Nyx, follow these steps: +1. Clone the repository: `git clone https://github.com/nyx-space/nyx.git` +2. Navigate to the directory: `cd nyx` +3. Run any of the [examples](./examples/), e.g. `RUST_LOG=info cargo run --example 01_orbit_prop --release` + +### Python + +For Python projects, get started by installing the library via `pip`: `pip install nyx_space`. + +**Important:** The Python package has been temporarily disabled. Refer to for details. + +# License + +Nyx is provided under the [AGPLv3 License](./LICENSE). By using this software, you assume responsibility for adhering to the license. Refer to [the pricing page](https://nyxspace.com/pricing/) for an FAQ on the AGPLv3 license. Notably, any software that incorporates, links to, or depends on Nyx must also be released under the AGPLv3 license, even if you distribute an unmodified version of Nyx. + + [cratesio-image]: https://img.shields.io/crates/v/nyx-space.svg [cratesio]: https://crates.io/crates/nyx-space [docsrs-image]: https://docs.rs/nyx-space/badge.svg -[docsrs]: https://docs.rs/nyx-space/ +[docsrs]: https://rustdoc.nyxspace.com/ + +# Author information +> Chris Rabotin is a GNC and flight dynamics engineer with a heavy background in software. -## Who am I? -An GNC and flight dynamics engineer with a heavy background in software. I currently work for Rocket Lab USA on the Blue Ghost lunar lander. -- Find me on [LinkedIn](https://www.linkedin.com/in/chrisrabotin/). +I currently work for Rocket Lab USA as the lead flight dynamics engineer on both Blue Ghost lunar lander missions. -- Find me on [LinkedIn](https://www.linkedin.com/in/chrisrabotin/). diff --git a/examples/01_orbit_prop/README.md b/examples/01_orbit_prop/README.md new file mode 100644 index 00000000..3ff583e2 --- /dev/null +++ b/examples/01_orbit_prop/README.md @@ -0,0 +1,37 @@ +# Orbit propagation + +Propagating an orbit is the core of any astrodynamics analysis. In this example, you'll learn how to propagate an orbit state using basic two body dynamics or using a high fidelity propagator with the point masses of the Moon and Sun, solar radiation pressure, and a 21x21 gravity field. + +Then, you'll learn how to build some data products as dataframes using polars, and how to export the propagated trajectory as a CCSDS OEM file. + +To run this example, just execute: +```sh +RUST_LOG=info cargo run --example 01_orbit_prop --release +``` + +Building in `release` mode will make the computation significantly faster. Specifying `RUST_LOG=info` will allow you to see all of the information messages happening in ANISE and Nyx throughout the execution of the program. + +## Data products + +1. Export the trajectory as a CCSDS OEM version 2.0 file and as a parquet file, which includes the Keplerian orbital elements. This can be quickly analyzed and plotted in Python. +2. Compare the difference in the radial, in-track, cross-track frame between the high fidelity and two-body/Keplerian propagation. The RIC frame is commonly used to compute the difference in position and velocity of different spacecraft. Build a Dataframe from this data and print +3. Build a Dataframe containing the azimuth, elevation, range, and range-rate data of that spacecraft as seen from Boulder, CO, USA. Print out when the spacecraft has an elevation of 15 degrees or above on the horizon. + +## Force models + +The force models used here are akin to STK's "HPOP" propagator. Specifically, this example runs with the following models: +- Point masses of the Earth, Moon, and Sun, where the gravitational parameters are sourced from NASA's [pck00011.tpc](https://naif.jpl.nasa.gov/pub/naif/generic_kernels/pck/pck00011.tpc) file (note that these differ slightly from GMAT's values). The planetary ephemeris used is the DE440s.bsp, provided by NASA as well. +- Solar radiation pressure, with only the Earth as an eclipsing body +- Spherical Harmonics of the Earth of order and degree 21x21, computed in the IAU Earth frame. The data comes from the JGM3 gravitational model, downloaded automatically using ANISE's `MetaFile` downloading and local caching mechanism. This is the default gravity field model of GMAT. + +## Quick analysis + +In two body propagation, all orbital elements remain constant apart from the true anomaly. In real life, the oblateness of the Earth causes the right ascension of the ascending node to drift with time (red line below). The other force models also affect the overall orbit. + +![RAAN, AOP, INC over time](./cubesat-angles-v-time.png) + +![SMA (km) over time](./cubesat-sma-v-time.png) + +![ECC over time](./cubesat-ecc-v-time.png) + +_Note_: These plots were generated with an SRP area of that was ten times larger than the correct value, hence you may notice slightly different Keplerian orbital elements, notably for the change in the shape of the orbit. diff --git a/examples/01_orbit_prop/cubesat-angles-v-time.png b/examples/01_orbit_prop/cubesat-angles-v-time.png new file mode 100644 index 00000000..090b4538 Binary files /dev/null and b/examples/01_orbit_prop/cubesat-angles-v-time.png differ diff --git a/examples/01_orbit_prop/cubesat-ecc-v-time.png b/examples/01_orbit_prop/cubesat-ecc-v-time.png new file mode 100644 index 00000000..14f5bb13 Binary files /dev/null and b/examples/01_orbit_prop/cubesat-ecc-v-time.png differ diff --git a/examples/01_orbit_prop/cubesat-sma-v-time.png b/examples/01_orbit_prop/cubesat-sma-v-time.png new file mode 100644 index 00000000..6c01d3d7 Binary files /dev/null and b/examples/01_orbit_prop/cubesat-sma-v-time.png differ diff --git a/examples/01_orbit_prop/main.rs b/examples/01_orbit_prop/main.rs new file mode 100644 index 00000000..0667f3e2 --- /dev/null +++ b/examples/01_orbit_prop/main.rs @@ -0,0 +1,280 @@ +extern crate log; +extern crate nyx_space as nyx; +extern crate pretty_env_logger as pel; + +use anise::{ + almanac::metaload::MetaFile, + constants::{ + celestial_objects::{MOON, SUN}, + frames::{EARTH_J2000, IAU_EARTH_FRAME}, + }, +}; +use hifitime::{Epoch, Unit}; +use log::warn; +use nyx::{ + cosmic::{MetaAlmanac, Orbit, SrpConfig}, + dynamics::{Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, + io::{gravity::HarmonicsMem, ExportCfg}, + od::GroundStation, + propagators::Propagator, + Spacecraft, State, +}; +use polars::{df, series::ChunkCompare}; + +use std::{error::Error, sync::Arc}; + +fn main() -> Result<(), Box> { + pel::init(); + // Dynamics models require planetary constants and ephemerides to be defined. + // Let's start by grabbing those by using ANISE's latest MetaAlmanac. + // This will automatically download the DE440s planetary ephemeris, + // the daily-updated Earth Orientation Parameters, the high fidelity Moon orientation + // parameters (for the Moon Mean Earth and Moon Principal Axes frames), and the PCK11 + // planetary constants kernels. + // For details, refer to https://github.com/nyx-space/anise/blob/master/data/latest.dhall. + // Note that we place the Almanac into an Arc so we can clone it cheaply and provide read-only + // references to many functions. + let almanac = Arc::new(MetaAlmanac::latest().map_err(Box::new)?); + // Define the orbit epoch + let epoch = Epoch::from_gregorian_utc_hms(2024, 2, 29, 12, 13, 14); + + // Define the orbit. + // First we need to fetch the Earth J2000 from information from the Almanac. + // This allows the frame to include the gravitational parameters and the shape of the Earth, + // defined as a tri-axial ellipoid. Note that this shape can be changed manually or in the Almanac + // by loading a different set of planetary constants. + let earth_j2000 = almanac.frame_from_uid(EARTH_J2000)?; + + let orbit = + Orbit::try_keplerian_altitude(300.0, 0.015, 68.5, 65.2, 75.0, 0.0, epoch, earth_j2000)?; + // Print in in Keplerian form. + println!("{orbit:x}"); + + // There are two ways to propagate an orbit. We can make a quick approximation assuming only two-body + // motion. This is a useful first order approximation but it isn't used in real-world applications. + + // This approach is a feature of ANISE. + let future_orbit_tb = orbit.at_epoch(epoch + Unit::Day * 3)?; + println!("{future_orbit_tb:x}"); + + // Two body propagation relies solely on Kepler's laws, so only the true anomaly will change. + println!( + "SMA changed by {:.3e} km", + orbit.sma_km()? - future_orbit_tb.sma_km()? + ); + println!( + "ECC changed by {:.3e}", + orbit.ecc()? - future_orbit_tb.ecc()? + ); + println!( + "INC changed by {:.3e} deg", + orbit.inc_deg()? - future_orbit_tb.inc_deg()? + ); + println!( + "RAAN changed by {:.3e} deg", + orbit.raan_deg()? - future_orbit_tb.raan_deg()? + ); + println!( + "AOP changed by {:.3e} deg", + orbit.aop_deg()? - future_orbit_tb.aop_deg()? + ); + println!( + "TA changed by {:.3} deg", + orbit.ta_deg()? - future_orbit_tb.ta_deg()? + ); + + // Nyx is used for high fidelity propagation, not Keplerian propagation as above. + // Nyx only propagates Spacecraft at the moment, which allows it to account for acceleration + // models such as solar radiation pressure. + + // Let's build a cubesat sized spacecraft, with an SRP area of 10 cm^2 and a mass of 9.6 kg. + let sc = Spacecraft::builder() + .orbit(orbit) + .dry_mass_kg(9.60) + .srp(SrpConfig { + area_m2: 10e-4, + cr: 1.1, + }) + .build(); + println!("{sc:x}"); + + // Set up the spacecraft dynamics. + + // Specify that the orbital dynamics must account for the graviational pull of the Moon and the Sun. + // The gravity of the Earth will also be accounted for since the spaceraft in an Earth orbit. + let mut orbital_dyn = OrbitalDynamics::point_masses(vec![MOON, SUN]); + + // We want to include the spherical harmonics, so let's download the gravitational data from the Nyx Cloud. + // We're using the JGM3 model here, which is the default in GMAT. + let mut jgm3_meta = MetaFile { + uri: "http://public-data.nyxspace.com/nyx/models/JGM3.cof.gz".to_string(), + crc32: Some(0xF446F027), // Specifying the CRC32 avoids redownloading it if it's cached. + }; + // And let's download it if we don't have it yet. + jgm3_meta.process()?; + + // Build the spherical harmonics. + // The harmonics must be computed in the body fixed frame. + // We're using the long term prediction of the Earth centered Earth fixed frame, IAU Earth. + let harmonics_21x21 = Harmonics::from_stor( + almanac.frame_from_uid(IAU_EARTH_FRAME)?, + HarmonicsMem::from_cof(&jgm3_meta.uri, 21, 21, true).unwrap(), + ); + + // Include the spherical harmonics into the orbital dynamics. + orbital_dyn.accel_models.push(harmonics_21x21); + + // We define the solar radiation pressure, using the default solar flux and accounting only + // for the eclipsing caused by the Earth. + let srp_dyn = SolarPressure::default(EARTH_J2000, almanac.clone())?; + + // Finalize setting up the dynamics, specifying the force models (orbital_dyn) separately from the + // acceleration models (SRP in this case). Use `from_models` to specify multiple accel models. + let dynamics = SpacecraftDynamics::from_model(orbital_dyn, srp_dyn); + + println!("{dynamics}"); + + // Finally, let's propagate this orbit to the same epoch as above. + // The first returned value is the spacecraft state at the final epoch. + // The second value is the full trajectory where the step size is variable step used by the propagator. + let (future_sc, trajectory) = Propagator::default(dynamics) + .with(sc, almanac.clone()) + .until_epoch_with_traj(future_orbit_tb.epoch)?; + + println!("=== High fidelity propagation ==="); + println!( + "SMA changed by {:.3} km", + orbit.sma_km()? - future_sc.orbit.sma_km()? + ); + println!( + "ECC changed by {:.6}", + orbit.ecc()? - future_sc.orbit.ecc()? + ); + println!( + "INC changed by {:.3e} deg", + orbit.inc_deg()? - future_sc.orbit.inc_deg()? + ); + println!( + "RAAN changed by {:.3} deg", + orbit.raan_deg()? - future_sc.orbit.raan_deg()? + ); + println!( + "AOP changed by {:.3} deg", + orbit.aop_deg()? - future_sc.orbit.aop_deg()? + ); + println!( + "TA changed by {:.3} deg", + orbit.ta_deg()? - future_sc.orbit.ta_deg()? + ); + + // We also have access to the full trajectory throughout the propagation. + println!("{trajectory}"); + + // With the trajectory, let's build a few data products. + + // 1. Export the trajectory as a CCSDS OEM version 2.0 file and as a parquet file, which includes the Keplerian orbital elements. + + trajectory.to_oem_file( + "./01_cubesat_hf_prop.oem", + ExportCfg::builder().step(Unit::Minute * 2).build(), + )?; + + trajectory.to_parquet_with_cfg( + "./01_cubesat_hf_prop.parquet", + ExportCfg::builder().step(Unit::Minute * 2).build(), + almanac.clone(), + )?; + + // 2. Compare the difference in the radial-intrack-crosstrack frame between the high fidelity + // and Keplerian propagation. The RIC frame is commonly used to compute the difference in position + // and velocity of different spacecraft. + // 3. Compute the azimuth, elevation, range, and range-rate data of that spacecraft as seen from Boulder, CO, USA. + + let boulder_station = GroundStation::from_point( + "Boulder, CO, USA".to_string(), + 40.014984, // latitude in degrees + -105.270546, // longitude in degrees + 1.6550, // altitude in kilometers + almanac.frame_from_uid(IAU_EARTH_FRAME)?, + ); + + // We iterate over the trajectory, grabbing a state every two minutes. + let mut offset_s = vec![]; + let mut epoch_str = vec![]; + let mut ric_x_km = vec![]; + let mut ric_y_km = vec![]; + let mut ric_z_km = vec![]; + let mut ric_vx_km_s = vec![]; + let mut ric_vy_km_s = vec![]; + let mut ric_vz_km_s = vec![]; + + let mut azimuth_deg = vec![]; + let mut elevation_deg = vec![]; + let mut range_km = vec![]; + let mut range_rate_km_s = vec![]; + for state in trajectory.every(Unit::Minute * 2) { + // Try to compute the Keplerian/two body state just in time. + // This method occasionally fails to converge on an appropriate true anomaly + // from the mean anomaly. If that happens, we just skip this state. + // The high fidelity and Keplerian states diverge continuously, and we're curious + // about the divergence in this quick analysis. + let this_epoch = state.epoch(); + match orbit.at_epoch(this_epoch) { + Ok(tb_then) => { + offset_s.push((this_epoch - orbit.epoch).to_seconds()); + epoch_str.push(format!("{this_epoch}")); + // Compute the two body state just in time. + let ric = state.orbit.ric_difference(&tb_then)?; + ric_x_km.push(ric.radius_km.x); + ric_y_km.push(ric.radius_km.y); + ric_z_km.push(ric.radius_km.z); + ric_vx_km_s.push(ric.velocity_km_s.x); + ric_vy_km_s.push(ric.velocity_km_s.y); + ric_vz_km_s.push(ric.velocity_km_s.z); + + // Compute the AER data for each state. + let aer = almanac.azimuth_elevation_range_sez( + state.orbit, + boulder_station.to_orbit(this_epoch, &almanac)?, + )?; + azimuth_deg.push(aer.azimuth_deg); + elevation_deg.push(aer.elevation_deg); + range_km.push(aer.range_km); + range_rate_km_s.push(aer.range_rate_km_s); + } + Err(e) => warn!("{} {e}", state.epoch()), + }; + } + + // Build the data frames. + let ric_df = df!( + "Offset (s)" => offset_s.clone(), + "Epoch" => epoch_str.clone(), + "RIC X (km)" => ric_x_km, + "RIC Y (km)" => ric_y_km, + "RIC Z (km)" => ric_z_km, + "RIC VX (km/s)" => ric_vx_km_s, + "RIC VY (km/s)" => ric_vy_km_s, + "RIC VZ (km/s)" => ric_vz_km_s, + )?; + + println!("RIC difference at start\n{}", ric_df.head(Some(10))); + println!("RIC difference at end\n{}", ric_df.tail(Some(10))); + + let aer_df = df!( + "Offset (s)" => offset_s.clone(), + "Epoch" => epoch_str.clone(), + "azimuth (deg)" => azimuth_deg, + "elevation (deg)" => elevation_deg, + "range (km)" => range_km, + "range rate (km/s)" => range_rate_km_s, + )?; + + // Finally, let's see when the spacecraft is visible, assuming 15 degrees minimum elevation. + let mask = aer_df.column("elevation (deg)")?.gt(15.0)?; + let cubesat_visible = aer_df.filter(&mask)?; + + println!("{cubesat_visible}"); + + Ok(()) +} diff --git a/examples/02_jwst_covar_monte_carlo/README.md b/examples/02_jwst_covar_monte_carlo/README.md new file mode 100644 index 00000000..554e9585 --- /dev/null +++ b/examples/02_jwst_covar_monte_carlo/README.md @@ -0,0 +1,122 @@ +# James Webb Space Telescope covariance Monte Carlo + +Spaceflight dynamics requires a lot of statistical analysis to convince ourselves that the results we'll present to all other teams are correct. + +In this example, you'll learn how to sample the orbital state from SPICE BSP file (the industry standard for ephemeris information) and build a spacecraft structure around that orbit. Then, we'll build an uncertainty in the position and velocity of that spacecraft, and propagate it into the future. + +To run this example, just execute: +```sh +RUST_LOG=info cargo run --example 02_jwst --release +``` + +Building in `release` mode will make the computation significantly faster. Specifying `RUST_LOG=info` will allow you to see all of the information messages happening in ANISE and Nyx throughout the execution of the program. + +## Objective + +We aim to compare two different statistical methods in order to demonstrate two advanced features of Nyx. + +First, we'll use a _covariance mapping_ approach, whereby the covariance at some time `t0` is propagated to a future time `t1` along with the spacecraft trajectory. This is commonly used for the _time update_ or _prediction_ step of an orbit determination process. + +Then, we'll use the Monte Carlo framework of Nyx to propagate the initial spacecraft state after dispersions using all threads of the computer. Multi-threaded propagation is not common in other astrodynamics software. + +Finally, we'll check that the 3-sigma (i.e. 99.7%) covariance bounds of the covariance mapping approach matches the Monte Carlo results in terms of uncertainty in the state vector and in the Keplerian orbital elements. + +## Example run + +Nyx is **_blazing fast_**. The covariance mapping and the Monte Carlo runs of 5000 runs is executed in less than 30 seconds. Then it takes about 25 seconds to export all of the trajectory data into a ~61 MB parquet file. + +```sh +Finished `release` profile [optimized] target(s) in 9.53s + Running `target/release/examples/02_jwst` +INFO anise::almanac::metaload::metafile > Saved https://naif.jpl.nasa.gov/pub/naif/JWST/kernels/spk/jwst_rec.bsp to /home/crabotin/.local/share/nyx-space/anise/jwst_rec.bsp (CRC32 = a8460057) +INFO anise::almanac::metaload::metafile > Using cached /home/crabotin/.local/share/nyx-space/anise/de440s.bsp +INFO anise::almanac::metaload::metafile > Using cached /home/crabotin/.local/share/nyx-space/anise/pck11.pca +INFO anise::almanac::metaload::metafile > Discarding cached /home/crabotin/.local/share/nyx-space/anise/moon_fk.epa - CRC32 differ (got 194230817, config expected 292928914) +INFO anise::almanac::metaload::metafile > Saved http://public-data.nyxspace.com/anise/v0.4/moon_fk.epa to /home/crabotin/.local/share/nyx-space/anise/moon_fk.epa (CRC32 = b93ba21) +INFO anise::almanac::metaload::metafile > Discarding cached /home/crabotin/.local/share/nyx-space/anise/moon_pa_de440_200625.bpc - CRC32 differ (got 3454388861, config expected 1817759242) +INFO anise::almanac::metaload::metafile > Saved http://public-data.nyxspace.com/anise/moon_pa_de440_200625.bpc to /home/crabotin/.local/share/nyx-space/anise/moon_pa_de440_200625.bpc (CRC32 = cde5ca7d) +INFO anise::almanac::metaload::metafile > Saved https://naif.jpl.nasa.gov/pub/naif/generic_kernels/pck/earth_latest_high_prec.bpc to /home/crabotin/.local/share/nyx-space/anise/earth_latest_high_prec.bpc (CRC32 = 1fbb5b72) +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/de440s.bsp +INFO anise::almanac > Loading as DAF/SPK +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/pck11.pca +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/moon_fk.epa +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/moon_pa_de440_200625.bpc +INFO anise::almanac > Loading as DAF/PCK +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/earth_latest_high_prec.bpc +INFO anise::almanac > Loading as DAF/PCK +INFO anise::almanac > Loading almanac from /home/crabotin/.local/share/nyx-space/anise/jwst_rec.bsp +INFO anise::almanac > Loading as DAF/SPK +JWST defined from 2024-03-30T22:40:09.185653761 ET to 2024-06-24T00:01:09.184303103 ET +[Earth J2000] 2024-06-24T00:01:09.184303103 ET sma = 881064.546158 km ecc = 0.989962 inc = 42.614418 deg raan = 37.843422 deg aop = 62.970831 deg ta = 180.020951 deg +total mass = 6200.000 kg @ [Earth J2000] 2024-06-24T00:01:09.184303103 ET position = [76518.064167, -1396268.919369, -1057612.565026] km velocity = [0.043331, 0.014876, -0.013649] km/s Coast +RIC Σ_x = 0.5 km Σ_y = 0.3 km Σ_z = 1.5 km +RIC Σ_vx = 0.0001 km/s Σ_vy = 0.0006 km/s Σ_vz = 0.003 km/s +Σ_cr = 0 Σ_cd = 0 Σ_mass = 0 kg + +INFO nyx_space::od::process > Mapping covariance for 1 day 6 h 30 min with 1 min step +INFO nyx_space::od::process::export > Exporting orbit determination result to parquet file... +INFO nyx_space::od::process::export > Serialized 1830 estimates and residuals +INFO nyx_space::od::process::export > Orbit determination results written to ./02_jwst_covar_map.parquet in 259 ms 743 μs 488 ns +INFO nyx_space::mc::montecarlo > Propagated 5000 states in 12 s 475 ms 70 μs 85 ns +INFO nyx_space::mc::results > Exporting Monte Carlo results to parquet file... +INFO nyx_space::mc::results > Serialized 229888 states from 2024-06-24T00:01:09.184303103 ET to 2024-06-25T06:31:09.184303103 ET +INFO nyx_space::mc::results > Evaluating 2 event(s) +INFO nyx_space::mc::results > Trajectory written to 02_jwst_monte_carlo.parquet in 23 s 190 ms 416 μs 896 ns +``` + +## Analysis + +### State uncertainties + +As expected from any orbit determination software, Nyx can output uncertainties in the state vector in the integration frame and in the RIC frame. **Note:** these plots look pretty linear, but that's because we're running a pure prediction filter and JWST is in a stable halo orbit. + +![JWST MC X (km)](./plots/jwst_mc_X_km.png) + +![JWST MC Y (km)](./plots/jwst_mc_Y_km.png) + +![JWST MC Z (km)](./plots/jwst_mc_Z_km.png) + +![JWST MC VX (km/s)](./plots/jwst_mc_VX_km_s.png) + +![JWST MC VY (km/s)](./plots/jwst_mc_VY_km_s.png) + +![JWST MC VZ (km/s)](./plots/jwst_mc_VZ_km_s.png) + +### Keplerian uncertainties + +A few tools try to provide Keplerian uncertainties, but often fail to do so correctly (I'm looking at you, ODTK). Nyx rotates the covariance from its Cartesian form into the Keplerian state space by computing the partial derivatives of each requested parameter with respect to the nominal state. This computation is flawless because it uses automatic differentiation (via _hyperdual numbers_). As such, the OD export also includes all of the state computations supported in Nyx, including uncommon ones like the uncertainties in the energy of the orbit or in the true anomaly. + +The following columns are also provided as 1-sigma in the OD dataframe: +- Sigma aol (deg) +- Sigma c3 (km^2/s^2) +- Sigma declin (deg) +- Sigma ea (deg) +- Sigma fpa (deg) +- Sigma hmag (km) +- Sigma hx (km) +- Sigma hy (km) +- Sigma hz (km) +- Sigma ma (deg) +- Sigma right_asc (deg) +- Sigma rmag (km) +- Sigma semi_parameter (km) +- Sigma semi_minor (km) +- Sigma tlong (deg) +- Sigma vmag (km/s) +- Sigma Cr (Earth J2000) (unitless) +- Sigma Cd (Earth J2000) (unitless) +- Sigma Mass (Earth J2000) (kg) + +![JWST SMA (km)](./plots/jwst_mc_sma_km.png) + +![JWST ECC](./plots/jwst_mc_ecc.png) + +![JWST Energy](./plots/jwst_mc_energy_km2_s2.png) + +![JWST INC (deg)](./plots/jwst_mc_inc_deg.png) + +![JWST RAAN (deg)](./plots/jwst_mc_raan_deg.png) + +![JWST AoP (deg)](./plots/jwst_mc_aop_deg.png) + +![JWST True Anomaly (deg)](./plots/jwst_mc_ta_deg.png) diff --git a/examples/02_jwst_covar_monte_carlo/main.rs b/examples/02_jwst_covar_monte_carlo/main.rs new file mode 100644 index 00000000..bc9c1202 --- /dev/null +++ b/examples/02_jwst_covar_monte_carlo/main.rs @@ -0,0 +1,162 @@ +extern crate log; +extern crate nyx_space as nyx; +extern crate pretty_env_logger as pel; + +use anise::{ + almanac::metaload::MetaFile, + constants::{ + celestial_objects::{JUPITER_BARYCENTER, MOON, SUN}, + frames::{EARTH_J2000, MOON_J2000}, + }, +}; +use hifitime::{TimeUnits, Unit}; +use nyx::{ + cosmic::{eclipse::EclipseLocator, Frame, MetaAlmanac, SrpConfig}, + dynamics::{guidance::LocalFrame, OrbitalDynamics, SolarPressure, SpacecraftDynamics}, + io::ExportCfg, + linalg::{Matrix2, Vector2}, + mc::MonteCarlo, + od::{prelude::KF, process::SpacecraftUncertainty, SpacecraftODProcess}, + propagators::Propagator, + Spacecraft, State, +}; + +use std::{error::Error, sync::Arc}; + +fn main() -> Result<(), Box> { + pel::init(); + // Dynamics models require planetary constants and ephemerides to be defined. + // Let's start by grabbing those by using ANISE's latest MetaAlmanac. + // For details, refer to https://github.com/nyx-space/anise/blob/master/data/latest.dhall. + + // Download the regularly update of the James Webb Space Telescope reconstucted (or definitive) ephemeris. + // Refer to https://naif.jpl.nasa.gov/pub/naif/JWST/kernels/spk/aareadme.txt for details. + let mut latest_jwst_ephem = MetaFile { + uri: "https://naif.jpl.nasa.gov/pub/naif/JWST/kernels/spk/jwst_rec.bsp".to_string(), + crc32: None, + }; + latest_jwst_ephem.process()?; + + // Load this ephem in the general Almanac we're using for this analysis. + let almanac = Arc::new( + MetaAlmanac::latest() + .map_err(Box::new)? + .load_from_metafile(latest_jwst_ephem)?, + ); + + // By loading this ephemeris file in the ANISE GUI or ANISE CLI, we can find the NAIF ID of the JWST + // in the BSP. We need this ID in order to query the ephemeris. + const JWST_NAIF_ID: i32 = -170; + // Let's build a frame in the J2000 orientation centered on the JWST. + const JWST_J2000: Frame = Frame::from_ephem_j2000(JWST_NAIF_ID); + + // Since the ephemeris file is updated regularly, we'll just grab the latest state in the ephem. + let (earliest_epoch, latest_epoch) = almanac.spk_domain(JWST_NAIF_ID)?; + println!("JWST defined from {earliest_epoch} to {latest_epoch}"); + // Fetch the state, printing it in the Earth J2000 frame. + let jwst_orbit = almanac.transform(JWST_J2000, EARTH_J2000, latest_epoch, None)?; + println!("{jwst_orbit:x}"); + + // Build the spacecraft + // SRP area assumed to be the full sunshield and mass if 6200.0 kg, c.f. https://webb.nasa.gov/content/about/faqs/facts.html + // SRP Coefficient of reflectivity assumed to be that of Kapton, i.e. 2 - 0.44 = 1.56, table 1 from https://amostech.com/TechnicalPapers/2018/Poster/Bengtson.pdf + let jwst = Spacecraft::builder() + .orbit(jwst_orbit) + .srp(SrpConfig { + area_m2: 21.197 * 14.162, + cr: 1.56, + }) + .dry_mass_kg(6200.0) + .build(); + + // Build up the spacecraft uncertainty builder. + // We can use the spacecraft uncertainty structure to build this up. + // We start by specifying the nominal state (as defined above), then the uncertainty in position and velocity + // in the RIC frame. We could also specify the Cr, Cd, and mass uncertainties, but these aren't accounted for until + // Nyx can also estimate the deviation of the spacecraft parameters. + let jwst_uncertainty = SpacecraftUncertainty::builder() + .nominal(jwst) + .frame(LocalFrame::RIC) + .x_km(0.5) + .y_km(0.3) + .z_km(1.5) + .vx_km_s(1e-4) + .vy_km_s(0.6e-3) + .vz_km_s(3e-3) + .build(); + + println!("{jwst_uncertainty}"); + + // Build the Kalman filter estimate. + // Note that we could have used the KfEstimate structure directly (as seen throughout the OD integration tests) + // but this approach requires quite a bit more boilerplate code. + let jwst_estimate = jwst_uncertainty.to_estimate()?; + + // Set up the spacecraft dynamics. + // We'll use the point masses of the Earth, Sun, Jupiter (barycenter, because it's in the DE440), and the Moon. + // We'll also enable solar radiation pressure since the James Webb has a huge and highly reflective sun shield. + + let orbital_dyn = OrbitalDynamics::point_masses(vec![MOON, SUN, JUPITER_BARYCENTER]); + let srp_dyn = SolarPressure::new(vec![EARTH_J2000, MOON_J2000], almanac.clone())?; + + // Finalize setting up the dynamics. + let dynamics = SpacecraftDynamics::from_model(orbital_dyn, srp_dyn); + + // Build the propagator set up to use for the whole analysis. + let setup = Propagator::default(dynamics); + + // All of the analysis will use this duration. + let prediction_duration = 30.5 * Unit::Hour; + + // === Covariance mapping === + // For the covariance mapping / prediction, we'll use the common orbit determination approach. + // This is done by setting up a spacecraft OD process, and predicting for the analysis duration. + + let measurement_noise = Matrix2::from_diagonal(&Vector2::new(1e-6, 1e-3)); + let ckf = KF::no_snc(jwst_estimate, measurement_noise); + + // 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()); + + // 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())?; + + // === Monte Carlo framework === + // Nyx comes with a complete multi-threaded Monte Carlo frame. It's blazing fast. + + let my_mc = MonteCarlo::new( + jwst, // Nominal state + jwst_estimate.to_random_variable()?, + "02_jwst".to_string(), // Scenario name + None, // No specific seed specified, so one will be drawn from the computer's entropy. + ); + + let num_runs = 5_000; + let rslts = my_mc.run_until_epoch( + setup, + almanac.clone(), + jwst.epoch() + prediction_duration, + num_runs, + ); + + assert_eq!(rslts.runs.len(), num_runs); + // Finally, export these results, computing the eclipse percentage for all of these results. + + // For all of the resulting trajectories, we'll want to compute the percentage of penumbra and umbra. + let eclipse_loc = EclipseLocator::cislunar(almanac.clone()); + let umbra_event = eclipse_loc.to_umbra_event(); + let penumbra_event = eclipse_loc.to_penumbra_event(); + + rslts.to_parquet( + "02_jwst_monte_carlo.parquet", + Some(vec![&umbra_event, &penumbra_event]), + ExportCfg::default(), + almanac, + )?; + + Ok(()) +} diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VX_km_s.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VX_km_s.png new file mode 100644 index 00000000..282dbcb1 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VX_km_s.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VY_km_s.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VY_km_s.png new file mode 100644 index 00000000..056d588d Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VY_km_s.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VZ_km_s.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VZ_km_s.png new file mode 100644 index 00000000..1e474931 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_VZ_km_s.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_X_km.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_X_km.png new file mode 100644 index 00000000..c5aba945 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_X_km.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Y_km.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Y_km.png new file mode 100644 index 00000000..bf60a582 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Y_km.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Z_km.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Z_km.png new file mode 100644 index 00000000..289efe55 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_Z_km.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_aop_deg.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_aop_deg.png new file mode 100644 index 00000000..2725f03b Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_aop_deg.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ecc.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ecc.png new file mode 100644 index 00000000..5846fdac Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ecc.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_energy_km2_s2.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_energy_km2_s2.png new file mode 100644 index 00000000..308fafb0 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_energy_km2_s2.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_inc_deg.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_inc_deg.png new file mode 100644 index 00000000..29c541c4 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_inc_deg.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_raan_deg.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_raan_deg.png new file mode 100644 index 00000000..f5f5ec93 Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_raan_deg.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_sma_km.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_sma_km.png new file mode 100644 index 00000000..e0f22e8b Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_sma_km.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ta_deg.png b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ta_deg.png new file mode 100644 index 00000000..1631af7d Binary files /dev/null and b/examples/02_jwst_covar_monte_carlo/plots/jwst_mc_ta_deg.png differ diff --git a/examples/02_jwst_covar_monte_carlo/plotting.py b/examples/02_jwst_covar_monte_carlo/plotting.py new file mode 100644 index 00000000..fb87c42d --- /dev/null +++ b/examples/02_jwst_covar_monte_carlo/plotting.py @@ -0,0 +1,198 @@ +import polars as pl +import plotly.graph_objs as go + +if __name__ == "__main__": + df_mc = pl.read_parquet("./02_jwst_monte_carlo.parquet") + df_mc = df_mc.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")) + print(df_mc.describe()) + + df_covar = pl.read_parquet("./02_jwst_covar_map.parquet") + df_covar = df_covar.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")) + print(df_covar.describe()) + + # Build the position plots + for coord in ["X", "Y", "Z"]: + col = coord.lower() + " (km)" + fig = go.Figure( + data=[ + go.Scattergl( + x=df_mc["Epoch (UTC)"], + y=df_mc[col], + mode="lines", + opacity=0.05, + showlegend=True, + name=f"[MC] {coord} (km)", + text=df_mc["Monte Carlo Run Index"], + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col], + mode="lines", + showlegend=True, + name=f"[Nominal] {coord} (km)", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] + 3.0 * df_covar[f"Sigma {coord} (Earth J2000) (km)"], + mode="lines", + showlegend=True, + name=coord + " (km) + 3-Σ", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] - 3.0 * df_covar[f"Sigma {coord} (Earth J2000) (km)"], + mode="lines", + showlegend=True, + name=coord + " (km) - 3-Σ", + ), + ] + ) + fig.update_layout( + title=f"JWST {coord} state prediction - {len(df_mc)} Monte Carlo Results", + xaxis_title="Epoch", + yaxis_title=coord + " (km) EME2000", + legend_title="Legend", + ) + fig.show() + + # Build the velocity plots + for coord in ["VX", "VY", "VZ"]: + col = coord.lower() + " (km/s)" + fig = go.Figure( + data=[ + go.Scattergl( + x=df_mc["Epoch (UTC)"], + y=df_mc[col], + mode="lines", + opacity=0.05, + showlegend=True, + name=f"[MC] {coord} (km/s)", + text=df_mc["Monte Carlo Run Index"], + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col], + mode="lines", + showlegend=True, + name=f"[Nominal] {coord} (km/s)", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] + + 3.0 * df_covar[f"Sigma {coord.capitalize()} (Earth J2000) (km/s)"], + mode="lines", + showlegend=True, + name=coord + " (km/s) + 3-Σ", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] + - 3.0 * df_covar[f"Sigma {coord.capitalize()} (Earth J2000) (km/s)"], + mode="lines", + showlegend=True, + name=coord + " (km/s) - 3-Σ", + ), + ] + ) + fig.update_layout( + title=f"JWST {coord} velocity prediction - {len(df_mc)} Monte Carlo Results", + xaxis_title="Epoch", + yaxis_title=coord + " (km/s) EME2000", + legend_title="Legend", + ) + fig.show() + + # Build the RIC uncertainties plots + fig = go.Figure( + data=[ + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=3.0 * df_covar[col], + mode="lines", + showlegend=True, + name=f"3-{col}", + ) + for col in ["Sigma X (RIC) (km)", "Sigma Y (RIC) (km)", "Sigma Z (RIC) (km)"] + ] + ) + fig.update_layout( + title=f"JWST RIC 3-Σ position prediction", + xaxis_title="Epoch", + legend_title="Legend", + ) + fig.show() + fig = go.Figure( + data=[ + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=3.0 * df_covar[col], + mode="lines", + showlegend=True, + name=f"3-{col}", + ) + for col in ["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"] + ] + ) + fig.update_layout( + title=f"JWST RIC 3-Σ position prediction", + xaxis_title="Epoch", + legend_title="Legend", + ) + fig.show() + + # Build the Keplerian uncertainty plots + keplerian_columns = [ + "sma (km)", + "ecc", + "inc (deg)", + "raan (deg)", + "aop (deg)", + "ta (deg)", + "energy (km^2/s^2)", + ] + for col in keplerian_columns: + fig = go.Figure( + data=[ + go.Scattergl( + x=df_mc["Epoch (UTC)"], + y=df_mc[col], + mode="lines", + opacity=0.05, + showlegend=True, + name=f"[MC] {col}", + text=df_mc["Monte Carlo Run Index"], + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col], + mode="lines", + showlegend=True, + name=f"[Nominal] {col}", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] + 3.0 * df_covar[f"Sigma {col}"], + mode="lines", + showlegend=True, + name=col + " + 3-Σ", + ), + go.Scattergl( + x=df_covar["Epoch (UTC)"], + y=df_covar[col] - 3.0 * df_covar[f"Sigma {col}"], + mode="lines", + showlegend=True, + name=col + " - 3-Σ", + ), + ] + ) + fig.show() + + print("\nThe following columns are also provided as 1-sigma in the OD dataframe:") + for col in df_covar.columns: + skip = False + for pre_plotted in keplerian_columns + ["X", "Y", "Z", "Vx", "Vy", "Vz"]: + if pre_plotted in col: + skip = True + break + if not skip and "Sigma" in col: + print(f"\t- {col}") diff --git a/examples/02_jwst_covar_monte_carlo/requirements.txt b/examples/02_jwst_covar_monte_carlo/requirements.txt new file mode 100644 index 00000000..fcd2d3ec --- /dev/null +++ b/examples/02_jwst_covar_monte_carlo/requirements.txt @@ -0,0 +1,37 @@ +asttokens==2.4.1 +attrs==23.2.0 +decorator==5.1.1 +executing==2.0.1 +iniconfig==2.0.0 +ipython==8.25.0 +jedi==0.19.1 +matplotlib-inline==0.1.7 +maturin==1.6.0 +numpy==1.26.4 +packaging==24.1 +pandas==2.1.4 +parso==0.8.4 +pexpect==4.9.0 +pip==24.0 +plotly==5.16.1 +pluggy==1.5.0 +polars==0.20.31 +prompt-toolkit==3.0.47 +ptyprocess==0.7.0 +pure-eval==0.2.2 +pyarrow==13.0.0 +pygments==2.18.0 +pytest==7.2.2 +python-dateutil==2.9.0.post0 +python-slugify==8.0.4 +pytz==2024.1 +ruff==0.5.0 +scipy==1.11.4 +six==1.16.0 +stack-data==0.6.3 +tenacity==8.3.0 +text-unidecode==1.3 +traitlets==5.14.3 +typing-extensions==4.12.2 +tzdata==2024.1 +wcwidth==0.2.13 diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 00000000..af525dcc --- /dev/null +++ b/examples/README.md @@ -0,0 +1,3 @@ +# Nyx examples + +This folder contains examples on how to use Nyx in Rust. If there is a specific example you'd like to see, feel free to create a [ticket](https://github.com/nyx-space/nyx/issues/new?assignees=&labels=Documentation&projects=&template=documentation.md&title=) or to reach out [directly](https://7ug5imdtt8v.typeform.com/to/neFvVW3p). diff --git a/python/nyx_space/analysis/traj.py b/python/nyx_space/analysis/traj.py index 418e50df..5cdbf4bb 100644 --- a/python/nyx_space/analysis/traj.py +++ b/python/nyx_space/analysis/traj.py @@ -50,9 +50,7 @@ def loader(path: str) -> TrajectoryLoader: # Try to be somewhat clever if path.lower().endswith(".oem"): # Load as OEM and build the parquet file - return TrajectoryLoader( - str(path), "oem", path[:-3] + "parquet" - ).to_orbit_traj() + return TrajectoryLoader(str(path), "oem", path[:-3] + "parquet").to_orbit_traj() else: return TrajectoryLoader(str(path), "parquet").to_orbit_traj() @@ -65,9 +63,7 @@ def loader(path: str) -> TrajectoryLoader: # Sample to build the data. - for epoch in TimeSeries( - traj1.first().epoch, traj1.last().epoch, step, inclusive=True - ): + for epoch in TimeSeries(traj1.first().epoch, traj1.last().epoch, step, inclusive=True): try: state1 = traj1.at(epoch) state2 = traj2.at(epoch) diff --git a/python/nyx_space/plots/od.py b/python/nyx_space/plots/od.py index 63178341..93a5a7e5 100644 --- a/python/nyx_space/plots/od.py +++ b/python/nyx_space/plots/od.py @@ -32,6 +32,7 @@ from nyx_space.time import Epoch + def plot_estimates( dfs, title, @@ -102,11 +103,7 @@ def plot_estimates( # Check that the requested covariance frame exists frames = set( - [ - re.search(r"\((.*?)\)", c).group(1) - for c in df.columns - if c.startswith("Covariance") - ] + [re.search(r"\((.*?)\)", c).group(1) for c in df.columns if c.startswith("Covariance")] ) if cov_frame not in frames: raise ValueError( @@ -131,9 +128,7 @@ def plot_estimates( else: cov_col_name = f"{covar_col} ({cov_frame}) {cov_sigma}-sigma {cov_fmt}" # Transform the current column - df[cov_col_name] = eval(f"np.{cov_fmt}")( - df[f"{covar_col} ({cov_frame})"] - ) + df[cov_col_name] = eval(f"np.{cov_fmt}")(df[f"{covar_col} ({cov_frame})"]) covar[f"{covar_var}"] = cov_col_name plt_df = df @@ -250,13 +245,9 @@ def plot_estimates( if msr_df is not None: # Plot the measurements on both plots - pos_fig = overlay_measurements( - pos_fig, msr_df, title, time_col_name, show=False - ) + pos_fig = overlay_measurements(pos_fig, msr_df, title, time_col_name, show=False) - vel_fig = overlay_measurements( - vel_fig, msr_df, title, time_col_name, show=False - ) + vel_fig = overlay_measurements(vel_fig, msr_df, title, time_col_name, show=False) if html_out: html_out = html_out.replace(".html", "_{}.html") @@ -342,11 +333,7 @@ def plot_covar( # Check that the requested covariance frame exists frames = set( - [ - re.search(r"\((.*?)\)", c).group(1) - for c in df.columns - if c.startswith("Covariance") - ] + [re.search(r"\((.*?)\)", c).group(1) for c in df.columns if c.startswith("Covariance")] ) if cov_frame not in frames: raise ValueError( @@ -457,13 +444,9 @@ def plot_covar( if msr_df is not None: # Plot the measurements on both plots - pos_fig = overlay_measurements( - pos_fig, msr_df, title, time_col_name, show=False - ) + pos_fig = overlay_measurements(pos_fig, msr_df, title, time_col_name, show=False) - vel_fig = overlay_measurements( - vel_fig, msr_df, title, time_col_name, show=False - ) + vel_fig = overlay_measurements(vel_fig, msr_df, title, time_col_name, show=False) if html_out: html_out = html_out.replace(".html", "_{}.html") @@ -661,7 +644,9 @@ def plot_residuals( ) # Add the 1-σ lines one_sig_color = colors["bright_green"] - one_sig_color = f"rgb({int(one_sig_color[0])}, {int(one_sig_color[1])}, {int(one_sig_color[2])})" + one_sig_color = ( + f"rgb({int(one_sig_color[0])}, {int(one_sig_color[1])}, {int(one_sig_color[2])})" + ) fig.add_hline( y=mean + std, line_dash="dot", @@ -677,13 +662,9 @@ def plot_residuals( if msr_df is not None: # Plot the measurements on both plots - fig = overlay_measurements( - fig, msr_df, title, time_col_name, show=False - ) + fig = overlay_measurements(fig, msr_df, title, time_col_name, show=False) - finalize_plot( - fig, title=f"{title} {col}", xtitle=x_title, copyright=copyright - ) + finalize_plot(fig, title=f"{title} {col}", xtitle=x_title, copyright=copyright) plt_any = True @@ -705,9 +686,7 @@ def plot_residuals( return rtn_plots -def plot_residual_histogram( - df, title, kind="Prefit", copyright=None, html_out=None, show=True -): +def plot_residual_histogram(df, title, kind="Prefit", copyright=None, html_out=None, show=True): """ Histogram of residuals """ @@ -749,6 +728,7 @@ def plot_residual_histogram( if show: fig.show() + def plot_measurements( df, msr_type=None, diff --git a/python/nyx_space/plots/traj.py b/python/nyx_space/plots/traj.py index b45c175b..085757f2 100644 --- a/python/nyx_space/plots/traj.py +++ b/python/nyx_space/plots/traj.py @@ -166,9 +166,7 @@ def plot_ground_track( for name, (lat, long) in landmarks.items(): print(f"Adding landmark {name} at lat={lat}, long={long}") - fig.add_trace( - go.Scattergeo(lon=[float(long)], lat=[float(lat)], name=name.strip()) - ) + fig.add_trace(go.Scattergeo(lon=[float(long)], lat=[float(lat)], name=name.strip())) fig.update_geos( lataxis_showgrid=True, @@ -373,9 +371,7 @@ def plot_traj_errors( col=col_i + 1, ) except KeyError: - raise KeyError( - f"Rebuild the trajectory and export the RIC frame: missing `{col}`" - ) + raise KeyError(f"Rebuild the trajectory and export the RIC frame: missing `{col}`") if vertical: col_i += 1 else: diff --git a/python/nyx_space/plots/utils.py b/python/nyx_space/plots/utils.py index 2a5427df..9ea6a21f 100644 --- a/python/nyx_space/plots/utils.py +++ b/python/nyx_space/plots/utils.py @@ -171,9 +171,7 @@ def plot_line(fig, df, x, y_name, color_name, xtitle, ytitle, title, copyright): Adds the trace for the provided X vs Y. """ - plot_raw_line( - fig, x, df[y_name], y_name, color_name, xtitle, ytitle, title, copyright - ) + plot_raw_line(fig, x, df[y_name], y_name, color_name, xtitle, ytitle, title, copyright) def plot_raw_line(fig, x, y, y_name, color_name, xtitle, ytitle, title, copyright=None): diff --git a/python/nyx_space/utils/doris.py b/python/nyx_space/utils/doris.py index 3a881d07..44082eec 100644 --- a/python/nyx_space/utils/doris.py +++ b/python/nyx_space/utils/doris.py @@ -93,19 +93,13 @@ def snx_to_groundstation( continue elif parsing: if line.startswith("-SITE/ID"): - print( - "Parsed {} ground stations from {}.".format( - len(entries), snx_path - ) - ) + print("Parsed {} ground stations from {}.".format(len(entries), snx_path)) break elif line.startswith("*CODE"): continue else: # Parse the data line - kwrds = [ - line[idx : idx + klen].strip() for idx, klen in offsets.values() - ] + kwrds = [line[idx : idx + klen].strip() for idx, klen in offsets.values()] # Create the YAML this_entry = dict(zip(offsets.keys(), kwrds)) # Fix the latitude and longitude to be decimals @@ -211,9 +205,7 @@ def basic_rinex(rnx_path, stations): offset = Unit.Second * float(splt[-2]) esplt = splt[1:-4] epoch = ( - Epoch( - f"{esplt[0]}-{esplt[1]}-{esplt[2]} {esplt[3]}:{esplt[4]}:{esplt[5]} TAI" - ) + Epoch(f"{esplt[0]}-{esplt[1]}-{esplt[2]} {esplt[3]}:{esplt[4]}:{esplt[5]} TAI") + offset ) action = "msr_start" @@ -273,9 +265,7 @@ def prototype(gs_yaml, rnx_path, plot=True): if len(msrs) > 100 and plot: fig = go.Figure() fig.add_trace( - go.Scatter( - x=time_offsets, y=c1_corr, mode="lines", name="C1 data (2 GHz)" - ) + go.Scatter(x=time_offsets, y=c1_corr, mode="lines", name="C1 data (2 GHz)") ) # Add τ fig.add_vline( @@ -291,9 +281,7 @@ def prototype(gs_yaml, rnx_path, plot=True): # Plot the data itself too fig = go.Figure() fig.add_trace( - go.Scatter( - x=time_offsets, y=c1_data, mode="lines", name="C1 data (2 GHz)" - ) + go.Scatter(x=time_offsets, y=c1_data, mode="lines", name="C1 data (2 GHz)") ) fig.update_layout(title=title) fig.show() diff --git a/src/cosmic/bplane.rs b/src/cosmic/bplane.rs index e79599a1..8dace89d 100644 --- a/src/cosmic/bplane.rs +++ b/src/cosmic/bplane.rs @@ -87,7 +87,7 @@ impl BPlane { // let b_vec = orbit.semi_minor_axis() // * ((1.0 - (1.0 / orbit.ecc()).powi(2)).sqrt() * e_hat // - (1.0 / orbit.ecc() * n_hat)); - let semi_minor_axis = orbit.semi_minor_axis().context(AstroPhysicsSnafu)?; + let semi_minor_axis = orbit.semi_minor_axis_km().context(AstroPhysicsSnafu)?; let b_vec = Vector3::new( semi_minor_axis.dual @@ -125,7 +125,7 @@ impl BPlane { param: StateParameter::BdotT, }, ltof_s: OrbitPartial { - dual: b_vec.dot(&s_hat) / orbit.vmag().dual, + dual: b_vec.dot(&s_hat) / orbit.vmag_km_s().dual, param: StateParameter::BLTOF, }, str_dcm: str_rot, diff --git a/src/cosmic/eclipse.rs b/src/cosmic/eclipse.rs index 62d8ce2c..93c4b8c3 100644 --- a/src/cosmic/eclipse.rs +++ b/src/cosmic/eclipse.rs @@ -126,11 +126,14 @@ pub struct EclipseLocator { impl fmt::Display for EclipseLocator { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - let shadow_bodies: Vec = - self.shadow_bodies.iter().map(|b| format!("{b}")).collect(); + let shadow_bodies: Vec = self + .shadow_bodies + .iter() + .map(|b| format!("{b:x}")) + .collect(); write!( f, - "light-source: {}, shadows casted by: {}", + "light-source: {:x}, shadows casted by: {}", self.light_source, shadow_bodies.join(", ") ) @@ -161,7 +164,8 @@ impl EclipseLocator { Ok(state) } - /// Creates an umbra event from this eclipse locator + /// Creates an umbra event from this eclipse locator. + /// Evaluation of the event, returns 0.0 for umbra, 1.0 for visibility (no shadow) and some value in between for penumbra pub fn to_umbra_event(&self) -> UmbraEvent { UmbraEvent { e_loc: self.clone(), @@ -169,6 +173,7 @@ impl EclipseLocator { } /// Creates a penumbra event from this eclipse locator + // Evaluation of the event, returns 0.0 for umbra, 1.0 for visibility (no shadow) and some value in between for penumbra pub fn to_penumbra_event(&self) -> PenumbraEvent { PenumbraEvent { e_loc: self.clone(), @@ -188,7 +193,7 @@ impl fmt::Display for UmbraEvent { } impl EventEvaluator for UmbraEvent { - // Evaluation of the event, returns 0.0 for umbra, 1.0 for visibility and some value in between for penumbra + // Evaluation of the event, returns 0.0 for umbra, 1.0 for visibility (no shadow) and some value in between for penumbra fn eval(&self, sc: &Spacecraft, almanac: Arc) -> Result { match self .e_loc diff --git a/src/cosmic/mod.rs b/src/cosmic/mod.rs index bc30067a..97f0cef5 100644 --- a/src/cosmic/mod.rs +++ b/src/cosmic/mod.rs @@ -17,7 +17,7 @@ */ use anise::errors::{AlmanacError, PhysicsError}; -pub use anise::prelude::{Frame, Orbit}; +pub use anise::prelude::*; pub use crate::cosmic::{GuidanceMode, Spacecraft}; use crate::dynamics::DynamicsError; @@ -26,7 +26,6 @@ use crate::errors::StateError; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector}; use crate::md::StateParameter; -use crate::time::Epoch; use snafu::Snafu; use std::fmt; diff --git a/src/cosmic/orbitdual.rs b/src/cosmic/orbitdual.rs index 6c885f6e..b3328ee9 100644 --- a/src/cosmic/orbitdual.rs +++ b/src/cosmic/orbitdual.rs @@ -145,48 +145,45 @@ impl OrbitDual { dual: self.vz, param: StateParameter::VZ, }), - StateParameter::Rmag => Ok(self.rmag()), - StateParameter::Vmag => Ok(self.vmag()), + StateParameter::Rmag => Ok(self.rmag_km()), + StateParameter::Vmag => Ok(self.vmag_km_s()), StateParameter::HX => Ok(self.hx()), StateParameter::HY => Ok(self.hy()), StateParameter::HZ => Ok(self.hz()), StateParameter::Hmag => Ok(self.hmag()), - StateParameter::Energy => Ok(self.energy().context(AstroPhysicsSnafu)?), - StateParameter::SMA => Ok(self.sma().context(AstroPhysicsSnafu)?), + StateParameter::Energy => Ok(self.energy_km2_s2().context(AstroPhysicsSnafu)?), + StateParameter::SMA => Ok(self.sma_km().context(AstroPhysicsSnafu)?), StateParameter::Eccentricity => Ok(self.ecc().context(AstroPhysicsSnafu)?), - StateParameter::Inclination => Ok(self.inc()), - StateParameter::AoP => Ok(self.aop().context(AstroPhysicsSnafu)?), - StateParameter::AoL => Ok(self.aol().context(AstroPhysicsSnafu)?), - StateParameter::RAAN => Ok(self.raan()), - StateParameter::Periapsis => Ok(self.periapsis().context(AstroPhysicsSnafu)?), - StateParameter::Apoapsis => Ok(self.apoapsis().context(AstroPhysicsSnafu)?), - StateParameter::TrueLongitude => Ok(self.tlong().context(AstroPhysicsSnafu)?), - StateParameter::FlightPathAngle => Ok(self.fpa().context(AstroPhysicsSnafu)?), - StateParameter::MeanAnomaly => Ok(self.ma().context(AstroPhysicsSnafu)?), - StateParameter::EccentricAnomaly => Ok(self.ea().context(AstroPhysicsSnafu)?), - StateParameter::GeodeticHeight => { - Ok(self.geodetic_height().context(AstroPhysicsSnafu)?) - } - StateParameter::GeodeticLatitude => { - Ok(self.geodetic_latitude().context(AstroPhysicsSnafu)?) - } - StateParameter::GeodeticLongitude => Ok(self.geodetic_longitude()), + StateParameter::Inclination => Ok(self.inc_deg()), + StateParameter::AoP => Ok(self.aop_deg().context(AstroPhysicsSnafu)?), + StateParameter::AoL => Ok(self.aol_deg().context(AstroPhysicsSnafu)?), + StateParameter::RAAN => Ok(self.raan_deg()), + StateParameter::Periapsis => Ok(self.periapsis_km().context(AstroPhysicsSnafu)?), + StateParameter::Apoapsis => Ok(self.apoapsis_km().context(AstroPhysicsSnafu)?), + StateParameter::TrueLongitude => Ok(self.tlong_deg().context(AstroPhysicsSnafu)?), + StateParameter::FlightPathAngle => Ok(self.fpa_deg().context(AstroPhysicsSnafu)?), + StateParameter::MeanAnomaly => Ok(self.ma_deg().context(AstroPhysicsSnafu)?), + StateParameter::EccentricAnomaly => Ok(self.ea_deg().context(AstroPhysicsSnafu)?), + StateParameter::Height => Ok(self.height_km().context(AstroPhysicsSnafu)?), + StateParameter::Latitude => Ok(self.latitude_deg().context(AstroPhysicsSnafu)?), + StateParameter::Longitude => Ok(self.longitude_deg()), StateParameter::C3 => Ok(self.c3().context(AstroPhysicsSnafu)?), - StateParameter::RightAscension => Ok(self.right_ascension()), - StateParameter::Declination => Ok(self.declination()), - StateParameter::HyperbolicAnomaly => self.hyperbolic_anomaly(), + StateParameter::RightAscension => Ok(self.right_ascension_deg()), + StateParameter::Declination => Ok(self.declination_deg()), + StateParameter::HyperbolicAnomaly => self.hyperbolic_anomaly_deg(), StateParameter::SemiParameter => { - Ok(self.semi_parameter().context(AstroPhysicsSnafu)?) + Ok(self.semi_parameter_km().context(AstroPhysicsSnafu)?) } StateParameter::SemiMinorAxis => { - Ok(self.semi_minor_axis().context(AstroPhysicsSnafu)?) + Ok(self.semi_minor_axis_km().context(AstroPhysicsSnafu)?) } + StateParameter::TrueAnomaly => Ok(self.ta_deg().context(AstroPhysicsSnafu)?), _ => Err(AstroError::PartialsUndefined), } } /// Returns the magnitude of the radius vector in km - pub fn rmag(&self) -> OrbitPartial { + pub fn rmag_km(&self) -> OrbitPartial { OrbitPartial { param: StateParameter::Rmag, dual: (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt(), @@ -194,7 +191,7 @@ impl OrbitDual { } /// Returns the magnitude of the velocity vector in km/s - pub fn vmag(&self) -> OrbitPartial { + pub fn vmag_km_s(&self) -> OrbitPartial { OrbitPartial { param: StateParameter::Vmag, dual: (self.vx.powi(2) + self.vy.powi(2) + self.vz.powi(2)).sqrt(), @@ -249,19 +246,19 @@ impl OrbitDual { } /// Returns the specific mechanical energy - pub fn energy(&self) -> PhysicsResult { + pub fn energy_km2_s2(&self) -> PhysicsResult { Ok(OrbitPartial { - dual: self.vmag().dual.powi(2) / OHyperdual::from(2.0) - - OHyperdual::from(self.frame.mu_km3_s2()?) / self.rmag().dual, + dual: self.vmag_km_s().dual.powi(2) / OHyperdual::from(2.0) + - OHyperdual::from(self.frame.mu_km3_s2()?) / self.rmag_km().dual, param: StateParameter::Energy, }) } /// Returns the semi-major axis in km - pub fn sma(&self) -> PhysicsResult { + pub fn sma_km(&self) -> PhysicsResult { Ok(OrbitPartial { dual: -OHyperdual::from(self.frame.mu_km3_s2()?) - / (OHyperdual::from(2.0) * self.energy()?.dual), + / (OHyperdual::from(2.0) * self.energy_km2_s2()?.dual), param: StateParameter::SMA, }) } @@ -289,7 +286,7 @@ impl OrbitDual { } /// Returns the inclination in degrees - pub fn inc(&self) -> OrbitPartial { + pub fn inc_deg(&self) -> OrbitPartial { OrbitPartial { dual: (self.hvec()[(2, 0)] / self.hmag().dual).acos().to_degrees(), param: StateParameter::Inclination, @@ -297,7 +294,7 @@ impl OrbitDual { } /// Returns the argument of periapsis in degrees - pub fn aop(&self) -> PhysicsResult { + pub fn aop_deg(&self) -> PhysicsResult { let n = Vector3::new( OHyperdual::from(0.0), OHyperdual::from(0.0), @@ -325,7 +322,7 @@ impl OrbitDual { } /// Returns the right ascension of ther ascending node in degrees - pub fn raan(&self) -> OrbitPartial { + pub fn raan_deg(&self) -> OrbitPartial { let n = Vector3::new( OHyperdual::from(0.0), OHyperdual::from(0.0), @@ -353,14 +350,14 @@ impl OrbitDual { } /// Returns the true anomaly in degrees between 0 and 360.0 - pub fn ta(&self) -> PhysicsResult { + pub fn ta_deg(&self) -> PhysicsResult { if self.ecc()?.real() < ECC_EPSILON { warn!( "true anomaly ill-defined for circular orbit (e = {})", self.ecc()? ); } - let cos_nu = self.evec()?.dot(&self.radius()) / (self.ecc()?.dual * self.rmag().dual); + let cos_nu = self.evec()?.dot(&self.radius()) / (self.ecc()?.dual * self.rmag_km().dual); if (cos_nu.real().abs() - 1.0).abs() < f64::EPSILON { // This bug drove me crazy when writing SMD in Go in 2017. if cos_nu > 1.0 { @@ -397,10 +394,10 @@ impl OrbitDual { } /// Returns the true longitude in degrees - pub fn tlong(&self) -> PhysicsResult { + pub fn tlong_deg(&self) -> PhysicsResult { // Angles already in degrees Ok(OrbitPartial { - dual: self.aop()?.dual + self.raan().dual + self.ta()?.dual, + dual: self.aop_deg()?.dual + self.raan_deg().dual + self.ta_deg()?.dual, param: StateParameter::TrueLongitude, }) } @@ -409,32 +406,32 @@ impl OrbitDual { /// /// NOTE: If the orbit is near circular, the AoL will be computed from the true longitude /// instead of relying on the ill-defined true anomaly. - pub fn aol(&self) -> PhysicsResult { + pub fn aol_deg(&self) -> PhysicsResult { if self.ecc()?.real() < ECC_EPSILON { Ok(OrbitPartial { - dual: self.tlong()?.dual - self.raan().dual, + dual: self.tlong_deg()?.dual - self.raan_deg().dual, param: StateParameter::AoL, }) } else { Ok(OrbitPartial { - dual: self.aop()?.dual + self.ta()?.dual, + dual: self.aop_deg()?.dual + self.ta_deg()?.dual, param: StateParameter::AoL, }) } } /// Returns the radius of periapsis (or perigee around Earth), in kilometers. - pub fn periapsis(&self) -> PhysicsResult { + pub fn periapsis_km(&self) -> PhysicsResult { Ok(OrbitPartial { - dual: self.sma()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual), + dual: self.sma_km()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual), param: StateParameter::Periapsis, }) } /// Returns the radius of apoapsis (or apogee around Earth), in kilometers. - pub fn apoapsis(&self) -> PhysicsResult { + pub fn apoapsis_km(&self) -> PhysicsResult { Ok(OrbitPartial { - dual: self.sma()?.dual * (OHyperdual::from(1.0) + self.ecc()?.dual), + dual: self.sma_km()?.dual * (OHyperdual::from(1.0) + self.ecc()?.dual), param: StateParameter::Apoapsis, }) } @@ -442,8 +439,8 @@ impl OrbitDual { /// Returns the eccentric anomaly in degrees /// /// This is a conversion from GMAT's StateConversionUtil::TrueToEccentricAnomaly - pub fn ea(&self) -> PhysicsResult { - let (sin_ta, cos_ta) = self.ta()?.dual.to_radians().sin_cos(); + pub fn ea_deg(&self) -> PhysicsResult { + let (sin_ta, cos_ta) = self.ta_deg()?.dual.to_radians().sin_cos(); let ecc_cos_ta = self.ecc()?.dual * cos_ta; let sin_ea = ((OHyperdual::from(1.0) - self.ecc()?.dual.powi(2)).sqrt() * sin_ta) / (OHyperdual::from(1.0) + ecc_cos_ta); @@ -456,8 +453,8 @@ impl OrbitDual { } /// Returns the flight path angle in degrees - pub fn fpa(&self) -> PhysicsResult { - let nu = self.ta()?.dual.to_radians(); + pub fn fpa_deg(&self) -> PhysicsResult { + let nu = self.ta_deg()?.dual.to_radians(); let ecc = self.ecc()?.dual; let denom = (OHyperdual::from(1.0) + OHyperdual::from(2.0) * ecc * nu.cos() + ecc.powi(2)).sqrt(); @@ -470,11 +467,11 @@ impl OrbitDual { } /// Returns the mean anomaly in degrees - pub fn ma(&self) -> PhysicsResult { + pub fn ma_deg(&self) -> PhysicsResult { if self.ecc()?.real() < 1.0 { Ok(OrbitPartial { - dual: (self.ea()?.dual.to_radians() - - self.ecc()?.dual * self.ea()?.dual.to_radians().sin()) + dual: (self.ea_deg()?.dual.to_radians() + - self.ecc()?.dual * self.ea_deg()?.dual.to_radians().sin()) .to_degrees(), param: StateParameter::MeanAnomaly, }) @@ -482,11 +479,11 @@ impl OrbitDual { info!("computing the hyperbolic anomaly"); // From GMAT's TrueToHyperbolicAnomaly Ok(OrbitPartial { - dual: ((self.ta()?.dual.to_radians().sin() + dual: ((self.ta_deg()?.dual.to_radians().sin() * (self.ecc()?.dual.powi(2) - OHyperdual::from(1.0))) .sqrt() / (OHyperdual::from(1.0) - + self.ecc()?.dual * self.ta()?.dual.to_radians().cos())) + + self.ecc()?.dual * self.ta_deg()?.dual.to_radians().cos())) .asinh() .to_degrees(), param: StateParameter::MeanAnomaly, @@ -501,9 +498,9 @@ impl OrbitDual { } /// Returns the semi parameter (or semilatus rectum) - pub fn semi_parameter(&self) -> PhysicsResult { + pub fn semi_parameter_km(&self) -> PhysicsResult { Ok(OrbitPartial { - dual: self.sma()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual.powi(2)), + dual: self.sma_km()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual.powi(2)), param: StateParameter::SemiParameter, }) } @@ -512,23 +509,23 @@ impl OrbitDual { /// /// Although the reference is not Vallado, the math from Vallado proves to be equivalent. /// Reference: G. Xu and Y. Xu, "GPS", DOI 10.1007/978-3-662-50367-6_2, 2016 - pub fn geodetic_longitude(&self) -> OrbitPartial { + pub fn longitude_deg(&self) -> OrbitPartial { OrbitPartial { dual: self.y.atan2(self.x).to_degrees(), - param: StateParameter::GeodeticLongitude, + param: StateParameter::Longitude, } } /// Returns the geodetic latitude (φ) in degrees. Value is between -180 and +180 degrees. /// /// Reference: Vallado, 4th Ed., Algorithm 12 page 172. - pub fn geodetic_latitude(&self) -> PhysicsResult { + pub fn latitude_deg(&self) -> PhysicsResult { let flattening = self.frame.flattening()?; let eps = 1e-12; let max_attempts = 20; let mut attempt_no = 0; let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt(); - let mut latitude = (self.z / self.rmag().dual).asin(); + let mut latitude = (self.z / self.rmag_km().dual).asin(); let e2 = OHyperdual::from(flattening * (2.0 - flattening)); loop { attempt_no += 1; @@ -538,7 +535,7 @@ impl OrbitDual { if (latitude - new_latitude).abs() < eps { return Ok(OrbitPartial { dual: new_latitude.to_degrees(), - param: StateParameter::GeodeticLatitude, + param: StateParameter::Latitude, }); } else if attempt_no >= max_attempts { error!( @@ -547,7 +544,7 @@ impl OrbitDual { ); return Ok(OrbitPartial { dual: new_latitude.to_degrees(), - param: StateParameter::GeodeticLatitude, + param: StateParameter::Latitude, }); } latitude = new_latitude; @@ -557,11 +554,11 @@ impl OrbitDual { /// Returns the geodetic height in km. /// /// Reference: Vallado, 4th Ed., Algorithm 12 page 172. - pub fn geodetic_height(&self) -> PhysicsResult { + pub fn height_km(&self) -> PhysicsResult { let flattening = self.frame.flattening()?; let semi_major_radius = self.frame.semi_major_radius_km()?; let e2 = OHyperdual::from(flattening * (2.0 - flattening)); - let latitude = self.geodetic_latitude()?.dual.to_radians(); + let latitude = self.latitude_deg()?.dual.to_radians(); let sin_lat = latitude.sin(); if (latitude - OHyperdual::from(1.0)).abs() < 0.1 { // We are near poles, let's use another formulation. @@ -570,7 +567,7 @@ impl OrbitDual { / ((OHyperdual::from(1.0) - e2 * sin_lat.powi(2)).sqrt()); Ok(OrbitPartial { dual: self.z / latitude.sin() - s_earth, - param: StateParameter::GeodeticHeight, + param: StateParameter::Height, }) } else { let c_earth = OHyperdual::from(semi_major_radius) @@ -578,14 +575,14 @@ impl OrbitDual { let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt(); Ok(OrbitPartial { dual: r_delta / latitude.cos() - c_earth, - param: StateParameter::GeodeticHeight, + param: StateParameter::Height, }) } } /// Returns the right ascension of this orbit in degrees #[allow(clippy::eq_op)] - pub fn right_ascension(&self) -> OrbitPartial { + pub fn right_ascension_deg(&self) -> OrbitPartial { OrbitPartial { dual: (self.y.atan2(self.x)).to_degrees(), param: StateParameter::RightAscension, @@ -594,19 +591,20 @@ impl OrbitDual { /// Returns the declination of this orbit in degrees #[allow(clippy::eq_op)] - pub fn declination(&self) -> OrbitPartial { + pub fn declination_deg(&self) -> OrbitPartial { OrbitPartial { - dual: (self.z / self.rmag().dual).asin().to_degrees(), + dual: (self.z / self.rmag_km().dual).asin().to_degrees(), param: StateParameter::Declination, } } /// Returns the semi minor axis in km, includes code for a hyperbolic orbit - pub fn semi_minor_axis(&self) -> PhysicsResult { + pub fn semi_minor_axis_km(&self) -> PhysicsResult { if self.ecc()?.real() <= 1.0 { Ok(OrbitPartial { - dual: ((self.sma()?.dual * self.ecc()?.dual).powi(2) - self.sma()?.dual.powi(2)) - .sqrt(), + dual: ((self.sma_km()?.dual * self.ecc()?.dual).powi(2) + - self.sma_km()?.dual.powi(2)) + .sqrt(), param: StateParameter::SemiMinorAxis, }) } else { @@ -622,7 +620,7 @@ impl OrbitDual { /// Returns the velocity declination of this orbit in degrees pub fn velocity_declination(&self) -> OrbitPartial { OrbitPartial { - dual: (self.vz / self.vmag().dual).asin().to_degrees(), + dual: (self.vz / self.vmag_km_s().dual).asin().to_degrees(), param: StateParameter::VelocityDeclination, } } @@ -630,19 +628,19 @@ impl OrbitDual { /// Returns the $C_3$ of this orbit pub fn c3(&self) -> PhysicsResult { Ok(OrbitPartial { - dual: -OHyperdual::from(self.frame.mu_km3_s2()?) / self.sma()?.dual, + dual: -OHyperdual::from(self.frame.mu_km3_s2()?) / self.sma_km()?.dual, param: StateParameter::C3, }) } /// Returns the hyperbolic anomaly in degrees between 0 and 360.0 - pub fn hyperbolic_anomaly(&self) -> Result { + pub fn hyperbolic_anomaly_deg(&self) -> Result { let ecc = self.ecc().context(AstroPhysicsSnafu)?; if ecc.real() <= 1.0 { Err(AstroError::NotHyperbolic) } else { let (sin_ta, cos_ta) = self - .ta() + .ta_deg() .context(AstroPhysicsSnafu)? .dual .to_radians() diff --git a/src/cosmic/spacecraft.rs b/src/cosmic/spacecraft.rs index bf9b6a73..c874e7d5 100644 --- a/src/cosmic/spacecraft.rs +++ b/src/cosmic/spacecraft.rs @@ -18,7 +18,7 @@ use anise::astro::PhysicsResult; use anise::constants::frames::EARTH_J2000; -pub use anise::prelude::{Almanac, Orbit}; +pub use anise::prelude::Orbit; use nalgebra::Vector3; use serde::{Deserialize, Serialize}; @@ -591,17 +591,17 @@ impl State for Spacecraft { .fpa_deg() .context(AstroPhysicsSnafu) .context(StateAstroSnafu { param }), - StateParameter::GeodeticHeight => self + StateParameter::Height => self .orbit .height_km() .context(AstroPhysicsSnafu) .context(StateAstroSnafu { param }), - StateParameter::GeodeticLatitude => self + StateParameter::Latitude => self .orbit .latitude_deg() .context(AstroPhysicsSnafu) .context(StateAstroSnafu { param }), - StateParameter::GeodeticLongitude => Ok(self.orbit.longitude_deg()), + StateParameter::Longitude => Ok(self.orbit.longitude_deg()), StateParameter::Hmag => self .orbit .hmag() diff --git a/src/dynamics/solarpressure.rs b/src/dynamics/solarpressure.rs index e7fffac8..6f1cff28 100644 --- a/src/dynamics/solarpressure.rs +++ b/src/dynamics/solarpressure.rs @@ -23,10 +23,15 @@ use crate::linalg::{Const, Matrix3, Vector3}; use anise::almanac::Almanac; use anise::constants::frames::SUN_J2000; use hyperdual::{hyperspace_from_vector, linalg::norm, Float, OHyperdual}; +use log::warn; use snafu::ResultExt; use std::fmt; use std::sync::Arc; +// Default solar flux in W/m^2 +#[allow(non_upper_case_globals)] +pub const SOLAR_FLUX_W_m2: f64 = 1367.0; + /// Computation of solar radiation pressure is based on STK: http://help.agi.com/stk/index.htm#gator/eq-solar.htm . #[derive(Clone)] pub struct SolarPressure { @@ -47,9 +52,21 @@ impl SolarPressure { action: "planetary data from third body not loaded", } })?, - shadow_bodies, + shadow_bodies: shadow_bodies + .iter() + .filter_map(|object| match almanac.frame_from_uid(object) { + Ok(loaded_obj) => Some(loaded_obj), + Err(e) => { + warn!("when initializing SRP model for {object}, {e}"); + None + } + }) + .collect(), }; - Ok(Self { phi: 1367.0, e_loc }) + Ok(Self { + phi: SOLAR_FLUX_W_m2, + e_loc, + }) } /// Accounts for the shadowing of only one body and will set the solar flux at 1 AU to: Phi = 1367.0 @@ -67,6 +84,14 @@ impl SolarPressure { me.phi = flux_w_m2; Ok(Arc::new(me)) } + + /// Solar radiation pressure force model accounting for the provided shadow bodies. + pub fn new( + shadow_bodies: Vec, + almanac: Arc, + ) -> Result, DynamicsError> { + Ok(Arc::new(Self::default_raw(shadow_bodies, almanac)?)) + } } impl ForceModel for SolarPressure { diff --git a/src/errors.rs b/src/errors.rs index a8278aa4..ff4eb164 100644 --- a/src/errors.rs +++ b/src/errors.rs @@ -145,4 +145,11 @@ pub enum MonteCarloError { StateError { source: StateError }, #[snafu(display("for {param}, expected percentage between 0.0 and 1.0 but got {prct}"))] ParamPercentage { param: StateParameter, prct: f64 }, + #[snafu(display( + "could {action} because none of the Monte Carlo {num_runs} runs were successful" + ))] + NoSuccessfulRuns { + action: &'static str, + num_runs: usize, + }, } diff --git a/src/io/tracking_data.rs b/src/io/tracking_data.rs index 3f5e4bfb..cd554c6f 100644 --- a/src/io/tracking_data.rs +++ b/src/io/tracking_data.rs @@ -108,7 +108,7 @@ impl DynamicTrackingArc { let mut rate_avail = false; for field in &reader.schema().fields { match field.name().as_str() { - "Epoch:TAI (s)" => has_epoch = true, + "Epoch (UTC)" => has_epoch = true, "Tracking device" => has_tracking_dev = true, "Range (km)" => range_avail = true, "Doppler (km/s)" => rate_avail = true, @@ -119,7 +119,7 @@ impl DynamicTrackingArc { ensure!( has_epoch, MissingDataSnafu { - which: "Epoch: TAI (s)" + which: "Epoch (UTC)" } ); @@ -185,10 +185,10 @@ impl DynamicTrackingArc { .unwrap(); let epochs = batch - .column_by_name("Epoch:TAI (s)") + .column_by_name("Epoch (UTC)") .unwrap() .as_any() - .downcast_ref::() + .downcast_ref::() .unwrap(); // Now read the data depending on what we're deserializing as @@ -213,7 +213,11 @@ impl DynamicTrackingArc { arc.measurements.push(( tracking_device.value(i).to_string(), Msr::from_observation( - Epoch::from_tai_seconds(epochs.value(i)), + Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?, OVector::::from_iterator([ range_data.value(i), rate_data.value(i), @@ -235,7 +239,11 @@ impl DynamicTrackingArc { arc.measurements.push(( tracking_device.value(i).to_string(), Msr::from_observation( - Epoch::from_tdb_seconds(epochs.value(i)), + Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?, OVector::::from_iterator([ range_data.value(i) ]), @@ -256,7 +264,11 @@ impl DynamicTrackingArc { arc.measurements.push(( tracking_device.value(i).to_string(), Msr::from_observation( - Epoch::from_tdb_seconds(epochs.value(i)), + Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?, OVector::::from_iterator([ rate_data.value(i) ]), diff --git a/src/io/trajectory_data.rs b/src/io/trajectory_data.rs index 39430948..3c1477e7 100644 --- a/src/io/trajectory_data.rs +++ b/src/io/trajectory_data.rs @@ -17,6 +17,7 @@ */ use anise::frames::Frame; +use arrow::array::StringArray; use arrow::{array::Float64Array, record_batch::RecordBatchReader}; use hifitime::Epoch; use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder; @@ -127,7 +128,7 @@ impl TrajectoryLoader { })?; for field in &reader.schema().fields { - if field.name().as_str() == "Epoch:TAI (s)" { + if field.name().as_str() == "Epoch (UTC)" { has_epoch = true; } else { for potential_field in &mut found_fields { @@ -156,7 +157,7 @@ impl TrajectoryLoader { ensure!( has_epoch, MissingDataSnafu { - which: "Epoch: TAI (s)" + which: "Epoch (UTC)" } ); @@ -207,10 +208,10 @@ impl TrajectoryLoader { let batch = maybe_batch.unwrap(); let epochs = batch - .column_by_name("Epoch:TAI (s)") + .column_by_name("Epoch (UTC)") .unwrap() .as_any() - .downcast_ref::() + .downcast_ref::() .unwrap(); let mut shared_data = vec![]; @@ -243,7 +244,11 @@ impl TrajectoryLoader { // Build the states for i in 0..batch.num_rows() { let mut state = S::zeros(); - state.set_epoch(Epoch::from_tai_seconds(epochs.value(i))); + state.set_epoch(Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| { + InputOutputError::Inconsistency { + msg: format!("{e} when parsing epoch"), + } + })?); state.set_frame(frame.unwrap()); // We checked it was set above with an ensure! call state.unset_stm(); // We don't have any STM data, so let's unset this. diff --git a/src/io/watermark.rs b/src/io/watermark.rs index c48ae31d..3c095b6a 100644 --- a/src/io/watermark.rs +++ b/src/io/watermark.rs @@ -60,5 +60,5 @@ pub(crate) fn pq_writer(metadata: Option>) -> Option String { - format!("{} {}", build::PROJECT_NAME, build::PKG_VERSION) + format!("Nyx v{}", build::PKG_VERSION) } diff --git a/src/lib.rs b/src/lib.rs index 0b85511b..505eddd4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -26,6 +26,10 @@ Refer to [nyxspace.com](https://nyxspace.com) for a user guide, a show case, the // Allow confusable identifiers, as the code tries to use the literature's notation where possible. #![allow(confusable_idents)] +#[macro_use] +extern crate log; +extern crate nalgebra as na; + /// Provides all the propagators / integrators available in `nyx`. pub mod propagators; @@ -63,11 +67,6 @@ pub mod mc; /// Polynomial and fitting module pub mod polyfit; -#[macro_use] -extern crate log; -extern crate hifitime; -extern crate nalgebra as na; - /// Re-export of hifitime pub mod time { pub use hifitime::prelude::*; diff --git a/src/mc/montecarlo.rs b/src/mc/montecarlo.rs index 209e327d..efb2b1a6 100644 --- a/src/mc/montecarlo.rs +++ b/src/mc/montecarlo.rs @@ -16,7 +16,7 @@ along with this program. If not, see . */ -use super::{Generator, Pcg64Mcg}; +use super::Pcg64Mcg; use crate::dynamics::Dynamics; use crate::linalg::allocator::Allocator; use crate::linalg::DefaultAllocator; @@ -31,6 +31,8 @@ use crate::time::{Duration, Epoch}; use crate::State; use anise::almanac::Almanac; use indicatif::{ParallelProgressIterator, ProgressBar, ProgressStyle}; +use log::info; +use rand::SeedableRng; use rand_distr::Distribution; use rayon::prelude::ParallelIterator; use rayon::prelude::*; @@ -43,7 +45,7 @@ use std::time::Instant as StdInstant; /// A Monte Carlo framework, automatically running on all threads via a thread pool. This framework is targeted toward analysis of time-continuous variables. /// One caveat of the design is that the trajectory is used for post processing, not each individual state. This may prevent some event switching from being shown in GNC simulations. -pub struct MonteCarlo + Copy> +pub struct MonteCarlo>> where DefaultAllocator: Allocator + Allocator @@ -51,20 +53,34 @@ where + Allocator, { /// Seed of the [64bit PCG random number generator](https://www.pcg-random.org/index.html) - pub seed: u64, + pub seed: Option, /// Generator of states for the Monte Carlo run - pub generator: Generator, + pub random_state: Distr, /// Name of this run, will be reflected in the progress bar and in the output structure pub scenario: String, + pub nominal_state: S, } -impl + Copy> MonteCarlo +impl>> MonteCarlo where DefaultAllocator: Allocator + Allocator + Allocator + Allocator, { + pub fn new( + nominal_state: S, + random_variable: Distr, + scenario: String, + seed: Option, + ) -> Self { + Self { + random_state: random_variable, + seed, + scenario, + nominal_state, + } + } // Just the template for the progress bar fn progress_bar(&self, num_runs: usize) -> ProgressBar { let pb = ProgressBar::new(num_runs.try_into().unwrap()); @@ -126,7 +142,7 @@ where ::VecLength>>::Buffer: Send, { // Generate the initial states - let init_states = self.generate_states(skip, num_runs); + let init_states = self.generate_states(skip, num_runs, self.seed); // Setup the progress bar let pb = self.progress_bar(num_runs); // Setup the thread friendly communication @@ -135,6 +151,7 @@ where // Generate all states (must be done separately because the rng is not thread safe) #[cfg(not(target_arch = "wasm32"))] let start = StdInstant::now(); + init_states.par_iter().progress_with(pb).for_each_with( (prop, tx), |(prop, tx), (index, dispersed_state)| { @@ -158,7 +175,7 @@ where #[cfg(not(target_arch = "wasm32"))] { let clock_time = StdInstant::now() - start; - println!( + info!( "Propagated {} states in {}", num_runs, clock_time.as_secs_f64() * Unit::Second @@ -220,7 +237,7 @@ where ::VecLength>>::Buffer: Send, { // Generate the initial states - let init_states = self.generate_states(skip, num_runs); + let init_states = self.generate_states(skip, num_runs, self.seed); // Setup the progress bar let pb = self.progress_bar(num_runs); // Setup the thread friendly communication @@ -253,7 +270,7 @@ where #[cfg(not(target_arch = "wasm32"))] { let clock_time = StdInstant::now() - start; - println!( + info!( "Propagated {} states in {}", num_runs, clock_time.as_secs_f64() * Unit::Second @@ -272,12 +289,20 @@ where /// Set up the seed and generate the states. This is useful for checking the generated states before running a large scale Monte Carlo. #[must_use = "Generated states for a Monte Carlo run must be used"] - pub fn generate_states(&self, skip: usize, num_runs: usize) -> Vec<(usize, DispersedState)> { + pub fn generate_states( + &self, + skip: usize, + num_runs: usize, + seed: Option, + ) -> Vec<(usize, DispersedState)> { // Setup the RNG - let rng = Pcg64Mcg::new(self.seed.into()); + let rng = match seed { + Some(seed) => Pcg64Mcg::new(seed), + None => Pcg64Mcg::from_entropy(), + }; // Generate the states, forcing the borrow as specified in the `sample_iter` docs. - (&self.generator) + (&self.random_state) .sample_iter(rng) .skip(skip) .take(num_runs) @@ -286,7 +311,8 @@ where } } -impl + Copy> fmt::Display for MonteCarlo +impl>> fmt::Display + for MonteCarlo where DefaultAllocator: Allocator + Allocator @@ -296,13 +322,14 @@ where fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!( f, - "{} - Nyx Monte Carlo - seed: {}", + "{} - Nyx Monte Carlo - seed: {:?}", self.scenario, self.seed ) } } -impl + Copy> fmt::LowerHex for MonteCarlo +impl>> fmt::LowerHex + for MonteCarlo where DefaultAllocator: Allocator + Allocator @@ -313,7 +340,7 @@ where fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!( f, - "mc-data-{}-seed-{}", + "mc-data-{}-seed-{:?}", self.scenario.replace(' ', "-"), self.seed ) diff --git a/src/mc/results.rs b/src/mc/results.rs index 7e3a07eb..ada9eb1d 100644 --- a/src/mc/results.rs +++ b/src/mc/results.rs @@ -15,14 +15,32 @@ You should have received a copy of the GNU Affero General Public License along with this program. If not, see . */ + +use std::collections::HashMap; +use std::error::Error; +use std::fs::File; +use std::path::{Path, PathBuf}; +use std::sync::Arc; + +use crate::errors::{MonteCarloError, NoSuccessfulRunsSnafu, StateError}; +use crate::io::watermark::pq_writer; +use crate::io::{ExportCfg, InputOutputError}; use crate::linalg::allocator::Allocator; use crate::linalg::DefaultAllocator; +use crate::md::prelude::GuidanceMode; use crate::md::trajectory::{Interpolatable, Traj}; -use crate::md::StateParameter; +use crate::md::{EventEvaluator, StateParameter}; use crate::propagators::PropagationError; -use crate::time::{Duration, Epoch}; -use crate::NyxError; +use crate::time::{Duration, Epoch, TimeUnits}; +use anise::almanac::Almanac; +use anise::constants::frames::EARTH_J2000; +use arrow::array::{Array, Float64Builder, Int32Builder, StringBuilder}; +use arrow::datatypes::{DataType, Field, Schema}; +use arrow::record_batch::RecordBatch; +use hifitime::TimeScale; +use parquet::arrow::ArrowWriter; pub use rstats::Stats; +use snafu::ensure; use super::DispersedState; @@ -216,7 +234,7 @@ where } /// Returns the dispersion values of the requested state parameter - pub fn dispersion_values_of(&self, param: StateParameter) -> Result, NyxError> { + pub fn dispersion_values_of(&self, param: StateParameter) -> Result, MonteCarloError> { let mut report = Vec::with_capacity(self.runs.len()); 'run_loop: for run in &self.runs { for (dparam, val) in &run.dispersed_state.actual_dispersions { @@ -226,11 +244,192 @@ where } } // Oh, this parameter was not found! - return Err(NyxError::StateParameterUnavailable { - param, - msg: "not among dispersions of Monte Carlo setup".to_string(), + return Err(MonteCarloError::StateError { + source: StateError::Unavailable { param }, }); } Ok(report) } + + pub fn to_parquet>( + &self, + path: P, + events: Option>>, + cfg: ExportCfg, + almanac: Arc, + ) -> Result> { + let tick = Epoch::now().unwrap(); + info!("Exporting Monte Carlo results to parquet file..."); + + // Grab the path here before we move stuff. + let path_buf = cfg.actual_path(path); + + // Build the schema + let mut hdrs = vec![ + Field::new("Epoch (UTC)", DataType::Utf8, false), + Field::new("Monte Carlo Run Index", DataType::Int32, false), + ]; + + // Use the first successful run to build up some data shared for all + let mut frame = EARTH_J2000; + let mut fields = match cfg.fields { + Some(fields) => fields, + None => S::export_params(), + }; + + let mut start = None; + let mut end = None; + + // Literally all of the states of all the successful runs. + let mut all_states: Vec = vec![]; + let mut run_indexes: Vec = vec![]; + + for run in &self.runs { + if let Ok(success) = &run.result { + if start.is_none() { + // No need to check other states. + frame = success.state.frame(); + + // Check that we can retrieve this information + fields.retain(|param| success.state.value(*param).is_ok()); + + start = Some(success.traj.first().epoch()); + end = Some(success.state.epoch()); + } + + // Build the states iterator. + let states = + if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { + // Must interpolate the data! + let start = cfg.start_epoch.unwrap_or_else(|| start.unwrap()); + let end = cfg.end_epoch.unwrap_or_else(|| end.unwrap()); + let step = cfg.step.unwrap_or_else(|| 1.minutes()); + success + .traj + .every_between(step, start, end) + .collect::>() + } else { + success.traj.states.to_vec() + }; + // Mark all of these states as part of this run index. + for _ in 0..states.len() { + run_indexes.push(run.index as i32); + } + all_states.extend(states.iter()); + } + } + + ensure!( + start.is_some(), + NoSuccessfulRunsSnafu { + action: "export", + num_runs: self.runs.len() + } + ); + + let more_meta = Some(vec![( + "Frame".to_string(), + serde_dhall::serialize(&frame).to_string().map_err(|e| { + Box::new(InputOutputError::SerializeDhall { + what: format!("frame `{frame}`"), + err: e.to_string(), + }) + })?, + )]); + + for field in &fields { + hdrs.push(field.to_field(more_meta.clone())); + } + + if let Some(events) = events.as_ref() { + for event in events { + let field = Field::new(format!("{event}"), DataType::Float64, false); + hdrs.push(field); + } + } + + // Build the schema + let schema = Arc::new(Schema::new(hdrs)); + let mut record: Vec> = Vec::new(); + + // Build all of the records + + // Epochs + let mut utc_epoch = StringBuilder::new(); + let mut idx_col = Int32Builder::new(); + for (sno, s) in all_states.iter().enumerate() { + utc_epoch.append_value(&s.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); + + // Copy this a bunch of times because all columns must have the same length + idx_col.append_value(run_indexes[sno]); + } + record.push(Arc::new(utc_epoch.finish())); + record.push(Arc::new(idx_col.finish())); + + // Add all of the fields + for field in fields { + if field == StateParameter::GuidanceMode { + let mut guid_mode = StringBuilder::new(); + for s in &all_states { + guid_mode + .append_value(format!("{:?}", GuidanceMode::from(s.value(field).unwrap()))); + } + record.push(Arc::new(guid_mode.finish())); + } else { + let mut data = Float64Builder::new(); + for s in &all_states { + data.append_value(s.value(field).unwrap()); + } + record.push(Arc::new(data.finish())); + } + } + + info!( + "Serialized {} states from {} to {}", + all_states.len(), + start.unwrap(), + end.unwrap() + ); + + // Add all of the evaluated events + if let Some(events) = events { + info!("Evaluating {} event(s)", events.len()); + for event in events { + let mut data = Float64Builder::new(); + for s in &all_states { + data.append_value(event.eval(s, almanac.clone()).map_err(Box::new)?); + } + record.push(Arc::new(data.finish())); + } + } + + // Serialize all of the devices and add that to the parquet file too. + let mut metadata = HashMap::new(); + metadata.insert( + "Purpose".to_string(), + "Monte Carlo Trajectory 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()?; + + // Return the path this was written to + let tock_time = Epoch::now().unwrap() - tick; + info!( + "Trajectory written to {} in {tock_time}", + path_buf.display() + ); + Ok(path_buf) + } } diff --git a/src/md/events/search.rs b/src/md/events/search.rs index c1df06f8..29a9c2c0 100644 --- a/src/md/events/search.rs +++ b/src/md/events/search.rs @@ -215,7 +215,7 @@ where }); } let heuristic = (end_epoch - start_epoch) / 100; - info!("Searching for {event} with initial heuristic of {heuristic}",); + info!("Searching for {event} with initial heuristic of {heuristic}"); let (sender, receiver) = channel(); diff --git a/src/md/param.rs b/src/md/param.rs index 1069a982..311d8fd8 100644 --- a/src/md/param.rs +++ b/src/md/param.rs @@ -67,11 +67,11 @@ pub enum StateParameter { /// fuel mass in kilograms FuelMass, /// Geodetic height (km) - GeodeticHeight, + Height, /// Geodetic latitude (deg) - GeodeticLatitude, + Latitude, /// Geodetic longitude (deg) - GeodeticLongitude, + Longitude, /// Return the guidance mode of the spacecraft GuidanceMode, /// Orbital momentum @@ -142,8 +142,8 @@ impl StateParameter { Self::AoL | Self::AoP | Self::Declination - | Self::GeodeticLatitude - | Self::GeodeticLongitude + | Self::Latitude + | Self::Longitude | Self::FlightPathAngle | Self::Inclination | Self::RightAscension @@ -163,7 +163,7 @@ impl StateParameter { Self::ApoapsisRadius | Self::BdotR | Self::BdotT - | Self::GeodeticHeight + | Self::Height | Self::Hmag | Self::HX | Self::HY @@ -218,8 +218,8 @@ impl StateParameter { Self::AoL | Self::AoP | Self::Declination - | Self::GeodeticLatitude - | Self::GeodeticLongitude + | Self::Latitude + | Self::Longitude | Self::FlightPathAngle | Self::Inclination | Self::RightAscension @@ -237,7 +237,7 @@ impl StateParameter { Self::ApoapsisRadius | Self::BdotR | Self::BdotT - | Self::GeodeticHeight + | Self::Height | Self::Hmag | Self::HX | Self::HY @@ -279,6 +279,16 @@ impl StateParameter { impl StateParameter { /// Returns the parquet field of this parameter pub(crate) fn to_field(self, more_meta: Option>) -> Field { + self.to_field_generic(false, more_meta) + } + + /// Returns the parquet field of this parameter + pub(crate) fn to_cov_field(self, more_meta: Option>) -> Field { + self.to_field_generic(true, more_meta) + } + + /// Returns the parquet field of this parameter + fn to_field_generic(self, is_sigma: bool, more_meta: Option>) -> Field { let mut meta = HashMap::new(); meta.insert("unit".to_string(), self.unit().to_string()); if let Some(more_data) = more_meta { @@ -288,7 +298,11 @@ impl StateParameter { } Field::new( - format!("{self}"), + if is_sigma { + format!("Sigma {self}") + } else { + format!("{self}") + }, if self == Self::GuidanceMode { DataType::Utf8 } else { @@ -327,9 +341,9 @@ impl FromStr for StateParameter { "fpa" => Ok(Self::FlightPathAngle), "fuel_mass" => Ok(Self::FuelMass), "guidance_mode" | "mode" => Ok(Self::GuidanceMode), - "geodetic_height" => Ok(Self::GeodeticHeight), - "geodetic_latitude" => Ok(Self::GeodeticLatitude), - "geodetic_longitude" => Ok(Self::GeodeticLongitude), + "geodetic_height" => Ok(Self::Height), + "geodetic_latitude" => Ok(Self::Latitude), + "geodetic_longitude" => Ok(Self::Longitude), "ha" => Ok(Self::HyperbolicAnomaly), "hmag" => Ok(Self::Hmag), "hx" => Ok(Self::HX), @@ -387,9 +401,9 @@ impl fmt::Display for StateParameter { Self::FlightPathAngle => "fpa", Self::FuelMass => "fuel_mass", Self::GuidanceMode => "guidance_mode", - Self::GeodeticHeight => "geodetic_height", - Self::GeodeticLatitude => "geodetic_latitude", - Self::GeodeticLongitude => "geodetic_longitude", + Self::Height => "geodetic_height", + Self::Latitude => "geodetic_latitude", + Self::Longitude => "geodetic_longitude", Self::HyperbolicAnomaly => "ha", Self::Hmag => "hmag", Self::HX => "hx", @@ -453,9 +467,9 @@ mod ut_state_param { StateParameter::FlightPathAngle, StateParameter::FuelMass, StateParameter::GuidanceMode, - StateParameter::GeodeticHeight, - StateParameter::GeodeticLatitude, - StateParameter::GeodeticLongitude, + StateParameter::Height, + StateParameter::Latitude, + StateParameter::Longitude, StateParameter::HyperbolicAnomaly, StateParameter::Hmag, StateParameter::HX, diff --git a/src/md/trajectory/interpolatable.rs b/src/md/trajectory/interpolatable.rs index c6a3fb55..9c713055 100644 --- a/src/md/trajectory/interpolatable.rs +++ b/src/md/trajectory/interpolatable.rs @@ -132,9 +132,9 @@ impl Interpolatable for Spacecraft { | StateParameter::VY | StateParameter::VZ | StateParameter::HyperbolicAnomaly - | StateParameter::GeodeticHeight - | StateParameter::GeodeticLatitude - | StateParameter::GeodeticLongitude + | StateParameter::Height + | StateParameter::Latitude + | StateParameter::Longitude ) }) .collect::>(); diff --git a/src/md/trajectory/mod.rs b/src/md/trajectory/mod.rs index 32f62728..cde6e3c9 100644 --- a/src/md/trajectory/mod.rs +++ b/src/md/trajectory/mod.rs @@ -20,7 +20,6 @@ use anise::math::interpolation::InterpolationError; use snafu::prelude::*; mod interpolatable; -// mod orbit_traj; mod sc_traj; mod traj; mod traj_it; diff --git a/src/md/trajectory/orbit_traj.rs b/src/md/trajectory/orbit_traj.rs deleted file mode 100644 index cbdd78d2..00000000 --- a/src/md/trajectory/orbit_traj.rs +++ /dev/null @@ -1,543 +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 super::TrajError; -use super::{ExportCfg, Traj}; -use crate::cosmic::{Cosm, Frame, Orbit}; -use crate::errors::NyxError; -use crate::io::watermark::prj_name_ver; -use crate::md::prelude::StateParameter; -use crate::md::EventEvaluator; -use crate::time::{Epoch, Format, Formatter, TimeUnits}; -use crate::{Spacecraft, State}; -use std::collections::{HashMap, HashSet}; -use std::error::Error; -use std::fs::File; -use std::io::{BufRead, BufReader, BufWriter, Write}; -use std::path::{Path, PathBuf}; -use std::str::FromStr; -use std::sync::Arc; -#[cfg(not(target_arch = "wasm32"))] -use std::time::Instant; - -impl Traj { - /// Allows converting the source trajectory into the (almost) equivalent trajectory in another frame. - /// This simply converts each state into the other frame and may lead to aliasing due to the Nyquist–Shannon sampling theorem. - #[allow(clippy::map_clone)] - pub fn to_frame(&self, new_frame: Frame, almanac: Arc) -> Result { - if self.states.is_empty() { - return Err(NyxError::Trajectory { - source: TrajError::CreationError { - msg: "No trajectory to convert".to_string(), - }, - }); - } - - #[cfg(not(target_arch = "wasm32"))] - let start_instant = Instant::now(); - let mut traj = Self::new(); - for state in &self.states { - traj.states.push(cosm.frame_chg(state, new_frame)); - } - traj.finalize(); - - #[cfg(not(target_arch = "wasm32"))] - debug!( - "Converted trajectory from {} to {} in {} ms: {traj}", - self.first().frame, - new_frame, - (Instant::now() - start_instant).as_millis() - ); - - #[cfg(target_arch = "wasm32")] - debug!( - "Converted trajectory from {} to {}: {traj}", - self.first().frame, - new_frame, - ); - - Ok(traj) - } - - /// Exports this trajectory to the provided filename in parquet format with only the epoch, the geodetic latitude, longitude, and height at one state per minute. - /// Must provide a body fixed frame to correctly compute the latitude and longitude. - #[allow(clippy::identity_op)] - pub fn to_groundtrack_parquet>( - &self, - path: P, - body_fixed_frame: Frame, - events: Option>>, - metadata: Option>, - almanac: Arc, - ) -> Result> { - let traj = self.to_frame(body_fixed_frame, cosm)?; - - let mut cfg = ExportCfg::builder() - .step(1.minutes()) - .fields(vec![ - StateParameter::GeodeticLatitude, - StateParameter::GeodeticLongitude, - StateParameter::GeodeticHeight, - StateParameter::Rmag, - ]) - .build(); - cfg.metadata = metadata; - - traj.to_parquet(path, events, cfg) - } - - /// Convert this orbit trajectory into a spacecraft trajectory by copying the provided template and setting its orbit state to that of each state of the trajectory - pub fn upcast(&self, template: Spacecraft) -> Traj { - let mut out = Traj::new(); - for orbit in &self.states { - out.states.push(template.with_orbit(*orbit)); - } - out - } - - /// Initialize a new orbit trajectory from the path to a CCSDS OEM file. - /// - /// # Limitations - /// 1. Only text versions of the OEM format are supported - /// 2. The covariance information, if present, is ignored - /// 3. Only one spacecraft per OEM file is supported. - /// - /// # Thanks - /// GPT-4 because I didn't want to spend too much time coding this up since it'll be a feature in ANISE. - pub fn from_oem_file>(path: P) -> Result { - let cosm = Cosm::de438(); - // Open the file - let file = File::open(path).map_err(|e| NyxError::CCSDS { - msg: format!("File opening error: {e}"), - })?; - let reader = BufReader::new(file); - - // Parse the Orbit Element messages - let mut frame: Option = None; - let mut time_system = String::new(); - - let ignored_tokens: HashSet<_> = [ - "CCSDS_OMM_VERS".to_string(), - "CREATION_DATE".to_string(), - "ORIGINATOR".to_string(), - ] - .into(); - - let mut traj = Self::default(); - - let mut parse = false; - - 'lines: for (lno, line) in reader.lines().enumerate() { - let line = line.map_err(|e| NyxError::CCSDS { - msg: format!("File read error: {e}"), - })?; - let line = line.trim(); - if line.is_empty() { - continue; - } - - if ignored_tokens.iter().any(|t| line.starts_with(t)) { - continue 'lines; - } - if line.starts_with("OBJECT_NAME") { - // Extract the object ID from the line - let parts: Vec<&str> = line.split('=').collect(); - let name = parts[1].trim().to_string(); - debug!("[line: {}] Found object {name}", lno + 1); - traj.name = Some(name); - } else if line.starts_with("CENTER_NAME") { - let parts: Vec<&str> = line.split('=').collect(); - let center_name = parts[1].trim(); - frame = Some(cosm.try_frame(&format!("{center_name} J2000"))?); - } else if line.starts_with("TIME_SYSTEM") { - let parts: Vec<&str> = line.split('=').collect(); - time_system = parts[1].trim().to_string(); - debug!("[line: {}] Found time system `{time_system}`", lno + 1); - } else if line.starts_with("META_STOP") { - // We can start parsing now - parse = true; - } else if line.starts_with("META_START") { - // Stop the parsing - parse = false; - } else if line.starts_with("COVARIANCE_START") { - // Stop the parsing - warn!("[line: {}] Skipping covariance in OEM parsing", lno + 1); - parse = false; - } else if parse { - // Split the line into components - let parts: Vec<&str> = line.split_whitespace().collect(); - - if parts.len() < 7 { - debug!("[line: {}] Could not understand `{parts:?}`", lno + 1); - } else { - // Extract the values - let epoch_str = format!("{} {time_system}", parts[0]); - match parts[1].parse::() { - Ok(x_km) => { - // Look good! - let y_km = parts[2].parse::().unwrap(); - let z_km = parts[3].parse::().unwrap(); - let vx_km_s = parts[4].parse::().unwrap(); - let vy_km_s = parts[5].parse::().unwrap(); - let vz_km_s = parts[6].parse::().unwrap(); - - let orbit = Orbit { - epoch: Epoch::from_str(epoch_str.trim()).map_err(|e| { - NyxError::CCSDS { - msg: format!("Parsing epoch error: {e}"), - } - })?, - x_km, - y_km, - z_km, - vx_km_s, - vy_km_s, - vz_km_s, - frame: frame.unwrap(), - stm: None, - }; - - traj.states.push(orbit); - } - Err(_) => { - // Probably a comment - debug!("[line: {}] Could not parse `{parts:?}`", lno + 1); - continue; - } - }; - } - } - } - - traj.finalize(); - - Ok(traj) - } - - pub fn to_oem_file>( - &self, - path: P, - cfg: ExportCfg, - ) -> Result { - if self.states.is_empty() { - return Err(NyxError::CCSDS { - msg: "Cannot export an empty trajectory to OEM".to_string(), - }); - } - let tick = Epoch::now().unwrap(); - info!("Exporting trajectory to CCSDS OEM file..."); - - // Grab the path here before we move stuff. - let path_buf = cfg.actual_path(path); - - let metadata = cfg.metadata.unwrap_or_default(); - - let file = File::create(&path_buf).map_err(|e| NyxError::CCSDS { - msg: format!("File creation error: {e}"), - })?; - let mut writer = BufWriter::new(file); - - let err_hdlr = |e| NyxError::CCSDS { - msg: format!("Could not write: {e}"), - }; - - // Build the states iterator -- this does require copying the current states but I can't either get a reference or a copy of all the states. - let states = if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { - // Must interpolate the data! - let start = cfg.start_epoch.unwrap_or_else(|| self.first().epoch()); - let end = cfg.end_epoch.unwrap_or_else(|| self.last().epoch()); - let step = cfg.step.unwrap_or_else(|| 1.minutes()); - self.every_between(step, start, end).collect() - } else { - self.states.to_vec() - }; - - // Epoch formmatter. - let iso8601_no_ts = Format::from_str("%Y-%m-%dT%H:%M:%S.%f").unwrap(); - - // Write mandatory metadata - writeln!(writer, "CCSDS_OMM_VERS = 2.0").map_err(err_hdlr)?; - writeln!( - writer, - "CREATION_DATE = {}", - Formatter::new(Epoch::now().unwrap(), iso8601_no_ts) - ) - .map_err(err_hdlr)?; - writeln!( - writer, - "ORIGINATOR = {}\n", - metadata - .get("originator") - .unwrap_or(&"Nyx Space".to_string()) - ) - .map_err(err_hdlr)?; - - writeln!(writer, "META_START").map_err(err_hdlr)?; - // Write optional metadata - if let Some(object_name) = metadata.get("object_name") { - writeln!(writer, "OBJECT_NAME = {}", object_name).map_err(err_hdlr)?; - } else if let Some(object_name) = &self.name { - writeln!(writer, "OBJECT_NAME = {}", object_name).map_err(err_hdlr)?; - } - - let frame_str = states[0].frame.to_string(); - let splt: Vec<&str> = frame_str.split(' ').collect(); - let center = splt[0]; - let ref_frame = frame_str.replace(center, " "); - writeln!( - writer, - "REF_FRAME = {}", - match ref_frame.trim() { - "J2000" => "ICRF", - _ => ref_frame.trim(), - } - ) - .map_err(err_hdlr)?; - - writeln!(writer, "CENTER_NAME = {center}",).map_err(err_hdlr)?; - - writeln!(writer, "TIME_SYSTEM = {}", states[0].epoch.time_scale).map_err(err_hdlr)?; - - writeln!( - writer, - "START_TIME = {}", - Formatter::new(states[0].epoch(), iso8601_no_ts) - ) - .map_err(err_hdlr)?; - writeln!( - writer, - "USEABLE_START_TIME = {}", - Formatter::new(states[0].epoch(), iso8601_no_ts) - ) - .map_err(err_hdlr)?; - writeln!( - writer, - "USEABLE_STOP_TIME = {}", - Formatter::new(states[states.len() - 1].epoch(), iso8601_no_ts) - ) - .map_err(err_hdlr)?; - writeln!( - writer, - "STOP_TIME = {}", - Formatter::new(states[states.len() - 1].epoch(), iso8601_no_ts) - ) - .map_err(err_hdlr)?; - - writeln!(writer, "META_STOP\n").map_err(err_hdlr)?; - - writeln!( - writer, - "COMMENT Generated by {} provided in AGPLv3 license -- https://nyxspace.com/\n", - prj_name_ver() - ) - .map_err(err_hdlr)?; - - for state in &states { - writeln!( - writer, - "{} {:E} {:E} {:E} {:E} {:E} {:E}", - Formatter::new(state.epoch, iso8601_no_ts), - state.x_km, - state.y_km, - state.z_km, - state.vx_km_s, - state.vy_km_s, - state.vz_km_s - ) - .map_err(err_hdlr)?; - } - - #[allow(clippy::writeln_empty_string)] - writeln!(writer, "").map_err(err_hdlr)?; - - // Return the path this was written to - let tock_time = Epoch::now().unwrap() - tick; - info!( - "Trajectory written to {} in {tock_time}", - path_buf.display() - ); - Ok(path_buf) - } -} - -#[cfg(test)] -mod ut_ccsds_oem { - - use crate::md::prelude::{Cosm, OrbitalDynamics, Propagator}; - use crate::time::{Epoch, TimeUnits}; - use crate::{io::ExportCfg, md::prelude::Traj, Orbit}; - use pretty_env_logger; - use std::env; - use std::str::FromStr; - use std::{collections::HashMap, path::PathBuf}; - - #[test] - fn test_load_oem_leo() { - // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ - let path: PathBuf = [ - env!("CARGO_MANIFEST_DIR"), - "data", - "tests", - "ccsds", - "oem", - "LEO_10s.oem", - ] - .iter() - .collect(); - - let _ = pretty_env_logger::try_init(); - - let traj: Traj = Traj::::from_oem_file(path).unwrap(); - - // This trajectory has two duplicate epochs, which should be removed by the call to finalize() - assert_eq!(traj.states.len(), 361); - assert_eq!(traj.name.unwrap(), "TEST_OBJ".to_string()); - } - - #[test] - fn test_load_oem_meo() { - // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ - let path: PathBuf = [ - env!("CARGO_MANIFEST_DIR"), - "data", - "tests", - "ccsds", - "oem", - "MEO_60s.oem", - ] - .iter() - .collect(); - - let _ = pretty_env_logger::try_init(); - - let traj = Traj::::from_oem_file(path).unwrap(); - - assert_eq!(traj.states.len(), 61); - assert_eq!(traj.name.unwrap(), "TEST_OBJ".to_string()); - } - - #[test] - fn test_load_oem_geo() { - use pretty_env_logger; - use std::env; - - // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ - let path: PathBuf = [ - env!("CARGO_MANIFEST_DIR"), - "data", - "tests", - "ccsds", - "oem", - "GEO_20s.oem", - ] - .iter() - .collect(); - - let _ = pretty_env_logger::try_init(); - - let traj: Traj = Traj::::from_oem_file(path).unwrap(); - - assert_eq!(traj.states.len(), 181); - assert_eq!(traj.name.as_ref().unwrap(), &"TEST_OBJ".to_string()); - - // Reexport this to CCSDS. - let cfg = ExportCfg::builder() - .timestamp(true) - .metadata(HashMap::from([ - ("originator".to_string(), "Test suite".to_string()), - ("object_name".to_string(), "TEST_OBJ".to_string()), - ])) - .build(); - - let path: PathBuf = [ - env!("CARGO_MANIFEST_DIR"), - "output_data", - "GEO_20s_rebuilt.oem", - ] - .iter() - .collect(); - - let out_path = traj.to_oem_file(path.clone(), cfg).unwrap(); - // And reload, make sure we have the same data. - let traj_reloaded: Traj = Traj::::from_oem_file(out_path).unwrap(); - - assert_eq!(traj_reloaded, traj); - - // Now export after trimming one state on either end - let cfg = ExportCfg::builder() - .timestamp(true) - .metadata(HashMap::from([ - ("originator".to_string(), "Test suite".to_string()), - ("object_name".to_string(), "TEST_OBJ".to_string()), - ])) - .step(20.seconds()) - .start_epoch(traj.first().epoch + 1.seconds()) - .end_epoch(traj.last().epoch - 1.seconds()) - .build(); - let out_path = traj.to_oem_file(path, cfg).unwrap(); - // And reload, make sure we have the same data. - let traj_reloaded: Traj = Traj::::from_oem_file(out_path).unwrap(); - - // Note that the number of states has changed because we interpolated with a step similar to the original one but - // we started with a different time. - assert_eq!(traj_reloaded.states.len(), traj.states.len() - 1); - assert_eq!( - traj_reloaded.first().epoch, - traj.first().epoch + 1.seconds() - ); - // Note: because we used a fixed step, the last epoch is actually an offset of step size - end offset - // from the original trajectory - assert_eq!(traj_reloaded.last().epoch, traj.last().epoch - 19.seconds()); - } - - #[test] - fn test_moon_frame_long_prop() { - let cosm = Cosm::de438(); - let epoch = Epoch::from_str("2022-06-13T12:00:00").unwrap(); - let orbit = Orbit::keplerian_altitude( - 350.0, - 0.02, - 30.0, - 45.0, - 85.0, - 0.0, - epoch, - cosm.frame("Moon J2000"), - ); - - let mut traj = Propagator::default_dp78(OrbitalDynamics::two_body()) - .with(orbit) - .for_duration_with_traj(45.days()) - .unwrap() - .1; - // Set the name of this object - traj.name = Some("TEST_MOON_OBJ".to_string()); - - // Export CCSDS OEM file - let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "moon_45days.oem"] - .iter() - .collect(); - - let out_path = traj.to_oem_file(path, ExportCfg::default()).unwrap(); - - // And reload - let traj_reloaded: Traj = Traj::::from_oem_file(out_path).unwrap(); - - assert_eq!(traj, traj_reloaded); - } -} diff --git a/src/md/trajectory/sc_traj.rs b/src/md/trajectory/sc_traj.rs index 27dafe2f..f93e12bd 100644 --- a/src/md/trajectory/sc_traj.rs +++ b/src/md/trajectory/sc_traj.rs @@ -16,21 +16,23 @@ along with this program. If not, see . */ +use anise::constants::orientations::J2000; use anise::prelude::{Almanac, Frame, Orbit}; -use hifitime::Epoch; use snafu::ResultExt; use super::TrajError; use super::{ExportCfg, Traj}; use crate::cosmic::Spacecraft; use crate::errors::{FromAlmanacSnafu, NyxError}; +use crate::io::watermark::prj_name_ver; use crate::md::prelude::StateParameter; use crate::md::EventEvaluator; -use crate::time::{Duration, TimeUnits}; +use crate::time::{Duration, Epoch, Format, Formatter, TimeUnits}; +use crate::State; use std::collections::{HashMap, HashSet}; use std::error::Error; use std::fs::File; -use std::io::{BufRead, BufReader}; +use std::io::{BufRead, BufReader, BufWriter, Write}; use std::path::{Path, PathBuf}; use std::str::FromStr; use std::sync::Arc; @@ -116,9 +118,9 @@ impl Traj { let mut cfg = ExportCfg::builder() .step(1.minutes()) .fields(vec![ - StateParameter::GeodeticLatitude, - StateParameter::GeodeticLongitude, - StateParameter::GeodeticHeight, + StateParameter::Latitude, + StateParameter::Longitude, + StateParameter::Height, StateParameter::Rmag, ]) .build(); @@ -134,14 +136,20 @@ impl Traj { /// Initialize a new spacecraft trajectory from the path to a CCSDS OEM file. /// - /// CCSDS OEM only contains the orbit information, so you must provide a template spacecraft since we'll upcast the orbit trajectory into a spacecraft trajectory. - pub fn from_oem_file>(path: P, template: Spacecraft) -> Result { + /// CCSDS OEM only contains the orbit information but Nyx builds spacecraft trajectories. + /// If not spacecraft template is provided, then a default massless spacecraft will be built. + pub fn from_oem_file>( + path: P, + tpl_option: Option, + ) -> Result { // Open the file let file = File::open(path).map_err(|e| NyxError::CCSDS { msg: format!("File opening error: {e}"), })?; let reader = BufReader::new(file); + let template = tpl_option.unwrap_or_default(); + // Parse the Orbit Element messages let mut time_system = String::new(); @@ -247,4 +255,348 @@ impl Traj { Ok(traj) } + + pub fn to_oem_file>( + &self, + path: P, + cfg: ExportCfg, + ) -> Result { + if self.states.is_empty() { + return Err(NyxError::CCSDS { + msg: "Cannot export an empty trajectory to OEM".to_string(), + }); + } + let tick = Epoch::now().unwrap(); + info!("Exporting trajectory to CCSDS OEM file..."); + + // Grab the path here before we move stuff. + let path_buf = cfg.actual_path(path); + + let metadata = cfg.metadata.unwrap_or_default(); + + let file = File::create(&path_buf).map_err(|e| NyxError::CCSDS { + msg: format!("File creation error: {e}"), + })?; + let mut writer = BufWriter::new(file); + + let err_hdlr = |e| NyxError::CCSDS { + msg: format!("Could not write: {e}"), + }; + + // Build the states iterator -- this does require copying the current states but I can't either get a reference or a copy of all the states. + let states = if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() { + // Must interpolate the data! + let start = cfg.start_epoch.unwrap_or_else(|| self.first().epoch()); + let end = cfg.end_epoch.unwrap_or_else(|| self.last().epoch()); + let step = cfg.step.unwrap_or_else(|| 1.minutes()); + self.every_between(step, start, end).collect() + } else { + self.states.to_vec() + }; + + // Epoch formmatter. + let iso8601_no_ts = Format::from_str("%Y-%m-%dT%H:%M:%S.%f").unwrap(); + + // Write mandatory metadata + writeln!(writer, "CCSDS_OMM_VERS = 2.0").map_err(err_hdlr)?; + writeln!( + writer, + "CREATION_DATE = {}", + Formatter::new(Epoch::now().unwrap(), iso8601_no_ts) + ) + .map_err(err_hdlr)?; + writeln!( + writer, + "ORIGINATOR = {}\n", + metadata + .get("originator") + .unwrap_or(&"Nyx Space".to_string()) + ) + .map_err(err_hdlr)?; + + writeln!(writer, "META_START").map_err(err_hdlr)?; + // Write optional metadata + if let Some(object_name) = metadata.get("object_name") { + writeln!(writer, "OBJECT_NAME = {}", object_name).map_err(err_hdlr)?; + } else if let Some(object_name) = &self.name { + writeln!(writer, "OBJECT_NAME = {}", object_name).map_err(err_hdlr)?; + } + + let first_orbit = states[0].orbit; + let first_frame = first_orbit.frame; + let frame_str = format!( + "{first_frame:e} {}", + match first_frame.orientation_id { + J2000 => "ICRF".to_string(), + _ => format!("{first_frame:o}"), + } + ); + let splt: Vec<&str> = frame_str.split(' ').collect(); + let center = splt[0]; + let ref_frame = frame_str.replace(center, " "); + writeln!( + writer, + "REF_FRAME = {}", + match ref_frame.trim() { + "J2000" => "ICRF", + _ => ref_frame.trim(), + } + ) + .map_err(err_hdlr)?; + + writeln!(writer, "CENTER_NAME = {center}",).map_err(err_hdlr)?; + + writeln!(writer, "TIME_SYSTEM = {}", first_orbit.epoch.time_scale).map_err(err_hdlr)?; + + writeln!( + writer, + "START_TIME = {}", + Formatter::new(states[0].epoch(), iso8601_no_ts) + ) + .map_err(err_hdlr)?; + writeln!( + writer, + "USEABLE_START_TIME = {}", + Formatter::new(states[0].epoch(), iso8601_no_ts) + ) + .map_err(err_hdlr)?; + writeln!( + writer, + "USEABLE_STOP_TIME = {}", + Formatter::new(states[states.len() - 1].epoch(), iso8601_no_ts) + ) + .map_err(err_hdlr)?; + writeln!( + writer, + "STOP_TIME = {}", + Formatter::new(states[states.len() - 1].epoch(), iso8601_no_ts) + ) + .map_err(err_hdlr)?; + + writeln!(writer, "META_STOP\n").map_err(err_hdlr)?; + + writeln!( + writer, + "COMMENT Generated by {} provided in AGPLv3 license -- https://nyxspace.com/\n", + prj_name_ver() + ) + .map_err(err_hdlr)?; + + for sc_state in &states { + let state = sc_state.orbit; + writeln!( + writer, + "{} {:E} {:E} {:E} {:E} {:E} {:E}", + Formatter::new(state.epoch, iso8601_no_ts), + state.radius_km.x, + state.radius_km.y, + state.radius_km.z, + state.velocity_km_s.x, + state.velocity_km_s.y, + state.velocity_km_s.z + ) + .map_err(err_hdlr)?; + } + + #[allow(clippy::writeln_empty_string)] + writeln!(writer, "").map_err(err_hdlr)?; + + // Return the path this was written to + let tock_time = Epoch::now().unwrap() - tick; + info!( + "Trajectory written to {} in {tock_time}", + path_buf.display() + ); + Ok(path_buf) + } +} + +#[cfg(test)] +mod ut_ccsds_oem { + + use crate::md::prelude::{OrbitalDynamics, Propagator, SpacecraftDynamics}; + use crate::time::{Epoch, TimeUnits}; + use crate::Spacecraft; + use crate::{io::ExportCfg, md::prelude::Traj, Orbit}; + use anise::almanac::Almanac; + use anise::constants::frames::MOON_J2000; + use pretty_env_logger; + use std::env; + use std::str::FromStr; + use std::sync::Arc; + use std::{collections::HashMap, path::PathBuf}; + + #[test] + fn test_load_oem_leo() { + // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "data", + "tests", + "ccsds", + "oem", + "LEO_10s.oem", + ] + .iter() + .collect(); + + let _ = pretty_env_logger::try_init(); + + let traj: Traj = Traj::from_oem_file(path, None).unwrap(); + + // This trajectory has two duplicate epochs, which should be removed by the call to finalize() + assert_eq!(traj.states.len(), 361); + assert_eq!(traj.name.unwrap(), "TEST_OBJ".to_string()); + } + + #[test] + fn test_load_oem_meo() { + // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "data", + "tests", + "ccsds", + "oem", + "MEO_60s.oem", + ] + .iter() + .collect(); + + let _ = pretty_env_logger::try_init(); + + let traj: Traj = Traj::from_oem_file(path, None).unwrap(); + + assert_eq!(traj.states.len(), 61); + assert_eq!(traj.name.unwrap(), "TEST_OBJ".to_string()); + } + + #[test] + fn test_load_oem_geo() { + use pretty_env_logger; + use std::env; + + // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/ + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "data", + "tests", + "ccsds", + "oem", + "GEO_20s.oem", + ] + .iter() + .collect(); + + let _ = pretty_env_logger::try_init(); + + let traj: Traj = Traj::from_oem_file(path, None).unwrap(); + + assert_eq!(traj.states.len(), 181); + assert_eq!(traj.name.as_ref().unwrap(), &"TEST_OBJ".to_string()); + + // Reexport this to CCSDS. + let cfg = ExportCfg::builder() + .timestamp(true) + .metadata(HashMap::from([ + ("originator".to_string(), "Test suite".to_string()), + ("object_name".to_string(), "TEST_OBJ".to_string()), + ])) + .build(); + + let path: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "output_data", + "GEO_20s_rebuilt.oem", + ] + .iter() + .collect(); + + let out_path = traj.to_oem_file(path.clone(), cfg).unwrap(); + // And reload, make sure we have the same data. + let traj_reloaded: Traj = Traj::from_oem_file(out_path, None).unwrap(); + + assert_eq!(traj_reloaded, traj); + + // Now export after trimming one state on either end + let cfg = ExportCfg::builder() + .timestamp(true) + .metadata(HashMap::from([ + ("originator".to_string(), "Test suite".to_string()), + ("object_name".to_string(), "TEST_OBJ".to_string()), + ])) + .step(20.seconds()) + .start_epoch(traj.first().orbit.epoch + 1.seconds()) + .end_epoch(traj.last().orbit.epoch - 1.seconds()) + .build(); + let out_path = traj.to_oem_file(path, cfg).unwrap(); + // And reload, make sure we have the same data. + let traj_reloaded: Traj = Traj::from_oem_file(out_path, None).unwrap(); + + // Note that the number of states has changed because we interpolated with a step similar to the original one but + // we started with a different time. + assert_eq!(traj_reloaded.states.len(), traj.states.len() - 1); + assert_eq!( + traj_reloaded.first().orbit.epoch, + traj.first().orbit.epoch + 1.seconds() + ); + // Note: because we used a fixed step, the last epoch is actually an offset of step size - end offset + // from the original trajectory + assert_eq!( + traj_reloaded.last().orbit.epoch, + traj.last().orbit.epoch - 19.seconds() + ); + } + + #[test] + fn test_moon_frame_long_prop() { + use std::path::PathBuf; + + let manifest_dir = + PathBuf::from(std::env::var("CARGO_MANIFEST_DIR").unwrap_or(".".to_string())); + + let almanac = Almanac::new( + &manifest_dir + .clone() + .join("data/pck08.pca") + .to_string_lossy(), + ) + .unwrap() + .load(&manifest_dir.join("data/de440s.bsp").to_string_lossy()) + .unwrap(); + + let epoch = Epoch::from_str("2022-06-13T12:00:00").unwrap(); + let orbit = Orbit::try_keplerian_altitude( + 350.0, + 0.02, + 30.0, + 45.0, + 85.0, + 0.0, + epoch, + almanac.frame_from_uid(MOON_J2000).unwrap(), + ) + .unwrap(); + + let mut traj = + Propagator::default_dp78(SpacecraftDynamics::new(OrbitalDynamics::two_body())) + .with(orbit.into(), Arc::new(almanac)) + .for_duration_with_traj(45.days()) + .unwrap() + .1; + // Set the name of this object + traj.name = Some("TEST_MOON_OBJ".to_string()); + + // Export CCSDS OEM file + let path: PathBuf = [env!("CARGO_MANIFEST_DIR"), "output_data", "moon_45days.oem"] + .iter() + .collect(); + + let out_path = traj.to_oem_file(path, ExportCfg::default()).unwrap(); + + // And reload + let traj_reloaded: Traj = Traj::from_oem_file(out_path, None).unwrap(); + + assert_eq!(traj, traj_reloaded); + } } diff --git a/src/md/trajectory/traj.rs b/src/md/trajectory/traj.rs index 54427823..e841fe87 100644 --- a/src/md/trajectory/traj.rs +++ b/src/md/trajectory/traj.rs @@ -31,6 +31,7 @@ use anise::almanac::Almanac; use arrow::array::{Array, Float64Builder, StringBuilder}; use arrow::datatypes::{DataType, Field, Schema}; use arrow::record_batch::RecordBatch; +use hifitime::TimeScale; use parquet::arrow::ArrowWriter; use snafu::ResultExt; use std::collections::HashMap; @@ -182,11 +183,7 @@ where let path_buf = cfg.actual_path(path); // Build the schema - let mut hdrs = vec![ - Field::new("Epoch:Gregorian UTC", DataType::Utf8, false), - Field::new("Epoch:Gregorian TAI", DataType::Utf8, false), - Field::new("Epoch:TAI (s)", DataType::Float64, false), - ]; + let mut hdrs = vec![Field::new("Epoch (UTC)", DataType::Utf8, false)]; let frame = self.states[0].frame(); let more_meta = Some(vec![( @@ -205,13 +202,7 @@ where }; // Check that we can retrieve this information - fields.retain(|param| match self.first().value(*param) { - Ok(_) => true, - Err(_) => { - warn!("Removed unavailable field `{param}` from trajectory export",); - false - } - }); + fields.retain(|param| self.first().value(*param).is_ok()); for field in &fields { hdrs.push(field.to_field(more_meta.clone())); @@ -243,16 +234,10 @@ where // Epochs let mut utc_epoch = StringBuilder::new(); - let mut tai_epoch = StringBuilder::new(); - let mut tai_s = Float64Builder::new(); for s in &states { - utc_epoch.append_value(format!("{}", s.epoch())); - tai_epoch.append_value(format!("{:x}", s.epoch())); - tai_s.append_value(s.epoch().to_tai_seconds()); + utc_epoch.append_value(&s.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); } record.push(Arc::new(utc_epoch.finish())); - record.push(Arc::new(tai_epoch.finish())); - record.push(Arc::new(tai_s.finish())); // Add all of the fields for field in fields { @@ -281,7 +266,6 @@ where // Add all of the evaluated events if let Some(events) = events { - // warn!("Adding events was removed when switching to ANISE"); info!("Evaluating {} event(s)", events.len()); for event in events { let mut data = Float64Builder::new(); @@ -378,11 +362,7 @@ where let path_buf = cfg.actual_path(path); // Build the schema - let mut hdrs = vec![ - Field::new("Epoch:Gregorian UTC", DataType::Utf8, false), - Field::new("Epoch:Gregorian TAI", DataType::Utf8, false), - Field::new("Epoch:TAI (s)", DataType::Float64, false), - ]; + let mut hdrs = vec![Field::new("Epoch (UTC)", DataType::Utf8, false)]; // Add the RIC headers for coord in ["x", "y", "z"] { @@ -478,16 +458,10 @@ where // Epochs (both match for self and others) let mut utc_epoch = StringBuilder::new(); - let mut tai_epoch = StringBuilder::new(); - let mut tai_s = Float64Builder::new(); for s in &self_states { - utc_epoch.append_value(format!("{}", s.epoch())); - tai_epoch.append_value(format!("{:x}", s.epoch())); - tai_s.append_value(s.epoch().to_tai_seconds()); + utc_epoch.append_value(&s.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); } record.push(Arc::new(utc_epoch.finish())); - record.push(Arc::new(tai_epoch.finish())); - record.push(Arc::new(tai_s.finish())); // Add the RIC data for coord_no in 0..6 { diff --git a/src/od/estimate/kfestimate.rs b/src/od/estimate/kfestimate.rs index 37b07262..dae8a127 100644 --- a/src/od/estimate/kfestimate.rs +++ b/src/od/estimate/kfestimate.rs @@ -17,11 +17,14 @@ */ use super::{Estimate, State}; +use crate::cosmic::AstroError; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, Matrix, OMatrix, OVector}; -use crate::mc::GaussianGenerator; +use crate::mc::{GaussianGenerator, MultivariateNormal}; +use crate::md::prelude::OrbitDual; use crate::md::StateParameter; -use crate::Spacecraft; +use crate::{NyxError, Spacecraft}; +use na::SMatrix; use nalgebra::Const; use rand::SeedableRng; use rand_distr::Distribution; @@ -144,6 +147,90 @@ impl KfEstimate { stm: OMatrix::, Const<9>>::identity(), } } + + /// Builds a multivariate random variable from this estimate's nominal state and covariance, zero mean. + pub fn to_random_variable(&self) -> Result, NyxError> { + MultivariateNormal::zero_mean( + self.nominal_state, + vec![ + StateParameter::X, + StateParameter::Y, + StateParameter::Z, + StateParameter::VX, + StateParameter::VY, + StateParameter::VZ, + StateParameter::Cr, + StateParameter::Cd, + StateParameter::FuelMass, + ], + self.covar, + ) + } + + /// Returns the 1-sigma uncertainty for a given parameter, in that parameter's unit + /// + /// This method uses the [OrbitDual] structure to compute the estimate in the hyperdual space + /// and rotate the nominal covariance into that space. + pub fn sigma_for(&self, param: StateParameter) -> Result { + // Build the rotation matrix using Orbit Dual. + let mut rotmat = SMatrix::::zeros(); + let orbit_dual = OrbitDual::from(self.nominal_state.orbit); + + let xf_partial = orbit_dual.partial_for(param)?; + for (cno, val) in [ + xf_partial.wtr_x(), + xf_partial.wtr_y(), + xf_partial.wtr_z(), + xf_partial.wtr_vx(), + xf_partial.wtr_vy(), + xf_partial.wtr_vz(), + ] + .iter() + .copied() + .enumerate() + { + rotmat[(0, cno)] = val; + } + + Ok((rotmat * self.covar.fixed_view::<6, 6>(0, 0) * rotmat.transpose())[(0, 0)].sqrt()) + } + + /// Returns the 6x6 covariance (i.e. square of the sigma/uncertainty) of the SMA, ECC, INC, RAAN, AOP, and True Anomaly. + pub fn keplerian_covar(&self) -> SMatrix { + // Build the rotation matrix using Orbit Dual. + let mut rotmat = SMatrix::::zeros(); + let orbit_dual = OrbitDual::from(self.nominal_state.orbit); + for (pno, param) in [ + StateParameter::SMA, + StateParameter::Eccentricity, + StateParameter::Inclination, + StateParameter::RAAN, + StateParameter::AoP, + StateParameter::TrueAnomaly, + ] + .iter() + .copied() + .enumerate() + { + let xf_partial = orbit_dual.partial_for(param).unwrap(); + for (cno, val) in [ + xf_partial.wtr_x(), + xf_partial.wtr_y(), + xf_partial.wtr_z(), + xf_partial.wtr_vx(), + xf_partial.wtr_vy(), + xf_partial.wtr_vz(), + ] + .iter() + .copied() + .enumerate() + { + rotmat[(pno, cno)] = val; + } + } + + rotmat * self.covar.fixed_view::<6, 6>(0, 0) * rotmat.transpose() + } } impl Estimate for KfEstimate diff --git a/src/od/estimate/mod.rs b/src/od/estimate/mod.rs index ed0559f7..d0dfabae 100644 --- a/src/od/estimate/mod.rs +++ b/src/od/estimate/mod.rs @@ -18,10 +18,10 @@ use super::State; use crate::cosmic::Orbit; -use crate::hifitime::Epoch; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, OMatrix, OVector}; use crate::Spacecraft; +use hifitime::Epoch; use std::cmp::PartialEq; use std::fmt; @@ -29,6 +29,8 @@ pub mod residual; pub use residual::Residual; pub mod kfestimate; pub use kfestimate::KfEstimate; +mod sc_uncertainty; +pub use sc_uncertainty::SpacecraftUncertainty; /// Stores an Estimate, as the result of a `time_update` or `measurement_update`. pub trait Estimate diff --git a/src/od/estimate/residual.rs b/src/od/estimate/residual.rs index ca5ff34a..a73dfc9e 100644 --- a/src/od/estimate/residual.rs +++ b/src/od/estimate/residual.rs @@ -16,9 +16,9 @@ along with this program. If not, see . */ -use crate::hifitime::Epoch; use crate::linalg::allocator::Allocator; use crate::linalg::{DefaultAllocator, DimName, OVector}; +use hifitime::Epoch; use std::fmt; /// Stores an Estimate, as the result of a `time_update` or `measurement_update`. diff --git a/src/od/estimate/sc_uncertainty.rs b/src/od/estimate/sc_uncertainty.rs new file mode 100644 index 00000000..85c5cb40 --- /dev/null +++ b/src/od/estimate/sc_uncertainty.rs @@ -0,0 +1,297 @@ +/* + 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 core::fmt; + +use anise::errors::MathError; +use anise::{astro::PhysicsResult, errors::PhysicsError}; +use na::{SMatrix, SVector}; +use typed_builder::TypedBuilder; + +use crate::{dynamics::guidance::LocalFrame, Spacecraft}; + +use super::KfEstimate; + +#[derive(Clone, Copy, Debug, TypedBuilder)] +/// Builds a spacecraft uncertainty in different local frames, dispersing any of the parameters of the spacecraft state. +/// +/// # Usage +/// Use the `TypeBuilder` trait, e.g `SpacecraftUncertainty::builder().nominal(spacecraft).frame(LocalFrame::RIC).x_km(0.5).y_km(0.5).z_km(0.5).build()` +/// to build an uncertainty on position in the RIC frame of 500 meters on R, I, and C, and zero on all other parameters (velocity components, Cr, Cd, mass). +pub struct SpacecraftUncertainty { + pub nominal: Spacecraft, + #[builder(default, setter(strip_option))] + pub frame: Option, + #[builder(default = 0.5)] + pub x_km: f64, + #[builder(default = 0.5)] + pub y_km: f64, + #[builder(default = 0.5)] + pub z_km: f64, + #[builder(default = 50e-5)] + pub vx_km_s: f64, + #[builder(default = 50e-5)] + pub vy_km_s: f64, + #[builder(default = 50e-5)] + pub vz_km_s: f64, + #[builder(default)] + pub cr: f64, + #[builder(default)] + pub cd: f64, + #[builder(default)] + pub mass_kg: f64, +} + +impl SpacecraftUncertainty { + /// Builds a Kalman filter estimate for a spacecraft state, ready to ingest into an OD Process. + /// + /// Note: this function will rotate from the provided local frame into the inertial frame with the same central body. + pub fn to_estimate(&self) -> PhysicsResult> { + if self.x_km < 0.0 + || self.y_km < 0.0 + || self.z_km < 0.0 + || self.vx_km_s < 0.0 + || self.vy_km_s < 0.0 + || self.vz_km_s < 0.0 + || self.cd < 0.0 + || self.cr < 0.0 + || self.mass_kg < 0.0 + { + return Err(PhysicsError::AppliedMath { + source: MathError::DomainError { + value: -0.0, + msg: "uncertainties must be positive ", + }, + }); + } + + // Build the orbit state vector as provided. + let orbit_vec = SVector::::new( + self.x_km, + self.y_km, + self.z_km, + self.vx_km_s, + self.vy_km_s, + self.vz_km_s, + ); + + // Rotate into the correct frame. + let dcm_local2inertial = match self.frame { + None => LocalFrame::Inertial.dcm_to_inertial(self.nominal.orbit)?, + Some(frame) => frame.dcm_to_inertial(self.nominal.orbit)?, + }; + + let mut init_covar = + SMatrix::::from_diagonal(&SVector::::from_iterator([ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + self.cr.powi(2), + self.cd.powi(2), + self.mass_kg.powi(2), + ])); + + let other_cov = SMatrix::::from_diagonal(&SVector::::from_iterator([ + orbit_vec[0], + orbit_vec[1], + orbit_vec[2], + orbit_vec[3], + orbit_vec[4], + orbit_vec[5], + ])); + + let rot_covar = + dcm_local2inertial.state_dcm().transpose() * other_cov * dcm_local2inertial.state_dcm(); + + for i in 0..6 { + for j in 0..6 { + init_covar[(i, j)] = rot_covar[(i, j)].powi(2); + } + } + + Ok(KfEstimate::from_covar(self.nominal, init_covar)) + } +} + +impl fmt::Display for SpacecraftUncertainty { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + let frame = match self.frame { + None => format!("{}", self.nominal.orbit.frame), + Some(frame) => match frame { + LocalFrame::Inertial => format!("{}", self.nominal.orbit.frame), + _ => format!("{frame:?}"), + }, + }; + writeln!(f, "{}", self.nominal)?; + writeln!( + f, + "{frame} Σ_x = {} km Σ_y = {} km Σ_z = {} km", + self.x_km, self.y_km, self.z_km + )?; + writeln!( + f, + "{frame} Σ_vx = {} km/s Σ_vy = {} km/s Σ_vz = {} km/s", + self.vx_km_s, self.vy_km_s, self.vz_km_s + )?; + writeln!( + f, + "Σ_cr = {} Σ_cd = {} Σ_mass = {} kg", + self.cr, self.cd, self.mass_kg + ) + } +} + +#[cfg(test)] +mod ut_sc_uncertainty { + + use super::{Spacecraft, SpacecraftUncertainty}; + use crate::dynamics::guidance::LocalFrame; + use crate::md::StateParameter; + use crate::GMAT_EARTH_GM; + use anise::constants::frames::EME2000; + use anise::prelude::{Epoch, Orbit}; + + use rstest::*; + #[fixture] + fn spacecraft() -> Spacecraft { + let eme2k = EME2000.with_mu_km3_s2(GMAT_EARTH_GM); + + Spacecraft::builder() + .orbit(Orbit::keplerian( + 7000.0, + 0.01, + 28.5, + 15.0, + 55.0, + 123.0, + Epoch::from_gregorian_utc_hms(2024, 2, 29, 1, 2, 3), + eme2k, + )) + .build() + } + + #[rstest] + fn test_inertial_frame(spacecraft: Spacecraft) { + let uncertainty = SpacecraftUncertainty::builder() + .nominal(spacecraft) + .x_km(0.5) + .y_km(0.5) + .z_km(0.5) + .vx_km_s(0.5e-3) + .vy_km_s(0.5e-3) + .vz_km_s(0.5e-3) + .build(); + + assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON); + assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON); + assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON); + + println!("{uncertainty}"); + + let estimate = uncertainty.to_estimate().unwrap(); + + // Ensure that the covariance is a diagonal. + for i in 0..6 { + for j in 0..6 { + if i == j { + if i < 3 { + assert!(estimate.covar[(i, j)] - 0.5_f64.powi(2) < f64::EPSILON); + } else { + assert!(estimate.covar[(i, j)] - 0.5e-3_f64.powi(2) < f64::EPSILON); + } + } else { + assert_eq!(estimate.covar[(i, j)], 0.0); + } + } + } + } + + #[rstest] + fn test_ric_frame(spacecraft: Spacecraft) { + let uncertainty = SpacecraftUncertainty::builder() + .nominal(spacecraft) + .frame(LocalFrame::RIC) + .x_km(0.5) + .y_km(0.5) + .z_km(0.5) + .vx_km_s(0.5e-3) + .vy_km_s(0.5e-3) + .vz_km_s(0.5e-3) + .build(); + + assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON); + assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON); + assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON); + assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON); + + println!("{uncertainty}"); + + let estimate = uncertainty.to_estimate().unwrap(); + + println!("{estimate}"); + + let sma_sigma_km = estimate.sigma_for(StateParameter::SMA).unwrap(); + assert!((sma_sigma_km - 1.352808889337306).abs() < f64::EPSILON); + + let covar_keplerian = estimate.keplerian_covar(); + for i in 1..=3 { + let val = covar_keplerian[(i, i)].sqrt(); + assert!(val.abs() < 1e-1); + } + for i in 4..6 { + let val = covar_keplerian[(i, i)].sqrt(); + assert!(val.abs() < 0.8); + } + + // Rotate back into the RIC frame. + let dcm_ric2inertial = estimate + .nominal_state + .orbit + .dcm_from_ric_to_inertial() + .unwrap() + .state_dcm(); + + // Build the matrix view of the orbit part of the covariance. + let orbit_cov = estimate.covar.fixed_view::<6, 6>(0, 0); + + // Rotate back into the RIC frame + let ric_covar = &dcm_ric2inertial * orbit_cov * &dcm_ric2inertial.transpose(); + + println!("{:.9}", ric_covar); + for i in 0..6 { + for j in 0..6 { + if i == j { + // We don't abs the data in the sqrt to ensure that the diag is positive. + if i < 3 { + assert!((ric_covar[(i, j)].sqrt() - 0.5).abs() < 1e-9); + } else { + assert!((ric_covar[(i, j)].sqrt() - 0.5e-3).abs() < 1e-9); + } + } + } + } + } +} diff --git a/src/od/mod.rs b/src/od/mod.rs index dc53734d..0cc6a229 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -59,6 +59,17 @@ pub use simulator::TrackingDeviceSim; /// Provides all state noise compensation functionality pub mod snc; +/// A helper type for spacecraft orbit determination. +pub type SpacecraftODProcess<'a> = self::process::ODProcess< + 'a, + crate::md::prelude::SpacecraftDynamics, + crate::propagators::RSSCartesianStep, + msr::RangeDoppler, + nalgebra::Const<3>, + crate::Spacecraft, + filter::kalman::KF, nalgebra::Const<2>>, +>; + #[allow(unused_imports)] pub mod prelude { pub use super::estimate::*; diff --git a/src/od/msr/arc.rs b/src/od/msr/arc.rs index a92269bd..3d827e85 100644 --- a/src/od/msr/arc.rs +++ b/src/od/msr/arc.rs @@ -35,6 +35,7 @@ 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. @@ -97,9 +98,7 @@ where // Build the schema let mut hdrs = vec![ - Field::new("Epoch:Gregorian UTC", DataType::Utf8, false), - Field::new("Epoch:Gregorian TAI", DataType::Utf8, false), - Field::new("Epoch:TAI (s)", DataType::Float64, false), + Field::new("Epoch (UTC)", DataType::Utf8, false), Field::new("Tracking device", DataType::Utf8, false), ]; @@ -137,16 +136,10 @@ where // Epochs let mut utc_epoch = StringBuilder::new(); - let mut tai_epoch = StringBuilder::new(); - let mut tai_s = Float64Builder::new(); for m in &measurements { - utc_epoch.append_value(format!("{}", m.1.epoch())); - tai_epoch.append_value(format!("{:x}", m.1.epoch())); - tai_s.append_value(m.1.epoch().to_tai_seconds()); + utc_epoch.append_value(&m.1.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); } record.push(Arc::new(utc_epoch.finish())); - record.push(Arc::new(tai_epoch.finish())); - record.push(Arc::new(tai_s.finish())); // Device names let mut device_names = StringBuilder::new(); diff --git a/src/od/process/export.rs b/src/od/process/export.rs index fa8fc2a3..e141ba35 100644 --- a/src/od/process/export.rs +++ b/src/od/process/export.rs @@ -23,12 +23,15 @@ use crate::linalg::{DefaultAllocator, DimName}; use crate::md::trajectory::Interpolatable; use crate::md::StateParameter; use crate::od::estimate::*; -use crate::od::*; use crate::propagators::error_ctrl::ErrorCtrl; use crate::State; +use crate::{od::*, Spacecraft}; use arrow::array::{Array, Float64Builder, StringBuilder}; use arrow::datatypes::{DataType, Field, Schema}; use arrow::record_batch::RecordBatch; +use filter::kalman::KF; +use hifitime::TimeScale; +use na::Const; use parquet::arrow::ArrowWriter; use snafu::prelude::*; use std::collections::HashMap; @@ -44,22 +47,23 @@ impl< E: ErrorCtrl, Msr: Measurement, A: DimName, - S: EstimateFrom + Interpolatable, - K: Filter, - > ODProcess<'a, D, E, Msr, A, S, K> + // K: Filter, + > ODProcess<'a, D, E, Msr, A, Spacecraft, KF> 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, Msr::MeasurementSize> + + Allocator::Size> + + Allocator::Size, ::Size> + Allocator + Allocator::Size> - + Allocator::Size> + + Allocator::Size> + Allocator::Size, Msr::MeasurementSize> - + Allocator::Size, Msr::MeasurementSize> + + Allocator::Size, Msr::MeasurementSize> + Allocator::Size, ::Size> + Allocator::Size, ::Size> + Allocator::VecLength> @@ -67,11 +71,12 @@ where + Allocator + Allocator::Size, A> + Allocator::Size> - + Allocator::Size> - + Allocator::VecLength> - + Allocator::Size, ::Size> - + Allocator::Size, A> - + Allocator::Size>, + + Allocator::Size> + + Allocator::VecLength> + + Allocator::Size, ::Size> + + Allocator::Size, A> + + Allocator::Size>, + Spacecraft: EstimateFrom, { /// Store the estimates and residuals in a parquet file pub fn to_parquet>(&self, path: P, cfg: ExportCfg) -> Result { @@ -102,11 +107,7 @@ where let path_buf = cfg.actual_path(path); // Build the schema - let mut hdrs = vec![ - Field::new("Epoch:Gregorian UTC", DataType::Utf8, false), - Field::new("Epoch:Gregorian TAI", DataType::Utf8, false), - Field::new("Epoch:TAI (s)", DataType::Float64, false), - ]; + let mut hdrs = vec![Field::new("Epoch (UTC)", DataType::Utf8, false)]; let frame = self.estimates[0].state().frame(); @@ -124,49 +125,106 @@ where let mut fields = match cfg.fields { Some(fields) => fields, - None => S::export_params(), + None => Spacecraft::export_params(), }; // Check that we can retrieve this information fields.retain(|param| match self.estimates[0].state().value(*param) { Ok(_) => param != &StateParameter::GuidanceMode, - Err(_) => { - warn!("Removed unavailable field `{param}` from orbit determination export",); - false - } + Err(_) => false, }); for field in &fields { hdrs.push(field.to_field(more_meta.clone())); } + let mut sigma_fields = fields.clone(); + // Check that we can retrieve this information + sigma_fields.retain(|param| { + !matches!( + param, + &StateParameter::X + | &StateParameter::Y + | &StateParameter::Z + | &StateParameter::VX + | &StateParameter::VY + | &StateParameter::VZ + ) && self.estimates[0].sigma_for(*param).is_ok() + }); + + for field in &sigma_fields { + hdrs.push(field.to_cov_field(more_meta.clone())); + } + let state_items = ["X", "Y", "Z", "Vx", "Vy", "Vz", "Cr", "Cd", "Mass"]; - let est_size = ::Size::dim(); - assert!( - est_size <= state_items.len(), - "state of size {est_size} is not yet supported" - ); + let state_units = [ + "km", "km", "km", "km/s", "km/s", "km/s", "unitless", "unitless", "kg", + ]; + let mut cov_units = vec![]; + + for i in 0..state_items.len() { + for j in i..state_items.len() { + let cov_unit = if i < 3 { + if j < 3 { + "km^2" + } else if (3..6).contains(&j) { + "km^2/s" + } else if j == 8 { + "km*kg" + } else { + "km" + } + } else if (3..6).contains(&i) { + if (3..6).contains(&j) { + "km^2/s^2" + } else if j == 8 { + "km/s*kg" + } else { + "km/s" + } + } else if i == 8 || j == 8 { + "kg^2" + } else { + "unitless" + }; + + cov_units.push(cov_unit); + } + } - let mut cov_hdrs = Vec::new(); + let est_size = ::Size::dim(); + let mut idx = 0; for i in 0..state_items.len() { for j in i..state_items.len() { - cov_hdrs.push(format!("Covariance {}{}", state_items[i], state_items[j])); + hdrs.push(Field::new( + format!( + "Covariance {}*{} ({frame:x}) ({})", + state_items[i], state_items[j], cov_units[idx] + ), + DataType::Float64, + false, + )); + idx += 1; } } - // Add the covariance in the integration frame - for hdr in &cov_hdrs { + // Add the uncertainty in the integration frame + for (i, coord) in state_items.iter().enumerate() { hdrs.push(Field::new( - format!("{hdr} ({frame:x})"), + format!("Sigma {coord} ({frame:x}) ({})", state_units[i]), DataType::Float64, false, )); } - // Add the covariance in the RIC frame - for hdr in &cov_hdrs { - hdrs.push(Field::new(format!("{hdr} (RIC)"), DataType::Float64, false)); + // Add the position and velocity uncertainty in the RIC frame + for (i, coord) in state_items.iter().enumerate().take(6) { + hdrs.push(Field::new( + format!("Sigma {coord} (RIC) ({})", state_units[i]), + DataType::Float64, + false, + )); } // Add the fields of the residuals @@ -211,7 +269,7 @@ where for (estimate, residual) in self.estimates.iter().zip(self.residuals.iter()) { if estimate.epoch() >= start && estimate.epoch() <= end { - estimates.push(estimate.clone()); + estimates.push(*estimate); residuals.push(residual.clone()); } } @@ -225,16 +283,10 @@ where // Epochs let mut utc_epoch = StringBuilder::new(); - let mut tai_epoch = StringBuilder::new(); - let mut tai_s = Float64Builder::new(); for s in &estimates { - utc_epoch.append_value(format!("{}", s.epoch())); - tai_epoch.append_value(format!("{:x}", s.epoch())); - tai_s.append_value(s.epoch().to_tai_seconds()); + utc_epoch.append_value(&s.epoch().to_time_scale(TimeScale::UTC).to_isoformat()); } record.push(Arc::new(utc_epoch.finish())); - record.push(Arc::new(tai_epoch.finish())); - record.push(Arc::new(tai_s.finish())); // Add all of the fields for field in fields { @@ -244,6 +296,16 @@ where } record.push(Arc::new(data.finish())); } + + // Add all of the 1-sigma uncertainties + for field in sigma_fields { + let mut data = Float64Builder::new(); + for s in &estimates { + data.append_value(s.sigma_for(field).unwrap()); + } + record.push(Arc::new(data.finish())); + } + // Add the 1-sigma covariance in the integration frame for i in 0..est_size { for j in i..est_size { @@ -254,37 +316,43 @@ where record.push(Arc::new(data.finish())); } } - // Add the 1-sigma covariance in the RIC frame + + // Add the sigma/uncertainty in the integration frame + for i in 0..est_size { + let mut data = Float64Builder::new(); + for s in &estimates { + data.append_value(s.covar()[(i, i)].sqrt()); + } + record.push(Arc::new(data.finish())); + } + + // Add the sigma/uncertainty covariance in the RIC frame let mut ric_covariances = Vec::new(); for s in &estimates { - let dcm6x6 = s + let dcm_ric2inertial = s .state() .orbit() .dcm_from_ric_to_inertial() .unwrap() .state_dcm(); - // Create a full DCM and only rotate the orbit part of it. - let mut dcm = OMatrix::::identity(); - for i in 0..6 { - for j in i..6 { - dcm[(i, j)] = dcm6x6[(i, j)]; - } - } - let ric_covar = &dcm * s.covar() * &dcm.transpose(); + // Build the matrix view of the orbit part of the covariance. + let cov = s.covar(); + let orbit_cov = cov.fixed_view::<6, 6>(0, 0); + + // Rotate back into the RIC frame + let ric_covar = dcm_ric2inertial * orbit_cov * dcm_ric2inertial.transpose(); ric_covariances.push(ric_covar); } // Now store the RIC covariance data. - for i in 0..est_size { - for j in i..est_size { - let mut data = Float64Builder::new(); - for cov in ric_covariances.iter().take(estimates.len()) { - data.append_value(cov[(i, j)]); - } - record.push(Arc::new(data.finish())); + for i in 0..6 { + let mut data = Float64Builder::new(); + for cov in ric_covariances.iter().take(estimates.len()) { + data.append_value(cov[(i, i)].sqrt()); } + record.push(Arc::new(data.finish())); } // Finally, add the residuals. diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 95948228..68c4874e 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -679,7 +679,7 @@ where /// Continuously predicts the trajectory until the provided end epoch, with covariance mapping at each step. In other words, this performs a time update. pub fn predict_until(&mut self, step: Duration, end_epoch: Epoch) -> Result<(), ODError> { let prop_time = end_epoch - self.kf.previous_estimate().epoch(); - info!("Propagating for {prop_time} and mapping covariance",); + info!("Mapping covariance for {prop_time} with {step} step"); loop { let mut epoch = self.prop.state.epoch(); diff --git a/src/propagators/instance.rs b/src/propagators/instance.rs index 90bd3c42..4f235ff0 100644 --- a/src/propagators/instance.rs +++ b/src/propagators/instance.rs @@ -85,7 +85,7 @@ where #[cfg(not(target_arch = "wasm32"))] let tick = Instant::now(); - let log_progress = duration.abs() >= 2 * Unit::Minute; + let log_progress = duration.abs() >= 7 * Unit::Day; if log_progress { // Prevent the print spam for orbit determination cases diff --git a/tests/benchmarks.md b/tests/benchmarks.md deleted file mode 100644 index 6640a88a..00000000 --- a/tests/benchmarks.md +++ /dev/null @@ -1,6 +0,0 @@ - -| name (release) | Cosm | anise metaalmaanc | anise bytes | -| -- | -- | -- | -- | -| val_earth_sph_harmonics_70x70_partials | 0.60 | 7.44 | 7.33 | -| tgt_raan_from_peri | 0.02 | 0.64 | 0.02 | -| test_monte_carlo_epoch | 0.13 | 3.26 | 2.68 | \ No newline at end of file diff --git a/tests/cosmic/orbit_dual.rs b/tests/cosmic/orbit_dual.rs index 5dfdd8de..ce12bb1d 100644 --- a/tests/cosmic/orbit_dual.rs +++ b/tests/cosmic/orbit_dual.rs @@ -33,12 +33,15 @@ fn orbit_dual_test(almanac: Almanac) { let cart_dual = OrbitDual::from(cart); println!("{}", eme2k.mu_km3_s2().unwrap()); - println!("|v| = {}", cart_dual.vmag()); - println!("|v|^2 = {}", cart_dual.vmag().dual * cart_dual.vmag().dual); + println!("|v| = {}", cart_dual.vmag_km_s()); + println!( + "|v|^2 = {}", + cart_dual.vmag_km_s().dual * cart_dual.vmag_km_s().dual + ); println!("x = {}", cart_dual.x); println!("y = {}", cart_dual.y); println!("z = {}", cart_dual.z); - println!("\\xi = {}\n\n", cart_dual.energy().unwrap()); + println!("\\xi = {}\n\n", cart_dual.energy_km2_s2().unwrap()); // Now print the table let params = vec![ @@ -51,9 +54,9 @@ fn orbit_dual_test(almanac: Almanac) { StateParameter::Eccentricity, StateParameter::Energy, StateParameter::FlightPathAngle, - StateParameter::GeodeticHeight, - StateParameter::GeodeticLatitude, - StateParameter::GeodeticLongitude, + StateParameter::Height, + StateParameter::Latitude, + StateParameter::Longitude, StateParameter::Hmag, StateParameter::HX, StateParameter::HY, diff --git a/tests/monte_carlo/framework.rs b/tests/monte_carlo/framework.rs index 7728df8a..bc7d4a46 100644 --- a/tests/monte_carlo/framework.rs +++ b/tests/monte_carlo/framework.rs @@ -31,8 +31,9 @@ fn test_monte_carlo_epoch(almanac: Arc) { // Build the state generator using a Gaussian distribution (you may use any distribution from rand_distr) // 5% error on SMA and 5% on Eccentricity - let generator = GaussianGenerator::from_std_dev_prcts( - Spacecraft::from(state), + let nominal_state = Spacecraft::from(state); + let random_state = GaussianGenerator::from_std_dev_prcts( + nominal_state, &[ (StateParameter::SMA, 0.05), (StateParameter::Eccentricity, 0.05), @@ -52,8 +53,9 @@ fn test_monte_carlo_epoch(almanac: Arc) { // Setup the Monte Carlo let my_mc = MonteCarlo { - generator, - seed: 0, + random_state, + nominal_state, + seed: Some(0), scenario: "test_monte_carlo_epoch".to_string(), }; diff --git a/tests/orbit_determination/robust.rs b/tests/orbit_determination/robust.rs index 16820e7f..15bce069 100644 --- a/tests/orbit_determination/robust.rs +++ b/tests/orbit_determination/robust.rs @@ -141,8 +141,8 @@ 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 + // 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, opts); @@ -161,7 +161,7 @@ fn od_robust_test_ekf_realistic_one_way(almanac: Arc) { let mut odp = ODProcess::ekf(prop_est, kf, trig, None, almanac); - // Let's filter and iterate on the initial subset of the arc to refine the initial estimate + // 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 remaining = arc.filter_by_offset(3.hours()..); @@ -339,8 +339,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); @@ -360,7 +360,7 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { 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. + // 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 @@ -441,9 +441,7 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { // Check that the position and velocity estimates are present, along with the epochs assert!(df .columns([ - "Epoch:Gregorian UTC", - "Epoch:Gregorian TAI", - "Epoch:TAI (s)", + "Epoch (UTC)", "x (km)", "y (km)", "z (km)", @@ -456,54 +454,66 @@ fn od_robust_test_ekf_realistic_two_way(almanac: Arc) { // Check that the covariance in the integration frame is present assert!(df .columns([ - "Covariance XX (Earth J2000)", - "Covariance XY (Earth J2000)", - "Covariance XZ (Earth J2000)", - "Covariance XVx (Earth J2000)", - "Covariance XVy (Earth J2000)", - "Covariance XVz (Earth J2000)", - "Covariance YY (Earth J2000)", - "Covariance YZ (Earth J2000)", - "Covariance YVx (Earth J2000)", - "Covariance YVy (Earth J2000)", - "Covariance YVz (Earth J2000)", - "Covariance ZZ (Earth J2000)", - "Covariance ZVx (Earth J2000)", - "Covariance ZVy (Earth J2000)", - "Covariance ZVz (Earth J2000)", - "Covariance VxVx (Earth J2000)", - "Covariance VxVy (Earth J2000)", - "Covariance VxVz (Earth J2000)", - "Covariance VyVy (Earth J2000)", - "Covariance VyVz (Earth J2000)", - "Covariance VzVz (Earth J2000)", - ]) - .is_ok()); - - // Check that the covariance in the RIC frame is present - assert!(df - .columns([ - "Covariance XX (RIC)", - "Covariance XY (RIC)", - "Covariance XZ (RIC)", - "Covariance XVx (RIC)", - "Covariance XVy (RIC)", - "Covariance XVz (RIC)", - "Covariance YY (RIC)", - "Covariance YZ (RIC)", - "Covariance YVx (RIC)", - "Covariance YVy (RIC)", - "Covariance YVz (RIC)", - "Covariance ZZ (RIC)", - "Covariance ZVx (RIC)", - "Covariance ZVy (RIC)", - "Covariance ZVz (RIC)", - "Covariance VxVx (RIC)", - "Covariance VxVy (RIC)", - "Covariance VxVz (RIC)", - "Covariance VyVy (RIC)", - "Covariance VyVz (RIC)", - "Covariance VzVz (RIC)", + "Covariance X*X (Earth J2000) (km^2)", + "Covariance X*Y (Earth J2000) (km^2)", + "Covariance X*Z (Earth J2000) (km^2)", + "Covariance X*Vx (Earth J2000) (km^2/s)", + "Covariance X*Vy (Earth J2000) (km^2/s)", + "Covariance X*Vz (Earth J2000) (km^2/s)", + "Covariance X*Cr (Earth J2000) (km)", + "Covariance X*Cd (Earth J2000) (km)", + "Covariance X*Mass (Earth J2000) (km*kg)", + "Covariance Y*Y (Earth J2000) (km^2)", + "Covariance Y*Z (Earth J2000) (km^2)", + "Covariance Y*Vx (Earth J2000) (km^2/s)", + "Covariance Y*Vy (Earth J2000) (km^2/s)", + "Covariance Y*Vz (Earth J2000) (km^2/s)", + "Covariance Y*Cr (Earth J2000) (km)", + "Covariance Y*Cd (Earth J2000) (km)", + "Covariance Y*Mass (Earth J2000) (km*kg)", + "Covariance Z*Z (Earth J2000) (km^2)", + "Covariance Z*Vx (Earth J2000) (km^2/s)", + "Covariance Z*Vy (Earth J2000) (km^2/s)", + "Covariance Z*Vz (Earth J2000) (km^2/s)", + "Covariance Z*Cr (Earth J2000) (km)", + "Covariance Z*Cd (Earth J2000) (km)", + "Covariance Z*Mass (Earth J2000) (km*kg)", + "Covariance Vx*Vx (Earth J2000) (km^2/s^2)", + "Covariance Vx*Vy (Earth J2000) (km^2/s^2)", + "Covariance Vx*Vz (Earth J2000) (km^2/s^2)", + "Covariance Vx*Cr (Earth J2000) (km/s)", + "Covariance Vx*Cd (Earth J2000) (km/s)", + "Covariance Vx*Mass (Earth J2000) (km/s*kg)", + "Covariance Vy*Vy (Earth J2000) (km^2/s^2)", + "Covariance Vy*Vz (Earth J2000) (km^2/s^2)", + "Covariance Vy*Cr (Earth J2000) (km/s)", + "Covariance Vy*Cd (Earth J2000) (km/s)", + "Covariance Vy*Mass (Earth J2000) (km/s*kg)", + "Covariance Vz*Vz (Earth J2000) (km^2/s^2)", + "Covariance Vz*Cr (Earth J2000) (km/s)", + "Covariance Vz*Cd (Earth J2000) (km/s)", + "Covariance Vz*Mass (Earth J2000) (km/s*kg)", + "Covariance Cr*Cr (Earth J2000) (unitless)", + "Covariance Cr*Cd (Earth J2000) (unitless)", + "Covariance Cr*Mass (Earth J2000) (kg^2)", + "Covariance Cd*Cd (Earth J2000) (unitless)", + "Covariance Cd*Mass (Earth J2000) (kg^2)", + "Covariance Mass*Mass (Earth J2000) (kg^2)", + "Sigma X (Earth J2000) (km)", + "Sigma Y (Earth J2000) (km)", + "Sigma Z (Earth J2000) (km)", + "Sigma Vx (Earth J2000) (km/s)", + "Sigma Vy (Earth J2000) (km/s)", + "Sigma Vz (Earth J2000) (km/s)", + "Sigma Cr (Earth J2000) (unitless)", + "Sigma Cd (Earth J2000) (unitless)", + "Sigma Mass (Earth J2000) (kg)", + "Sigma X (RIC) (km)", + "Sigma Y (RIC) (km)", + "Sigma Z (RIC) (km)", + "Sigma Vx (RIC) (km/s)", + "Sigma Vy (RIC) (km/s)", + "Sigma Vz (RIC) (km/s)", ]) .is_ok()); diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 26b4446c..3e750480 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -942,14 +942,7 @@ fn od_tb_ckf_map_covar(almanac: Arc) { let measurement_noise = Matrix2::from_diagonal(&Vector2::new(1e-6, 1e-3)); let ckf = KF::no_snc(initial_estimate, measurement_noise); - let mut odp: ODProcess< - SpacecraftDynamics, - nyx::propagators::RSSCartesianStep, - RangeDoppler, - nalgebra::Const<3>, - Spacecraft, - KF, nalgebra::Const<2>>, - > = ODProcess::ckf(prop_est, ckf, None, almanac); + let mut odp: SpacecraftODProcess = ODProcess::ckf(prop_est, ckf, None, almanac); odp.predict_for(30.seconds(), duration).unwrap(); diff --git a/tests/propagation/stopcond.rs b/tests/propagation/stopcond.rs index f2f6e272..60955310 100644 --- a/tests/propagation/stopcond.rs +++ b/tests/propagation/stopcond.rs @@ -234,7 +234,7 @@ fn line_of_nodes(almanac: Arc) { let period = state.period().unwrap(); - let lon_event = Event::new(StateParameter::GeodeticLongitude, 0.0); + let lon_event = Event::new(StateParameter::Longitude, 0.0); let setup = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body())); let mut prop = setup.with(state.into(), almanac); @@ -262,7 +262,7 @@ fn latitude(almanac: Arc) { let period = state.period().unwrap(); - let lat_event = Event::new(StateParameter::GeodeticLatitude, 2.0); + let lat_event = Event::new(StateParameter::Latitude, 2.0); let setup = Propagator::default_dp78(SpacecraftDynamics::new(OrbitalDynamics::two_body())); let mut prop = setup.with(state.into(), almanac); diff --git a/tests/propagation/trajectory.rs b/tests/propagation/trajectory.rs index dc949cb3..2f99e6cb 100644 --- a/tests/propagation/trajectory.rs +++ b/tests/propagation/trajectory.rs @@ -173,13 +173,13 @@ fn traj_ephem_forward(almanac: Arc) { // Check the data info one by one. Time may be very slightly off. let delta_t = orig_state.epoch() - loaded_state.epoch(); assert!( - delta_t < 500 * Unit::Nanosecond, + delta_t < 1 * Unit::Microsecond, "#{i} differ (Δt = {delta_t})" ); assert_eq!( orig_state.to_vector(), loaded_state.to_vector(), - "#{i} differ (Δt = {delta_t})" + "#{i} differ" ); } } diff --git a/tests/python/test_gauss_markov.py b/tests/python/test_gauss_markov.py index 32cd0440..eee32c7e 100644 --- a/tests/python/test_gauss_markov.py +++ b/tests/python/test_gauss_markov.py @@ -16,10 +16,7 @@ def test_fogm(plot=False): gm = GaussMarkov(tau=Unit.Hour * 24, sigma=5.6, steady_state=0.5) print(gm) - assert ( - str(gm) - == "First order Gauss-Markov process with τ = 1 days, σ_b = 5.6, σ_q = 0.5" - ) + assert str(gm) == "First order Gauss-Markov process with τ = 1 days, σ_b = 5.6, σ_q = 0.5" gm.simulate(str(outpath.joinpath("fogm.parquet"))) # Read in the file diff --git a/tests/python/test_mission_design.py b/tests/python/test_mission_design.py index 577f8e43..fa72f1e9 100644 --- a/tests/python/test_mission_design.py +++ b/tests/python/test_mission_design.py @@ -109,7 +109,6 @@ def test_propagate(): assert rebuilt_traj.first().epoch == epochs[1] assert rebuilt_traj.last().epoch == epochs[-2] - # Export this trajectory with additional metadata and the events # Base path root = Path(__file__).joinpath("../../../").resolve() @@ -190,9 +189,7 @@ def test_build_spacecraft(): assert sc.srp() == srp assert sc.drag().area_m2 == 0.0 - assert ( - sc.drag().cd == 2.2 - ) # Default value, but the area is zero, so it doesn't have any effect + assert sc.drag().cd == 2.2 # Default value, but the area is zero, so it doesn't have any effect # Using this spacecraft as a template, let's load an OEM file, convert it to Parquet, and ensure we can load it back in. # The orbit data will be overwritten with data from the OEM file. @@ -293,9 +290,7 @@ def test_merge_traj(): sc1 = Spacecraft.load(str(config_path.joinpath("spacecraft.yaml"))) - dynamics = SpacecraftDynamics.load_named( - str(config_path.joinpath("dynamics.yaml")) - )["lofi"] + dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml")))["lofi"] # Check loading from the YAML read from Python with open(config_path.joinpath("dynamics.yaml")) as fh: diff --git a/tests/python/test_orbit_determination.py b/tests/python/test_orbit_determination.py index 7895664c..78159091 100644 --- a/tests/python/test_orbit_determination.py +++ b/tests/python/test_orbit_determination.py @@ -92,9 +92,7 @@ def test_filter_arc(): # Generate the measurements print(arc_sim.generate_schedule()) arc_sim.build_schedule() - msr_path = arc_sim.generate_measurements( - str(outpath.joinpath("./msr.parquet")), cfg - ) + msr_path = arc_sim.generate_measurements(str(outpath.joinpath("./msr.parquet")), cfg) print(f"Saved {arc_sim} to {msr_path}") # Now let's filter this same data. @@ -102,9 +100,7 @@ def test_filter_arc(): arc = DynamicTrackingArc(msr_path) # Create the orbit estimate with the covariance diagonal (100 km on position and 1 km/s on velocity) - orbit_est = OrbitEstimate( - sc.orbit, covar=np.diag([100.0, 100.0, 100.0, 1.0, 1.0, 1.0]) - ) + orbit_est = OrbitEstimate(sc.orbit, covar=np.diag([100.0, 100.0, 100.0, 1.0, 1.0, 1.0])) # Check loading from the YAML read from Python with open(config_path.joinpath("orbit_estimates.yaml")) as fh: @@ -146,7 +142,7 @@ def test_filter_arc(): ekf_num_msr_trig, ekf_disable_time, snc_disable_time=Unit.Minute * 10.0, - snc_diagonals=[5e-12, 5e-12, 5e-12] + snc_diagonals=[5e-12, 5e-12, 5e-12], ) print(f"Stored to {rslt_path}") @@ -258,9 +254,7 @@ def test_one_way_msr(): dynamics = SpacecraftDynamics.load_named(str(config_path.joinpath("dynamics.yaml"))) # Load the devices - devices = GroundStation.load_many( - str(config_path.joinpath("./many_ground_stations.yaml")) - ) + devices = GroundStation.load_many(str(config_path.joinpath("./many_ground_stations.yaml"))) print(f"Loaded {devices}") # One way measurement @@ -281,9 +275,7 @@ def test_one_way_msr(): "elevation (deg)": [], } # Start by building a time series - ts = TimeSeries( - traj.first().epoch, traj.last().epoch, step=Unit.Minute * 30, inclusive=True - ) + ts = TimeSeries(traj.first().epoch, traj.last().epoch, step=Unit.Minute * 30, inclusive=True) # And iterate over it for epoch in ts: orbit = traj.at(epoch).orbit