diff --git a/Cargo.toml b/Cargo.toml index 74e9c4ce..b22d94b1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -27,6 +27,7 @@ exclude = [ "rustfmt.toml", "tests/GMAT_scripts/*", "*.png", + "*.pca", ] [badges] @@ -126,3 +127,8 @@ doc-scrape-examples = true name = "03_geo_sk" path = "examples/03_geo_analysis/stationkeeping.rs" doc-scrape-examples = true + +[[example]] +name = "04_lro_od" +path = "examples/04_lro_od/main.rs" +doc-scrape-examples = true diff --git a/README.md b/README.md index 8dba08fc..5daf9bb1 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,13 @@ # Nyx: Comprehensive Spaceflight Dynamics -[**Empowering flight dynamics engineers with open-source software**](https://nyxspace.com/) +[**Empowering flight dynamics engineers with open-source software**][website] 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 Badget][nyxspace-image]][website] +[![Contact Form][contact-form-image]][contact] [![nyx-space on crates.io][cratesio-image]][cratesio] [![nyx-space on docs.rs][docsrs-image]][docsrs] @@ -15,7 +15,7 @@ Nyx is revolutionizing the field of flight dynamics engineering as a powerful, o # 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). +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][contact] ## Quick start @@ -34,13 +34,17 @@ For Python projects, get started by installing the library via `pip`: `pip insta # 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. +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/?utm_source=readme-price) 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://rustdoc.nyxspace.com/ +[docsrs]: https://rustdoc.nyxspace.com/?utm_source=readme +[contact-form-image]: https://img.shields.io/badge/Nyx_Space-Contact-orange +[contact]: https://7ug5imdtt8v.typeform.com/to/neFvVW3p +[nyxspace-image]: https://img.shields.io/badge/Nyx_Space-Website-orange +[website]: https://nyxspace.com/?utm_source=readme # Author information > Chris Rabotin is a GNC and flight dynamics engineer with a heavy background in software. diff --git a/examples/03_geo_analysis/plot_3d_traj.py b/examples/03_geo_analysis/plot_3d_traj.py index 7dab2ffd..c289a803 100644 --- a/examples/03_geo_analysis/plot_3d_traj.py +++ b/examples/03_geo_analysis/plot_3d_traj.py @@ -1,8 +1,6 @@ import argparse -from datetime import datetime import polars as pl import plotly.graph_objs as go -from plotly.subplots import make_subplots import numpy as np diff --git a/examples/04_lro_od/README.md b/examples/04_lro_od/README.md new file mode 100644 index 00000000..c671bc65 --- /dev/null +++ b/examples/04_lro_od/README.md @@ -0,0 +1,224 @@ +# Orbit Determination of the Lunar Reconnaissance Orbiter + +**Spacecraft operations require high fidelity modeling of the orbital dynamics and high fidelity orbit determination. This example demonstrates that the LRO team could effectively use Nyx for orbit determination.** + +In this example, you'll learn how to use an "as-flown" (_definitive_) SPICE BSP ephemeris file to simulate orbit determination measurements from ground stations. Then, you'll learn how to set up an orbit determination process in Nyx with high fidelity Moon dynamics and estimate the state of LRO. Finally, you'll learn how to compare two ephemerides in the Radial, In-track, Cross-track (RIC) frame. + +**Jump to the [results](#results)** + +To run this example, just execute: +```sh +RUST_LOG=info cargo run --example 04_lro_od --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. + +Throughout this analysis, we'll be focusing on an arbitrarily chosen period of one day started on 2024-01-01 at midnight UTC. + +# Preliminary analysis: matching dynamical models + +In the case of the Lunar Reconnaissance Orbiter (herein _LRO_), NASA publishes the definitive ephemeris on the website. Therefore, the first step in this analysis is to match the dynamical models between the LRO team and Nyx. This serves as a validation of the dynamical models in Nyx as well. + +The original ephemeris file by NASA is in _big endian_ format, and my machine (like most computers) is little endian. I've used the `bingo` tool from https://naif.jpl.nasa.gov/naif/utilities_PC_Linux_64bit.html to convert the original file to little endian and upload it to the public data cloud hosted on . Refer to https://naif.jpl.nasa.gov/pub/naif/pds/data/lro-l-spice-6-v1.0/lrosp_1000/data/spk/?C=M;O=D for original file. Also note that throughout this analysis, we're using the JPL Development Ephemerides version 421 instead of the latest and greated DE440 because LRO uses DE421 (although analysis shows no noticeable difference in switching out these ephems). + +For this preliminary analysis, we'll configure the dynamical models taking inspiration from the 2015 paper by Slojkowski et al. [Orbit Determination For The Lunar Reconnaissance Orbiter Using +An Extended Kalman Filter](https://ntrs.nasa.gov/api/citations/20150019754/downloads/20150019754.pdf). In this paper, the LRO team compares the OD solution between GTDS and AGI/Ansys ODTK. We will be performing the same analysis but with Nyx! + +Cislunar propagation involves several well-determined forces, which can be directly used in Nyx: + +- Solar radiation pressure; +- Point mass gravity forces from the central object (Moon) and other celestial objects whose force is relevant, namely Earth, Sun, Jupiter, maybe Saturn; +- Moon gravity field because we're in a low lunar orbit, so it greatly affects the orbital dynamics + +The purpose of this analysis is to ensure that we've configured these models correctly. This process is tedious because each dynamical model must be configured differently and the difference between the propagation and the truth ephemeris need to be assessed. + +When using the GRAIL gravity model JGGRX 250x250 with the SRP configured with a coefficient of reflectivity of `0.96` (as per the LRO OD paper above), and the gravity parameter provided by JPL (and default in ANISE 0.4), we end up with a pretty large error shown in these Radial, In-Track, Cross-Track plots. + +![JPL GM Pos error](./plots/sim-default-ric-pos-err.png) + +![JPL GM Vel error](./plots/sim-default-ric-vel-err.png) + +Since the velocity is the time derivative of the position, and since the error in the velocity is almost exclusively in the radial direction (i.e. from the spacecraft to the center of the Moon), the primary hypothesis is that the error is due to the gravity parameter of the Moon. In other words, the mass of the Moon used by LRO when they publish their ephemeris is different from the latest and greatest provided in [`gm_de440.tpc`](https://naif.jpl.nasa.gov/pub/naif/generic_kernels/pck/gm_de440.tpc), used by ANISE. + +**In fact, after dozens of simulations using a secant method, we find that the gravity parameter that leads to the least error is 4902.74987 km^3/s^2**. This is surpringly far from the nominal value of 4902.800066163796 km^2/s^3. Other parameters that I've fiddled with include changing the GRAIL gravity field to an older version, changing the degree and order of the gravity field, changing the value of the coefficient of reflectivity, enabling the point mass gravity of the Saturn system barycenter, changing the body fixed frame of the gravity field to the Moon ME frame (which should only be used for cartography), and swapping the DE421 for the DE440 planetary ephemerides. Using the DE403 Earth gravity parameter of 398600.436 km^2/s^3 also decreases the error. + +## Dynamical models + +- Solar radiation pressure: **Cr 0.96** +- Point mass gravity forces from the central object, **Moon: GM = 4902.74987 km^2/s^3** and other celestial objects whose force is relevant, namely Earth (**GM = 398600.436 km^3/s^**), Sun, and Jupiter; +- Moon gravity field GRAIL model JGGRX with the Moon Principal Axes frames (MOON PA) in 80x80 (degree x order) + +After model tuning, we reach a reasonable error with an average range error of 175 meters and an average velocity error of 0.116 m/s. + +``` +== Sim vs Flown (04_lro_sim_truth_error) == +RIC Range (km) +shape: (9, 2) +┌────────────┬──────────┐ +│ statistic ┆ value │ +│ --- ┆ --- │ +│ str ┆ f64 │ +╞════════════╪══════════╡ +│ count ┆ 2881.0 │ +│ null_count ┆ 0.0 │ +│ mean ┆ 0.175464 │ +│ std ┆ 0.100212 │ +│ min ┆ 0.0 │ +│ 25% ┆ 0.091487 │ +│ 50% ┆ 0.168715 │ +│ 75% ┆ 0.263027 │ +│ max ┆ 0.37063 │ +└────────────┴──────────┘ +RIC Range Rate (km/s) +shape: (9, 2) +┌────────────┬──────────┐ +│ statistic ┆ value │ +│ --- ┆ --- │ +│ str ┆ f64 │ +╞════════════╪══════════╡ +│ count ┆ 2881.0 │ +│ null_count ┆ 0.0 │ +│ mean ┆ 0.000116 │ +│ std ┆ 0.000039 │ +│ min ┆ 0.0 │ +│ 25% ┆ 0.000092 │ +│ 50% ┆ 0.000118 │ +│ 75% ┆ 0.000145 │ +│ max ┆ 0.000195 │ +└────────────┴──────────┘ +``` + +![New Lunar GM Pos error](./plots/sim-new-pc-ric-pos-err.png) + +![New Lunar GM Vel error](./plots/sim-new-pc-ric-vel-err.png) + +# Orbit determination set up + +## Ground network + +For this example, we simulate measurements from three of the Deep Space Network ground stations: Canberra, Australia; Madrid, Spain; and Goldstone, CA, USA. Nyx allows configuration of ground stations using a YAML input file, cf. [`dsn-network`](./dsn-network.yaml). These are configured as unbiased white noise ground stations where the standard deviation of the white noise is taken directly from the JPL DESCANSO series. The stochastic modeling in Nyx supports first order Gauss Markov processes and biased white noise. + +In this simulation, we are generating geometric one-way range and Doppler measurements. Nyx supports all of the aberration computations provided by ANISE (and validated against SPICE). + +## Tracking schedule + +To prepare for a mission, flight dynamics engineers must simulate a tracking schedule and determine, through trial and error, how much tracking is required throughout the different orbital regimes of the mission. Unlike most orbit determination software, Nyx provides a "schedule generator" for simulation. Refer to [`tracking-cfg.yaml`](./tracking-cfg.yaml) for the tracking configuration. In short, this feature allows engineers to configure the following key inputs to a schedule: + +- how to deal with overlapping measurements: greedy, eager, or overlap. + - Greedy: when two stations overlap, the one which was previously tracking will continue until the vehicle is no longer in sight + - Earger: when two stations overlap, the new tracker will start tracking instead of the previous one + - Overlap: both stations may track and generate tracking data at the same time. +- how many minimum samples are needed for this pass to be included: this prevents very short passes; +- are the measurements taken exactly at a round number of seconds; +- what is the sampling rate of this ground station. + +The tracking scheduler will start by finding the exact times when the vehicle comes in view, using the embedded event finder on an elevation event. + +```log + INFO nyx_space::md::events::search > Searching for DSS-13 Goldstone (lat.: 35.2472 deg long.: 243.2050 deg alt.: 1071.149 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] with initial heuristic of 14 min 24 s + INFO nyx_space::md::events::search > Event DSS-13 Goldstone (lat.: 35.2472 deg long.: 243.2050 deg alt.: 1071.149 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] found 2 times from 2024-01-01T05:49:54.697010810 UTC until 2024-01-01T18:08:53.555326604 UTC + INFO nyx_space::od::simulator::arc > Built 1 tracking strands for DSS-13 Goldstone + INFO nyx_space::od::simulator::arc > Building schedule for DSS-34 Canberra + INFO nyx_space::md::trajectory::sc_traj > Converted trajectory from Moon J2000 (μ = 4902.74987 km^3/s^2, radius = 1737.4 km) to Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2) in 220 ms: Trajectory in Earth IAU_EARTH (μ = 398600.436 km^3/s^2, eq. radius = 6378.1366 km, polar radius = 6356.7519 km, f = 0.0033528131084554717) from 2024-01-01T00:00:00 UTC to 2024-01-02T00:00:00 UTC (1 day, or 86400.000 s) [17281 states] + INFO nyx_space::md::events::search > Searching for DSS-34 Canberra (lat.: -35.3983 deg long.: 148.9819 deg alt.: 691.750 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] with initial heuristic of 14 min 24 s + INFO nyx_space::md::events::search > Event DSS-34 Canberra (lat.: -35.3983 deg long.: 148.9819 deg alt.: 691.750 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] found 2 times from 2024-01-01T13:19:29.424409217 UTC until 2024-01-01T23:47:35.676704956 UTC + INFO nyx_space::od::simulator::arc > Built 1 tracking strands for DSS-34 Canberra + INFO nyx_space::od::simulator::arc > Building schedule for DSS-65 Madrid + INFO nyx_space::md::trajectory::sc_traj > Converted trajectory from Moon J2000 (μ = 4902.74987 km^3/s^2, radius = 1737.4 km) to Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2) in 220 ms: Trajectory in Earth IAU_EARTH (μ = 398600.436 km^3/s^2, eq. radius = 6378.1366 km, polar radius = 6356.7519 km, f = 0.0033528131084554717) from 2024-01-01T00:00:00 UTC to 2024-01-02T00:00:00 UTC (1 day, or 86400.000 s) [17281 states] + INFO nyx_space::md::events::search > Searching for DSS-65 Madrid (lat.: 40.4272 deg long.: 4.2506 deg alt.: 834.939 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] with initial heuristic of 14 min 24 s + INFO nyx_space::md::events::search > Event DSS-65 Madrid (lat.: 40.4272 deg long.: 4.2506 deg alt.: 834.939 m) [Earth IAU_EARTH (μ = 398600.435436096 km^3/s^2)] found 2 times from 2024-01-01T09:59:19.297494580 UTC until 2024-01-01T22:19:56.653993644 UTC + INFO nyx_space::od::simulator::arc > Built 2 tracking strands for DSS-65 Madrid + INFO nyx_space::od::simulator::arc > DSS-65 Madrid configured as Greedy, so DSS-13 Goldstone now starts on 2024-01-01T10:00:20 UTC + INFO nyx_space::od::simulator::arc > DSS-13 Goldstone configured as Greedy, so DSS-34 Canberra now starts on 2024-01-01T18:09:50 UTC + INFO nyx_space::od::simulator::arc > DSS-34 Canberra now hands off to DSS-65 Madrid on 2024-01-01T22:19:00 UTC because it's configured as Eager + INFO nyx_space::od::simulator::arc > Simulated 319 measurements for DSS-13 Goldstone for 1 tracking strands in 13 ms + INFO nyx_space::od::simulator::arc > Simulated 185 measurements for DSS-34 Canberra for 1 tracking strands in 7 ms + INFO nyx_space::od::simulator::arc > Simulated 490 measurements for DSS-65 Madrid for 2 tracking strands in 18 ms + INFO nyx_space::od::msr::arc > Serialized 994 measurements from {"DSS-34 Canberra", "DSS-13 Goldstone", "DSS-65 Madrid"} to ./04_lro_simulated_tracking.parquet +994 measurements from {"DSS-13 Goldstone", "DSS-65 Madrid", "DSS-34 Canberra"} +``` + +## Tracking arc + +In this simulation, we use the official ephemeris to generate simulated measurements. In other words, we don't simulate a new ephemeris. **This serves as a validation of the orbit estimation in low lunar orbits,** the most dynamical of the cislunar orbits. + +Nyx will not generate measurements if the vehicle is obstructed by a celestial object, in this case, the obstruction is the presence of the Moon itself. + +![Schedule](./plots/msr-schedule.png) + +![Range (km)](./plots/msr-range.png) + +![Doppler (km/s)](./plots/msr-doppler.png) + +## Filter set up + +The OD filter uses the [dynamics determined above](#dynamical-models) after the [model matching](#preliminary-analysis-matching-dynamical-models) analysis. + +However, as seen in the model matching section, there remains a difference in the modeling of the orbital dynamics between Nyx and the published LRO ephemeris. This causes an oscillation of roughly 250 meters of range in the RIC frame. In the LRO orbit determination paper, the authors mention that GTDS is a batch least squares estimator, whereas this example uses a Kalman filter. To account for the modeling difference, we bump up the state noise compensation of the filter to `1e-11` km/s^2, or approximately 0.6 cm/s over a 10 minute span. This causes the covariance to increase when there are no measurements to accomodate for the small but accumulating modeling differences. + +The filter is configured with the default automatic residual rejection of Nyx whereby any residual ratio greater than 4 sigmas causes the measurement to be rejected. If the residual ratio exceeds this threshold, the measurement is considered an outlier and is rejected by the filter. This process helps to prevent measurements with large errors from negatively impacting the orbit determination solution. + +```log + INFO nyx_space::od::process > Navigation propagating for a total of 1 day with step size 1 min + INFO nyx_space::od::process > Processing 994 measurements with covariance mapping + INFO nyx_space::od::process > 10% done - 101 measurements accepted, 0 rejected + INFO nyx_space::od::process > 20% done - 200 measurements accepted, 0 rejected + INFO nyx_space::od::process > 30% done - 300 measurements accepted, 0 rejected + INFO nyx_space::od::process > 40% done - 394 measurements accepted, 5 rejected + INFO nyx_space::od::process > 50% done - 489 measurements accepted, 9 rejected + INFO nyx_space::od::process > 60% done - 586 measurements accepted, 12 rejected + INFO nyx_space::od::process > 70% done - 677 measurements accepted, 20 rejected + INFO nyx_space::od::process > 80% done - 767 measurements accepted, 30 rejected + INFO nyx_space::od::process > 90% done - 863 measurements accepted, 33 rejected + INFO nyx_space::od::process > 100% done - 955 measurements accepted, 39 rejected (done in 17 s 267 ms 50 μs 496 ns) +``` + +# Results + +**Nyx provides a good estimation of the orbit of the Lunar Reconnaissance Orbiter, matching the LRO team's desired uncertainty of 800 meters when tracking the vehicle,** despite modeling and filtering differences between GTDS and Nyx. As discussed in the [filter setup](#filter-set-up) section just above, we've had to increase the state noise compensation to account for [oscillating modeling differences]((#preliminary-analysis-matching-dynamical-models)) between Nyx and the LRO definitive ephemeris. + +![RIC Covar Position](./plots/covar-ric-pos.png) + +![RIC Covar Velocity](./plots/covar-ric-vel.png) + +## Residual ratios + +One of the key metrics to determine whether an OD result is correct is to look at the residual ratios. Some orbit determination software, like ODTK (as specified in their MathSpec), treats all measurements as scalars. This could be problematic because the order in which the measurements are processed at each time step will lead to variations in the covariance. For example, if there is an excellent range measurement and an average Doppler measurement for that same time step, then the state covariance will decrease and that greater model filter confidence may cause the Doppler measurement to be rejected. _Instead_, Nyx treats all simultaneous measurements simultaneously, ensuring that the order in which each measurement is processed is not a concern. This mathematically correct approach leads to the loss of the sign of the residual because the ratio is computed as `y_k^T * R_k^{-1} * y_k`, where `y_k` is prefit residual (using the nomenclature from the [Nyx Kalman filter MathSpec](https://nyxspace.com/nyxspace/MathSpec/orbit_determination/kalman/?utm_source=github-lro-rr)). **Therefore, the goodness test of the residual ratios is Chi Square test** with a degree of freedom of 2 (range and Doppler). + +We can see that nearly all of the residuals are within the Chi Square distribution, although the tail-end of 4 sigmas is larger than it should be. This is entirely due to the models not matching perfectly, as can be seen in the plot of the residuals rejected over time: the further we are from the start of the OD track, the more measurements are rejected. + +![Chi-Squared](./plots/resid-chi-square.png) + +![Rejection](./plots/resid-rejections.png) + +![Per tracker](./plots/resid-per-tracker.png) + +## Measurement residuals + +Another key metric is whether the residuals fit well within the expected measurement noise. As in ODTK, Nyx varies the measurement noise with the state covariance. + +### Range residuals + +The following plots show an auto-scaled and a zoomed-in version of the range residuals over time because the state covariance rises with the state noise compensation, causing the auto-scaling to hide the fun details, namely that the oscillatory nature of the modeling difference shows up in the prefit residuals but that the filter adequately corrects its own knowledge with each accepted measurement. The values on this plot is in km as indicated by the legend. + +![Range resid auto](./plots/range-resid.png) + +![Range resid zoom](./plots/range-resid-zoom.png) + +### Doppler residuals + +The Doppler residuals look great with the automatic zoom because the modeling differences are small but accumulating velocity errors. The values on this plot is in km/s as indicated by the legend. + +![Doopler resid](./plots/doppler-resid.png) + +## Verification + +In our case, we have the true definitive ephemeris of LRO. Hence, we can compare the difference between the orbit determination results in Nyx and the definitive ephemeris in the RIC frame. **The OD results are slightly better than a pure propagation of the orbit with the modeling error.** The couple of singularities in the RIC plot are common interpolation artifacts which crop up especially when computing the RIC differences, and are not actual state differences. + +![RIC OD vs truth pos err](./plots/od-vs-truth-ric-pos-err.png) + +![RIC OD vs truth vel err](./plots/od-vs-truth-ric-vel-err.png) + +# Conclusion + +The successful orbit determination of the Lunar Reconnaissance Orbiter using Nyx validates its capability as a reliable tool for precise orbit estimation in low lunar orbits, which are known for their highly dynamic nature. This validation opens up opportunities for using Nyx in future lunar missions, potentially reducing the reliance on other orbit determination software. \ No newline at end of file diff --git a/examples/04_lro_od/dsn-network.yaml b/examples/04_lro_od/dsn-network.yaml new file mode 100644 index 00000000..4e8168ea --- /dev/null +++ b/examples/04_lro_od/dsn-network.yaml @@ -0,0 +1,59 @@ +- name: DSS-65 Madrid + frame: + ephemeris_id: 399 + orientation_id: 399 + mu_km3_s2: 398600.435436096 + shape: null + elevation_mask_deg: 5.0 + range_noise_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_noise_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s + light_time_correction: false + latitude_deg: 40.427222 + longitude_deg: 4.250556 + height_km: 0.834939 + +- name: DSS-34 Canberra + frame: + ephemeris_id: 399 + orientation_id: 399 + mu_km3_s2: 398600.435436096 + shape: null + latitude_deg: -35.398333 + longitude_deg: 148.981944 + height_km: 0.691750 + elevation_mask_deg: 5.0 + range_noise_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_noise_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s + light_time_correction: false + +- name: DSS-13 Goldstone + frame: + ephemeris_id: 399 + orientation_id: 399 + mu_km3_s2: 398600.435436096 + shape: null + latitude_deg: 35.247164 + longitude_deg: 243.205 + height_km: 1.071149 + elevation_mask_deg: 5.0 + range_noise_km: + white_noise: + mean: 0.0 + sigma: 5.0e-3 # 5 m + doppler_noise_km_s: + white_noise: + mean: 0.0 + sigma: 50.0e-6 # 5 cm/s + light_time_correction: false diff --git a/examples/04_lro_od/gfx/LRO-ANISE-GUI.png b/examples/04_lro_od/gfx/LRO-ANISE-GUI.png new file mode 100644 index 00000000..0755e1c3 Binary files /dev/null and b/examples/04_lro_od/gfx/LRO-ANISE-GUI.png differ diff --git a/examples/04_lro_od/lro-dynamics.dhall b/examples/04_lro_od/lro-dynamics.dhall new file mode 100644 index 00000000..e720ceb0 --- /dev/null +++ b/examples/04_lro_od/lro-dynamics.dhall @@ -0,0 +1,22 @@ +-- Default Almanac +{ files = + [ { crc32 = Some 1551416339 -- 0x6f2456aa + , uri = "http://public-data.nyxspace.com/anise/de421.bsp" + } + , { crc32 = Some 2182330089 + , uri = "http://public-data.nyxspace.com/anise/v0.4/pck11.pca" + } + , { crc32 = Some 3454388861 + , uri = "http://public-data.nyxspace.com/anise/moon_pa_de440_200625.bpc" + } + -- Download the latest (of time of writing) LRO definitive ephemeris from the public Nyx Space cloud. + -- Note that the original file is in _big endian_ format, and my machine is little endian, so I've used the + -- `bingo` tool from https://naif.jpl.nasa.gov/naif/utilities_PC_Linux_64bit.html to convert the original file + -- to little endian and upload it to the cloud. + -- Refer to https://naif.jpl.nasa.gov/pub/naif/pds/data/lro-l-spice-6-v1.0/lrosp_1000/data/spk/?C=M;O=D for original file. + , { crc32 = Some 3882673077 + , uri = + "http://public-data.nyxspace.com/nyx/examples/lrorg_2023349_2024075_v01_LE.bsp" + } + ] +} diff --git a/examples/04_lro_od/lro-specific.pca b/examples/04_lro_od/lro-specific.pca new file mode 100644 index 00000000..d786be44 --- /dev/null +++ b/examples/04_lro_od/lro-specific.pca @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fbccaaa03797a9c752f9f9823e891908c152dec84cd3c65a2716e2d6341f508b +size 38067 diff --git a/examples/04_lro_od/main.rs b/examples/04_lro_od/main.rs new file mode 100644 index 00000000..ab926aa9 --- /dev/null +++ b/examples/04_lro_od/main.rs @@ -0,0 +1,292 @@ +#![doc = include_str!("./README.md")] +extern crate log; +extern crate nyx_space as nyx; +extern crate pretty_env_logger as pel; + +use anise::{ + almanac::metaload::MetaFile, + constants::{ + celestial_objects::{EARTH, JUPITER_BARYCENTER, MOON, SUN}, + frames::{EARTH_J2000, MOON_J2000, MOON_PA_FRAME}, + }, +}; +use hifitime::{Epoch, TimeUnits, Unit}; +use nyx::{ + cosmic::{Aberration, Frame, MetaAlmanac, SrpConfig}, + dynamics::{ + guidance::LocalFrame, Harmonics, OrbitalDynamics, SolarPressure, SpacecraftDynamics, + }, + io::{ConfigRepr, ExportCfg}, + md::prelude::{HarmonicsMem, Traj}, + od::{ + msr::RangeDoppler, + prelude::{TrackingArcSim, TrkConfig, KF}, + process::{Estimate, NavSolution, ODProcess, ResidRejectCrit, SpacecraftUncertainty}, + snc::SNC3, + GroundStation, + }, + propagators::Propagator, + Orbit, Spacecraft, State, +}; + +use std::{collections::BTreeMap, error::Error, path::PathBuf, str::FromStr, sync::Arc}; + +fn main() -> Result<(), Box> { + pel::init(); + + // ====================== // + // === ALMANAC SET UP === // + // ====================== // + + // Dynamics models require planetary constants and ephemerides to be defined. + // Let's start by grabbing those by using ANISE's MetaAlmanac. + + let data_folder: PathBuf = [env!("CARGO_MANIFEST_DIR"), "examples", "04_lro_od"] + .iter() + .collect(); + + let meta = data_folder.join("lro-dynamics.dhall"); + + // Load this ephem in the general Almanac we're using for this analysis. + let mut almanac = MetaAlmanac::new(meta.to_string_lossy().to_string()) + .map_err(Box::new)? + .process() + .map_err(Box::new)?; + + let mut moon_pc = almanac.planetary_data.get_by_id(MOON)?; + moon_pc.mu_km3_s2 = 4902.74987; + almanac.planetary_data.set_by_id(MOON, moon_pc)?; + + let mut earth_pc = almanac.planetary_data.get_by_id(EARTH)?; + earth_pc.mu_km3_s2 = 398600.436; + almanac.planetary_data.set_by_id(EARTH, earth_pc)?; + + // Save this new kernel for reuse. + // In an operational context, this would be part of the "Lock" process, and should not change throughout the mission. + almanac + .planetary_data + .save_as(&data_folder.join("lro-specific.pca"), true)?; + + // Lock the almanac (an Arc is a read only structure). + let almanac = Arc::new(almanac); + + // Orbit determination requires a Trajectory structure, which can be saved as parquet file. + // In our case, the trajectory comes from the BSP file, so we need to build a Trajectory from the almanac directly. + // To query the Almanac, we need to build the LRO frame in the J2000 orientation in our case. + // Inspecting the LRO BSP in the ANISE GUI shows us that NASA has assigned ID -85 to LRO. + let lro_frame = Frame::from_ephem_j2000(-85); + + // To build the trajectory we need to provide a spacecraft template. + let sc_template = Spacecraft::builder() + .dry_mass_kg(1018.0) // Launch masses + .fuel_mass_kg(900.0) + .srp(SrpConfig { + // SRP configuration is arbitrary, but we will be estimating it anyway. + area_m2: 3.9 * 2.7, + cr: 0.96, + }) + .orbit(Orbit::zero(MOON_J2000)) // Setting a zero orbit here because it's just a template + .build(); + // Now we can build the trajectory from the BSP file. + // We'll arbitrarily set the tracking arc to 48 hours with a one minute time step. + let traj_as_flown = Traj::from_bsp( + lro_frame, + MOON_J2000, + almanac.clone(), + sc_template, + 5.seconds(), + Some(Epoch::from_str("2024-01-01 00:00:00 UTC")?), + Some(Epoch::from_str("2024-01-02 00:00:00 UTC")?), + Aberration::LT, + Some("LRO".to_string()), + )?; + + println!("{traj_as_flown}"); + + // ====================== // + // === MODEL MATCHING === // + // ====================== // + + // Set up the spacecraft dynamics. + + // Specify that the orbital dynamics must account for the graviational pull of the Earth and the Sun. + // The gravity of the Moon will also be accounted for since the spaceraft in a lunar orbit. + let mut orbital_dyn = OrbitalDynamics::point_masses(vec![EARTH, SUN, JUPITER_BARYCENTER]); + + // We want to include the spherical harmonics, so let's download the gravitational data from the Nyx Cloud. + // We're using the GRAIL JGGRX model. + let mut jggrx_meta = MetaFile { + uri: "http://public-data.nyxspace.com/nyx/models/Luna_jggrx_1500e_sha.tab.gz".to_string(), + crc32: Some(0x6bcacda8), // Specifying the CRC32 avoids redownloading it if it's cached. + }; + // And let's download it if we don't have it yet. + jggrx_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 Moon principal axes frame. + let moon_pa_frame = MOON_PA_FRAME.with_orient(31008); + // let moon_pa_frame = IAU_MOON_FRAME; + let sph_harmonics = Harmonics::from_stor( + almanac.frame_from_uid(moon_pa_frame)?, + HarmonicsMem::from_shadr(&jggrx_meta.uri, 80, 80, true)?, + ); + + // Include the spherical harmonics into the orbital dynamics. + orbital_dyn.accel_models.push(sph_harmonics); + + // We define the solar radiation pressure, using the default solar flux and accounting only + // for the eclipsing caused by the Earth and Moon. + // Note that by default, enabling the SolarPressure model will also enable the estimation of the coefficient of reflectivity. + let srp_dyn = SolarPressure::new(vec![EARTH_J2000, MOON_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}"); + + // Now we can build the propagator. + let setup = Propagator::default_dp78(dynamics.clone()); + + // For reference, let's build the trajectory with Nyx's models from that LRO state. + let (sim_final, traj_as_sim) = setup + .with(*traj_as_flown.first(), almanac.clone()) + .until_epoch_with_traj(traj_as_flown.last().epoch())?; + + println!("SIM INIT: {:x}", traj_as_flown.first()); + println!("SIM FINAL: {sim_final:x}"); + // Compute RIC difference between SIM and LRO ephem + let sim_lro_delta = sim_final + .orbit + .ric_difference(&traj_as_flown.last().orbit)?; + println!("{traj_as_sim}"); + println!( + "SIM v LRO - RIC Position (m): {:.3}", + sim_lro_delta.radius_km * 1e3 + ); + println!( + "SIM v LRO - RIC Velocity (m/s): {:.3}", + sim_lro_delta.velocity_km_s * 1e3 + ); + + traj_as_sim.ric_diff_to_parquet( + &traj_as_flown, + "./04_lro_sim_truth_error.parquet", + ExportCfg::default(), + )?; + + // ==================== // + // === OD SIMULATOR === // + // ==================== // + + // After quite some time trying to exactly match the model, we still end up with an oscillatory difference on the order of 150 meters between the propagated state + // and the truth LRO state. + + // Therefore, we will actually run an estimation from a dispersed LRO state. + // The sc_seed is the true LRO state from the BSP. + let sc_seed = *traj_as_flown.first(); + + // Load the Deep Space Network ground stations. + // Nyx allows you to build these at runtime but it's pretty static so we can just load them from YAML. + let ground_station_file: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "examples", + "04_lro_od", + "dsn-network.yaml", + ] + .iter() + .collect(); + + let devices = GroundStation::load_many(ground_station_file)?; + + // Typical OD software requires that you specify your own tracking schedule or you'll have overlapping measurements. + // Nyx can build a tracking schedule for you based on the first station with access. + let trkconfg_yaml: PathBuf = [ + env!("CARGO_MANIFEST_DIR"), + "examples", + "04_lro_od", + "tracking-cfg.yaml", + ] + .iter() + .collect(); + + let configs: BTreeMap = TrkConfig::load_named(trkconfg_yaml)?; + + // Build the tracking arc simulation to generate a "standard measurement". + let mut trk = TrackingArcSim::::new( + devices, + traj_as_flown.clone(), + configs, + )?; + + trk.build_schedule(almanac.clone())?; + let arc = trk.generate_measurements(almanac.clone())?; + // Save the simulated tracking data + arc.to_parquet_simple("./04_lro_simulated_tracking.parquet")?; + + // We'll note that in our case, we have continuous coverage of LRO when the vehicle is not behind the Moon. + println!("{arc}"); + + // Now that we have simulated measurements, we'll run the orbit determination. + + // ===================== // + // === OD ESTIMATION === // + // ===================== // + + let sc = SpacecraftUncertainty::builder() + .nominal(sc_seed) + .frame(LocalFrame::RIC) + .x_km(0.5) + .y_km(0.5) + .z_km(0.5) + .vx_km_s(5e-3) + .vy_km_s(5e-3) + .vz_km_s(5e-3) + .build(); + + // Build the filter initial estimate, which we will reuse in the filter. + let initial_estimate = sc.to_estimate()?; + + println!("== FILTER STATE ==\n{sc_seed:x}\n{initial_estimate}"); + + let kf = KF::new( + // Increase the initial covariance to account for larger deviation. + initial_estimate, + // Until https://github.com/nyx-space/nyx/issues/351, we need to specify the SNC in the acceleration of the Moon J2000 frame. + SNC3::from_diagonal(10 * Unit::Minute, &[1e-11, 1e-11, 1e-11]), + ); + + // We'll set up the OD process to reject measurements whose residuals are mover than 4 sigmas away from what we expect. + let mut odp = ODProcess::ckf( + setup.with(initial_estimate.state().with_stm(), almanac.clone()), + kf, + Some(ResidRejectCrit::default()), + almanac.clone(), + ); + + odp.process_arc::(&arc)?; + + let ric_err = traj_as_flown + .at(odp.estimates.last().unwrap().epoch())? + .orbit + .ric_difference(&odp.estimates.last().unwrap().orbital_state())?; + println!("== RIC at end =="); + println!("RIC Position (m): {}", ric_err.radius_km * 1e3); + println!("RIC Velocity (m/s): {}", ric_err.velocity_km_s * 1e3); + + odp.to_parquet("./04_lro_od_results.parquet", ExportCfg::default())?; + + // In our case, we have the truth trajectory from NASA. + // So we can compute the RIC state difference between the real LRO ephem and what we've just estimated. + // Export the OD trajectory first. + let od_trajectory = odp.to_traj()?; + // Build the RIC difference. + od_trajectory.ric_diff_to_parquet( + &traj_as_flown, + "./04_lro_od_truth_error.parquet", + ExportCfg::default(), + )?; + + Ok(()) +} diff --git a/examples/04_lro_od/plot_msr.py b/examples/04_lro_od/plot_msr.py new file mode 100644 index 00000000..018c79a3 --- /dev/null +++ b/examples/04_lro_od/plot_msr.py @@ -0,0 +1,23 @@ +import polars as pl +import plotly.express as px + +if __name__ == "__main__": + df = pl.read_parquet("./04_lro_simulated_tracking.parquet") + + df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( + "Epoch (UTC)", descending=False + ) + + # Build a title + station_names = ", ".join([name for name in df["Tracking device"].unique()]) + start = df["Epoch (UTC)"][0] + end = df["Epoch (UTC)"][-1] + arc_duration = end - start + title = f"Measurements from {station_names} spanning {start} to {end} ({arc_duration})" + + # Plot the overall tracking + px.strip(df, x="Epoch (UTC)", y="Tracking device", color="Tracking device").show() + + # Plot each measurement kind + for msr_col_name in ["Range (km)", "Doppler (km/s)"]: + px.scatter(df, x="Epoch (UTC)", y=msr_col_name, color="Tracking device").show() diff --git a/examples/04_lro_od/plot_od_rslt.py b/examples/04_lro_od/plot_od_rslt.py new file mode 100644 index 00000000..4d06dffb --- /dev/null +++ b/examples/04_lro_od/plot_od_rslt.py @@ -0,0 +1,156 @@ +import polars as pl +from scipy.stats import chi2 +import numpy as np +import plotly.graph_objects as go +import plotly.express as px + +if __name__ == "__main__": + df = pl.read_parquet("./04_lro_od_results.parquet") + + df = df.with_columns(pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f")).sort( + "Epoch (UTC)", descending=False + ) + # Add the +/- 3 sigmas on measurement noise + df = df.with_columns( + [ + (3.0 * pl.col("Measurement noise: Range (km)")).alias( + "Measurement noise 3-Sigma: Range (km)" + ), + (-3.0 * pl.col("Measurement noise: Range (km)")).alias( + "Measurement noise -3-Sigma: Range (km)" + ), + ] + ) + df = df.with_columns( + [ + (3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( + "Measurement noise 3-Sigma: Doppler (km/s)" + ), + (-3.0 * pl.col("Measurement noise: Doppler (km/s)")).alias( + "Measurement noise -3-Sigma: Doppler (km/s)" + ), + ] + ) + + # == Residual plots == + # Nyx uses the Mahalanobis distance for the residual ratios, so we test the goodness using the Chi Square distribution. + freedoms = 2 # Two degrees of freedoms for the range and the range rate. + x_chi = np.linspace(chi2.ppf(0.01, freedoms), chi2.ppf(0.99, freedoms), 100) + y_chi = chi2.pdf(x_chi, freedoms) + + # Compute the scaling factor + hist = np.histogram(df["Residual ratio"].fill_null(0.0), bins=50)[0] + max_hist = max(hist[1:]) # Ignore the bin of zeros + max_chi2_pdf = max(y_chi) + scale_factor = max_hist / max_chi2_pdf + + fig = go.Figure() + fig.add_trace(go.Scatter(x=x_chi, y=y_chi * scale_factor, mode="lines", name="Scaled Chi-Squared")) + fig.add_trace(go.Histogram(x=df["Residual ratio"], nbinsx=100, name="Residual ratios")) + fig.show() + + px.histogram( + df, + x="Residual ratio", + color="Tracker", + marginal="rug", # can be `box`, `violin` + hover_data=df.columns, + ).show() + + # Plot the residual ratios and whether they were accepted. + px.scatter(df, x="Epoch (UTC)", y="Residual ratio", color="Residual Rejected").show() + + df_resid_ok = df.filter(df["Residual Rejected"] == False) + + # Plot the measurement residuals and their noises. + for msr in ["Range (km)", "Doppler (km/s)"]: + y_cols = [ + f"{col}: {msr}" + for col in [ + "Prefit residual", + "Postfit residual", + "Measurement noise 3-Sigma", + "Measurement noise -3-Sigma", + ] + ] + fig = px.scatter(df_resid_ok, x="Epoch (UTC)", y=y_cols) + fig.update_traces( + mode="lines", + selector=dict(name=f"Measurement noise 3-Sigma: {msr}"), + connectgaps=True, + line=dict(dash="dash", color="black"), + ) + fig.update_traces( + mode="lines", + selector=dict(name=f"Measurement noise -3-Sigma: {msr}"), + connectgaps=True, + line=dict(dash="dash", color="black"), + ) + fig.show() + + # Plot the RIC uncertainty + px.line( + df, x="Epoch (UTC)", y=["Sigma X (RIC) (km)", "Sigma Y (RIC) (km)", "Sigma Z (RIC) (km)"] + ).show() + + px.line( + df, + x="Epoch (UTC)", + y=["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"], + ).show() + + # Load the RIC diff. + for fname, errname in [ + ("04_lro_od_truth_error", "OD vs Flown"), + ("04_lro_sim_truth_error", "Sim vs Flown (model matching)"), + ]: + df_ric = pl.read_parquet(f"./{fname}.parquet") + df_ric = df_ric.with_columns( + pl.col("Epoch (UTC)").str.to_datetime("%Y-%m-%dT%H:%M:%S%.f") + ).sort("Epoch (UTC)", descending=False) + # Compute the range and range rate columns + df_ric = df_ric.with_columns( + [ + ( + ( + pl.col("Delta X (RIC) (km)") ** 2 + + pl.col("Delta Y (RIC) (km)") ** 2 + + pl.col("Delta Z (RIC) (km)") ** 2 + ) + ** 0.5 + ).alias("RIC Range (km)"), + ( + ( + pl.col("Delta Vx (RIC) (km/s)") ** 2 + + pl.col("Delta Vy (RIC) (km/s)") ** 2 + + pl.col("Delta Vz (RIC) (km/s)") ** 2 + ) + ** 0.5 + ).alias("RIC Range Rate (km/s)"), + ] + ) + + print(f"== {errname} ({fname}) ==") + print("RIC Range (km)") + print(df_ric["RIC Range (km)"].describe()) + print("RIC Range Rate (km/s)") + print(df_ric["RIC Range Rate (km/s)"].describe()) + + # Plot the RIC difference + px.line( + df_ric, + x="Epoch (UTC)", + y=["Delta X (RIC) (km)", "Delta Y (RIC) (km)", "Delta Z (RIC) (km)", "RIC Range (km)"], + title=f"Position error with {errname} ({fname})", + ).show() + px.line( + df_ric, + x="Epoch (UTC)", + y=[ + "Delta Vx (RIC) (km/s)", + "Delta Vy (RIC) (km/s)", + "Delta Vz (RIC) (km/s)", + "RIC Range Rate (km/s)", + ], + title=f"Velocity error with {errname} ({fname})", + ).show() diff --git a/examples/04_lro_od/plots/covar-ric-pos.png b/examples/04_lro_od/plots/covar-ric-pos.png new file mode 100644 index 00000000..527966f0 Binary files /dev/null and b/examples/04_lro_od/plots/covar-ric-pos.png differ diff --git a/examples/04_lro_od/plots/covar-ric-vel.png b/examples/04_lro_od/plots/covar-ric-vel.png new file mode 100644 index 00000000..c28409cf Binary files /dev/null and b/examples/04_lro_od/plots/covar-ric-vel.png differ diff --git a/examples/04_lro_od/plots/doppler-resid.png b/examples/04_lro_od/plots/doppler-resid.png new file mode 100644 index 00000000..baf10d97 Binary files /dev/null and b/examples/04_lro_od/plots/doppler-resid.png differ diff --git a/examples/04_lro_od/plots/msr-doppler.png b/examples/04_lro_od/plots/msr-doppler.png new file mode 100644 index 00000000..7e6683b2 Binary files /dev/null and b/examples/04_lro_od/plots/msr-doppler.png differ diff --git a/examples/04_lro_od/plots/msr-range.png b/examples/04_lro_od/plots/msr-range.png new file mode 100644 index 00000000..cd324ac9 Binary files /dev/null and b/examples/04_lro_od/plots/msr-range.png differ diff --git a/examples/04_lro_od/plots/msr-schedule.png b/examples/04_lro_od/plots/msr-schedule.png new file mode 100644 index 00000000..36112288 Binary files /dev/null and b/examples/04_lro_od/plots/msr-schedule.png differ diff --git a/examples/04_lro_od/plots/od-vs-truth-ric-pos-err.png b/examples/04_lro_od/plots/od-vs-truth-ric-pos-err.png new file mode 100644 index 00000000..99002f26 Binary files /dev/null and b/examples/04_lro_od/plots/od-vs-truth-ric-pos-err.png differ diff --git a/examples/04_lro_od/plots/od-vs-truth-ric-vel-err.png b/examples/04_lro_od/plots/od-vs-truth-ric-vel-err.png new file mode 100644 index 00000000..12704980 Binary files /dev/null and b/examples/04_lro_od/plots/od-vs-truth-ric-vel-err.png differ diff --git a/examples/04_lro_od/plots/range-resid-zoom.png b/examples/04_lro_od/plots/range-resid-zoom.png new file mode 100644 index 00000000..19d9f8a8 Binary files /dev/null and b/examples/04_lro_od/plots/range-resid-zoom.png differ diff --git a/examples/04_lro_od/plots/range-resid.png b/examples/04_lro_od/plots/range-resid.png new file mode 100644 index 00000000..74200b8f Binary files /dev/null and b/examples/04_lro_od/plots/range-resid.png differ diff --git a/examples/04_lro_od/plots/resid-chi-square.png b/examples/04_lro_od/plots/resid-chi-square.png new file mode 100644 index 00000000..8bdaa249 Binary files /dev/null and b/examples/04_lro_od/plots/resid-chi-square.png differ diff --git a/examples/04_lro_od/plots/resid-per-tracker.png b/examples/04_lro_od/plots/resid-per-tracker.png new file mode 100644 index 00000000..05131260 Binary files /dev/null and b/examples/04_lro_od/plots/resid-per-tracker.png differ diff --git a/examples/04_lro_od/plots/resid-rejections.png b/examples/04_lro_od/plots/resid-rejections.png new file mode 100644 index 00000000..4c1fe434 Binary files /dev/null and b/examples/04_lro_od/plots/resid-rejections.png differ diff --git a/examples/04_lro_od/plots/sim-default-ric-pos-err.png b/examples/04_lro_od/plots/sim-default-ric-pos-err.png new file mode 100644 index 00000000..e0698685 Binary files /dev/null and b/examples/04_lro_od/plots/sim-default-ric-pos-err.png differ diff --git a/examples/04_lro_od/plots/sim-default-ric-vel-err.png b/examples/04_lro_od/plots/sim-default-ric-vel-err.png new file mode 100644 index 00000000..fe4b96ff Binary files /dev/null and b/examples/04_lro_od/plots/sim-default-ric-vel-err.png differ diff --git a/examples/04_lro_od/plots/sim-new-pc-ric-pos-err.png b/examples/04_lro_od/plots/sim-new-pc-ric-pos-err.png new file mode 100644 index 00000000..1971b57a Binary files /dev/null and b/examples/04_lro_od/plots/sim-new-pc-ric-pos-err.png differ diff --git a/examples/04_lro_od/plots/sim-new-pc-ric-vel-err.png b/examples/04_lro_od/plots/sim-new-pc-ric-vel-err.png new file mode 100644 index 00000000..165b8a26 Binary files /dev/null and b/examples/04_lro_od/plots/sim-new-pc-ric-vel-err.png differ diff --git a/examples/04_lro_od/tracking-cfg.yaml b/examples/04_lro_od/tracking-cfg.yaml new file mode 100644 index 00000000..123c180a --- /dev/null +++ b/examples/04_lro_od/tracking-cfg.yaml @@ -0,0 +1,27 @@ +DSS-65 Madrid: + scheduler: + # If there is an upcoming overlap, Madrid continues tracking instead of the next station + handoff: Greedy + cadence: Continuous + # min samples, i.e. 30 minutes arc minimum because of the 1 min sampling. + # This minimum samples does NOT include the handoff logic, only the visibility. + min_samples: 30 + sample_alignment: 10 s + sampling: 1 min + +DSS-34 Canberra: + scheduler: + # If there is an upcoming overlap, Goldstone will handoff to the next station even if it can still see LRO + handoff: Eager + cadence: Continuous + min_samples: 30 + sample_alignment: 10 s + sampling: 1 min + +DSS-13 Goldstone: + scheduler: + handoff: Greedy + cadence: Continuous + min_samples: 30 + sample_alignment: 10 s + sampling: 1 min diff --git a/src/dynamics/solarpressure.rs b/src/dynamics/solarpressure.rs index 37bbac2a..ca65761e 100644 --- a/src/dynamics/solarpressure.rs +++ b/src/dynamics/solarpressure.rs @@ -79,10 +79,10 @@ impl SolarPressure { /// Accounts for the shadowing of only one body and will set the solar flux at 1 AU to: Phi = 1367.0 pub fn default_no_estimation( - shadow_body: Frame, + shadow_bodies: Vec, almanac: Arc, ) -> Result, DynamicsError> { - let mut srp = Self::default_raw(vec![shadow_body], almanac)?; + let mut srp = Self::default_raw(shadow_bodies, almanac)?; srp.estimate = false; Ok(Arc::new(srp)) } diff --git a/src/md/trajectory/sc_traj.rs b/src/md/trajectory/sc_traj.rs index f93e12bd..d29b4c58 100644 --- a/src/md/trajectory/sc_traj.rs +++ b/src/md/trajectory/sc_traj.rs @@ -16,8 +16,11 @@ along with this program. If not, see . */ +use anise::astro::Aberration; use anise::constants::orientations::J2000; +use anise::errors::AlmanacError; use anise::prelude::{Almanac, Frame, Orbit}; +use hifitime::TimeSeries; use snafu::ResultExt; use super::TrajError; @@ -40,6 +43,41 @@ use std::sync::Arc; use std::time::Instant; impl Traj { + /// Builds a new trajectory built from the SPICE BSP (SPK) file loaded in the provided Almanac, provided the start and stop epochs. + /// + /// If the start and stop epochs are not provided, then the full domain of the trajectory will be used. + pub fn from_bsp( + target_frame: Frame, + observer_frame: Frame, + almanac: Arc, + sc_template: Spacecraft, + step: Duration, + start_epoch: Option, + end_epoch: Option, + ab_corr: Option, + name: Option, + ) -> Result { + let (domain_start, domain_end) = + almanac + .spk_domain(target_frame.ephemeris_id) + .map_err(|e| AlmanacError::Ephemeris { + action: "could not fetch domain", + source: Box::new(e), + })?; + + let start_epoch = start_epoch.unwrap_or(domain_start); + let end_epoch = end_epoch.unwrap_or(domain_end); + + let time_series = TimeSeries::inclusive(start_epoch, end_epoch, step); + let mut states = Vec::with_capacity(time_series.len()); + for epoch in time_series { + let orbit = almanac.transform(target_frame, observer_frame, epoch, ab_corr)?; + + states.push(sc_template.with_orbit(orbit)); + } + + Ok(Self { name, states }) + } /// Allows converting the source trajectory into the (almost) equivalent trajectory in another frame #[allow(clippy::map_clone)] pub fn to_frame(&self, new_frame: Frame, almanac: Arc) -> Result { @@ -129,11 +167,6 @@ impl Traj { traj.to_parquet(path, events, cfg, almanac) } - /// Convert this spacecraft trajectory into an Orbit trajectory, loosing all references to the spacecraft - pub fn downcast(&self) -> Self { - unimplemented!() - } - /// Initialize a new spacecraft trajectory from the path to a CCSDS OEM file. /// /// CCSDS OEM only contains the orbit information but Nyx builds spacecraft trajectories. diff --git a/src/md/trajectory/traj.rs b/src/md/trajectory/traj.rs index b981d43a..58ba2f45 100644 --- a/src/md/trajectory/traj.rs +++ b/src/md/trajectory/traj.rs @@ -363,12 +363,16 @@ where let mut hdrs = vec![Field::new("Epoch (UTC)", DataType::Utf8, false)]; // Add the RIC headers - for coord in ["x", "y", "z"] { + for coord in ["X", "Y", "Z"] { let mut meta = HashMap::new(); meta.insert("unit".to_string(), "km".to_string()); - let field = Field::new(format!("delta_{coord}_ric (km)"), DataType::Float64, false) - .with_metadata(meta); + let field = Field::new( + format!("Delta {coord} (RIC) (km)"), + DataType::Float64, + false, + ) + .with_metadata(meta); hdrs.push(field); } @@ -378,7 +382,7 @@ where meta.insert("unit".to_string(), "km/s".to_string()); let field = Field::new( - format!("delta_v{coord}_ric (km/s)"), + format!("Delta V{coord} (RIC) (km/s)"), DataType::Float64, false, ) @@ -390,12 +394,9 @@ where let frame = self.states[0].frame(); 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(), - }) - })?, + serde_dhall::serialize(&frame) + .to_string() + .unwrap_or(frame.to_string()), )]); let mut cfg = cfg; @@ -599,7 +600,7 @@ where f, "Trajectory {}in {} from {} to {} ({}, or {:.3} s) [{} states]", match &self.name { - Some(name) => format!("of {name}"), + Some(name) => format!("of {name} "), None => String::new(), }, self.first().frame(), diff --git a/src/od/estimate/kfestimate.rs b/src/od/estimate/kfestimate.rs index fb066f17..a03d3826 100644 --- a/src/od/estimate/kfestimate.rs +++ b/src/od/estimate/kfestimate.rs @@ -32,6 +32,7 @@ use rand_pcg::Pcg64Mcg; use std::cmp::PartialEq; use std::error::Error; use std::fmt; +use std::ops::Mul; /// Kalman filter Estimate #[derive(Debug, Copy, Clone, PartialEq)] @@ -329,6 +330,23 @@ where } } +impl Mul for KfEstimate +where + DefaultAllocator: Allocator<::Size> + + Allocator<::Size, ::Size> + + Allocator<::Size> + + Allocator<::VecLength>, + ::Size>>::Buffer: Copy, + ::Size, ::Size>>::Buffer: Copy, +{ + type Output = Self; + + fn mul(mut self, rhs: f64) -> Self::Output { + self.covar *= rhs.powi(2); + self + } +} + #[cfg(test)] mod ut_kfest { use crate::{ diff --git a/src/od/estimate/residual.rs b/src/od/estimate/residual.rs index adca78e2..30d3d0b1 100644 --- a/src/od/estimate/residual.rs +++ b/src/od/estimate/residual.rs @@ -34,7 +34,9 @@ where pub prefit: OVector, /// The postfit residual in the units of the measurement type pub postfit: OVector, - /// The prefit residual ratio, i.e. `r' * (H*P*H')^-1 * r`, where `r` is the prefit residual, `H` is the sensitivity matrix, and `P` is the covariance matrix. + /// The prefit residual ratio computed as the Mahalanobis distance, i.e. it is always positive + /// and computed as `r' * (H*P*H')^-1 * r`, where `r` is the prefit residual, `H` is the sensitivity matrix, and `P` is the covariance matrix. + /// To assess the performance, look at the Chi Square distribution for the number of measurements, e.g. 2 for range and range-rate. pub ratio: f64, /// The tracker measurement noise (variance)) for this tracker at this time. pub tracker_msr_noise: OVector, @@ -67,14 +69,14 @@ where epoch: Epoch, prefit: OVector, ratio: f64, - tracker_msr_noise: OVector, + tracker_msr_covar: OVector, ) -> Self { Self { epoch, prefit, postfit: OVector::::zeros(), ratio, - tracker_msr_noise, + tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: true, tracker: None, } @@ -85,14 +87,14 @@ where prefit: OVector, postfit: OVector, ratio: f64, - tracker_msr_noise: OVector, + tracker_msr_covar: OVector, ) -> Self { Self { epoch, prefit, postfit, ratio, - tracker_msr_noise, + tracker_msr_noise: tracker_msr_covar.map(|x| x.sqrt()), rejected: false, tracker: None, } diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index a0fb5451..86581b59 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -276,7 +276,7 @@ where nominal_state: T, real_obs: &OVector, computed_obs: &OVector, - measurement_noise: OMatrix, + measurement_covar: OMatrix, resid_rejection: Option, ) -> Result<(Self::Estimate, Residual), ODError> { if !self.h_tilde_updated { @@ -287,72 +287,29 @@ where let epoch = nominal_state.epoch(); - let mut covar_bar = stm * self.prev_estimate.covar * stm.transpose(); - // Try to apply an SNC, if applicable - for (i, snc) in self.process_noise.iter().enumerate().rev() { - if let Some(snc_matrix) = snc.to_matrix(epoch) { - // Check if we're using another SNC than the one before - if self.prev_used_snc != i { - info!("Switched to {}-th {}", i, snc); - self.prev_used_snc = i; - } - - // Let's compute the Gamma matrix, an approximation of the time integral - // which assumes that the acceleration is constant between these two measurements. - let mut gamma = OMatrix::::Size, A>::zeros(); - let delta_t = (epoch - self.prev_estimate.epoch()).to_seconds(); - for blk in 0..A::dim() / 3 { - for i in 0..3 { - let idx_i = i + A::dim() * blk; - let idx_j = i + 3 * blk; - let idx_k = i + 3 + A::dim() * blk; - // For first block - // (0, 0) (1, 1) (2, 2) <=> \Delta t^2/2 - // (3, 0) (4, 1) (5, 2) <=> \Delta t - // Second block - // (6, 3) (7, 4) (8, 5) <=> \Delta t^2/2 - // (9, 3) (10, 4) (11, 5) <=> \Delta t - // * \Delta t^2/2 - // (i, i) when blk = 0 - // (i + A::dim() * blk, i + 3) when blk = 1 - // (i + A::dim() * blk, i + 3 * blk) - // * \Delta t - // (i + 3, i) when blk = 0 - // (i + 3, i + 9) when blk = 1 (and I think i + 12 + 3) - // (i + 3 + A::dim() * blk, i + 3 * blk) - gamma[(idx_i, idx_j)] = delta_t.powi(2) / 2.0; - gamma[(idx_k, idx_j)] = delta_t; - } - } - // Let's add the process noise - covar_bar += &gamma * snc_matrix * &gamma.transpose(); - // And break so we don't add any more process noise - break; - } - } + let covar_bar = stm * self.prev_estimate.covar * stm.transpose(); let h_tilde_t = &self.h_tilde.transpose(); let h_p_ht = &self.h_tilde * covar_bar * h_tilde_t; // Account for state uncertainty in the measurement noise. Equation 4.10 of ODTK MathSpec. - let r_k = &h_p_ht + measurement_noise; + let r_k = &h_p_ht + measurement_covar; // Compute observation deviation (usually marked as y_i) let prefit = real_obs - computed_obs; - // Compute the prefit ratio - let ratio_mat = prefit.transpose() * &h_p_ht * &prefit; - let ratio = ratio_mat[0]; + // Compute the prefit ratio for the automatic rejection + let r_k_inv = r_k.clone().try_inverse().ok_or(ODError::SingularNoiseRk)?; + let ratio_mat = prefit.transpose() * r_k_inv * &prefit; + let ratio = ratio_mat[0].sqrt(); if let Some(resid_reject) = resid_rejection { - for ii in 0..M::USIZE { - if prefit[ii].abs() > r_k[(ii, ii)].sqrt() * resid_reject.num_sigmas { - // Reject this whole measurement and perform only a time update - let pred_est = self.time_update(nominal_state)?; - return Ok(( - pred_est, - Residual::rejected(epoch, prefit, ratio, r_k.diagonal()), - )); - } + if ratio > resid_reject.num_sigmas { + // Reject this whole measurement and perform only a time update + let pred_est = self.time_update(nominal_state)?; + return Ok(( + pred_est, + Residual::rejected(epoch, prefit, ratio, r_k.diagonal()), + )); } } diff --git a/src/od/filter/mod.rs b/src/od/filter/mod.rs index 54f6da07..3d0e1573 100644 --- a/src/od/filter/mod.rs +++ b/src/od/filter/mod.rs @@ -75,14 +75,14 @@ where /// * `nominal_state`: the nominal state at which the observation was computed. /// * `real_obs`: the real observation that was measured. /// * `computed_obs`: the computed observation from the nominal state. - /// * `measurement_noise`: the measurement noise associated with this time update. + /// * `measurement_covar`: the measurement covariance associated with this time update (i./e. the square of the standard deviation) /// * `resid_rejection`: the automatic residual rejection criteria, if enabled. fn measurement_update( &mut self, nominal_state: T, real_obs: &OVector, computed_obs: &OVector, - measurement_noise: OMatrix, + measurement_covar: OMatrix, resid_rejection: Option, ) -> Result<(Self::Estimate, Residual), ODError>; diff --git a/src/od/ground_station.rs b/src/od/ground_station.rs index 1867b2d1..1daeb146 100644 --- a/src/od/ground_station.rs +++ b/src/od/ground_station.rs @@ -22,7 +22,8 @@ use anise::prelude::{Almanac, Frame, Orbit}; use super::msr::RangeDoppler; use super::noise::StochasticNoise; -use super::{ODAlmanacSnafu, ODError, ODTrajSnafu, TrackingDeviceSim}; +use super::{ODAlmanacSnafu, ODError, ODPlanetaryDataSnafu, ODTrajSnafu, TrackingDeviceSim}; +use crate::cosmic::eclipse::{line_of_sight, EclipseState}; use crate::errors::EventError; use crate::io::ConfigRepr; use crate::md::prelude::{Interpolatable, Traj}; @@ -157,11 +158,9 @@ impl GroundStation { } /// Computes the azimuth and elevation of the provided object seen from this ground station, both in degrees. - /// Also returns the ground station's orbit in the frame of the receiver + /// This is a shortcut to almanac.azimuth_elevation_range_sez. pub fn azimuth_elevation_of(&self, rx: Orbit, almanac: &Almanac) -> AlmanacResult { - almanac - .clone() - .azimuth_elevation_range_sez(rx, self.to_orbit(rx.epoch, almanac).unwrap()) + almanac.azimuth_elevation_range_sez(rx, self.to_orbit(rx.epoch, almanac).unwrap()) } /// Return this ground station as an orbit in its current frame @@ -256,6 +255,29 @@ impl TrackingDeviceSim for GroundStation { return Ok(None); } + // If the frame of the trajectory is different from that of the ground station, then check that there is no eclipse. + for rx in [rx_0, rx_1] { + if !self.frame.ephem_origin_match(rx.frame()) { + let observer = self.to_orbit(rx.orbit.epoch, &almanac).unwrap(); + if line_of_sight( + observer, + rx.orbit, + almanac + .frame_from_uid(rx.frame()) + .context(ODPlanetaryDataSnafu { + action: "computing line of sight", + })?, + &almanac, + ) + .context(ODAlmanacSnafu { + action: "computing line of sight", + })? == EclipseState::Umbra + { + return Ok(None); + } + } + } + // Noises are computed at the midpoint of the integration time. let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = self.noises(epoch - integration_time * 0.5, rng)?; @@ -292,6 +314,26 @@ impl TrackingDeviceSim for GroundStation { action: "computing AER", })?; + if !self.frame.ephem_origin_match(rx.frame()) { + let observer = self.to_orbit(rx.orbit.epoch, &almanac).unwrap(); + if line_of_sight( + observer, + rx.orbit, + almanac + .frame_from_uid(rx.frame()) + .context(ODPlanetaryDataSnafu { + action: "computing line of sight", + })?, + &almanac, + ) + .context(ODAlmanacSnafu { + action: "computing line of sight", + })? == EclipseState::Umbra + { + return Ok(None); + } + } + if aer.elevation_deg >= self.elevation_mask_deg { // Only update the noises if the measurement is valid. let (timestamp_noise_s, range_noise_km, doppler_noise_km_s) = @@ -319,7 +361,7 @@ impl TrackingDeviceSim for GroundStation { /// The measurement noise is computed assuming that all measurements are independent variables, i.e. the measurement matrix is /// a diagonal matrix. The first item in the diagonal is the range noise (in km), set to the square of the steady state sigma. The /// second item is the Doppler noise (in km/s), set to the square of the steady state sigma of that Gauss Markov process. - fn measurement_noise( + fn measurement_covar( &mut self, epoch: Epoch, ) -> Result< @@ -330,22 +372,22 @@ impl TrackingDeviceSim for GroundStation { >, ODError, > { - let range_noise_km = self + let range_noise_km2 = self .range_noise_km .ok_or(ODError::NoiseNotConfigured { kind: "Range" })? - .variance(epoch); - let doppler_noise_km_s = self + .covariance(epoch); + let doppler_noise_km2_s2 = self .doppler_noise_km_s .ok_or(ODError::NoiseNotConfigured { kind: "Doppler" })? - .variance(epoch); + .covariance(epoch); let mut msr_noises = OMatrix::< f64, ::MeasurementSize, ::MeasurementSize, >::zeros(); - msr_noises[(0, 0)] = range_noise_km; - msr_noises[(1, 1)] = doppler_noise_km_s; + msr_noises[(0, 0)] = range_noise_km2; + msr_noises[(1, 1)] = doppler_noise_km2_s2; Ok(msr_noises) } diff --git a/src/od/mod.rs b/src/od/mod.rs index 962eb882..821074de 100644 --- a/src/od/mod.rs +++ b/src/od/mod.rs @@ -26,6 +26,7 @@ use crate::propagators::PropagationError; use crate::time::Epoch; use crate::Orbit; pub use crate::{State, TimeTagged}; +use anise::almanac::planetary::PlanetaryDataError; use anise::errors::AlmanacError; use hifitime::Duration; use snafu::prelude::Snafu; @@ -190,6 +191,8 @@ pub enum ODError { SensitivityNotUpdated, #[snafu(display("Kalman gain is singular"))] SingularKalmanGain, + #[snafu(display("Noise matrix is singular"))] + SingularNoiseRk, #[snafu(display("{kind} noise not configured"))] NoiseNotConfigured { kind: &'static str }, #[snafu(display("during an OD encountered {source}"))] @@ -204,6 +207,12 @@ pub enum ODError { source: Box, action: &'static str, }, + #[snafu(display("OD failed due to planetary data in Almanac: {action} {source}"))] + ODPlanetaryData { + #[snafu(source(from(PlanetaryDataError, Box::new)))] + source: Box, + action: &'static str, + }, #[snafu(display("not enough residuals to {action}"))] ODNoResiduals { action: &'static str }, } diff --git a/src/od/noise/gauss_markov.rs b/src/od/noise/gauss_markov.rs index 17b79950..1957e126 100644 --- a/src/od/noise/gauss_markov.rs +++ b/src/od/noise/gauss_markov.rs @@ -124,7 +124,7 @@ impl GaussMarkov { } impl Stochastics for GaussMarkov { - fn variance(&self, _epoch: Epoch) -> f64 { + fn covariance(&self, _epoch: Epoch) -> f64 { self.process_noise.powi(2) } diff --git a/src/od/noise/mod.rs b/src/od/noise/mod.rs index 5e9f81d1..5d02eed9 100644 --- a/src/od/noise/mod.rs +++ b/src/od/noise/mod.rs @@ -39,7 +39,7 @@ pub use white::WhiteNoise; /// Trait for any kind of stochastic modeling, developing primarily for synthetic orbit determination measurements. pub trait Stochastics { /// Return the variance of this stochastic noise model at a given time. - fn variance(&self, epoch: Epoch) -> f64; + fn covariance(&self, epoch: Epoch) -> f64; /// Returns a new sample of these stochastics fn sample(&mut self, epoch: Epoch, rng: &mut R) -> f64; @@ -105,16 +105,16 @@ impl StochasticNoise { sample } - /// Return the variance of these stochastics at a given time. - pub fn variance(&self, epoch: Epoch) -> f64 { + /// Return the covariance of these stochastics at a given time. + pub fn covariance(&self, epoch: Epoch) -> f64 { let mut variance = 0.0; if let Some(wn) = &self.white_noise { - variance += wn.variance(epoch).sqrt(); + variance += wn.covariance(epoch); } if let Some(gm) = &self.bias { - variance += gm.variance(epoch).sqrt(); + variance += gm.covariance(epoch); } - variance.powi(2) + variance } /// Simulate the configured stochastic model and store the bias in a parquet file. @@ -147,7 +147,7 @@ impl StochasticNoise { // Skip to see how the variance changes. continue; } - let variance = mdl.variance(epoch); + let variance = mdl.covariance(epoch); let sample = mdl.sample(epoch, &mut rng); samples.push(StochasticState { run, diff --git a/src/od/noise/white.rs b/src/od/noise/white.rs index 799d1e9b..305d1189 100644 --- a/src/od/noise/white.rs +++ b/src/od/noise/white.rs @@ -68,7 +68,7 @@ impl WhiteNoise { } impl Stochastics for WhiteNoise { - fn variance(&self, _epoch: Epoch) -> f64 { + fn covariance(&self, _epoch: Epoch) -> f64 { self.sigma.powi(2) } diff --git a/src/od/process/mod.rs b/src/od/process/mod.rs index 75558684..d4a44782 100644 --- a/src/od/process/mod.rs +++ b/src/od/process/mod.rs @@ -507,6 +507,7 @@ where let mut traj: Traj = Traj::new(); let mut msr_accepted_cnt: usize = 0; + let tick = Epoch::now().unwrap(); for (msr_cnt, (device_name, msr)) in measurements.iter().enumerate() { let next_msr_epoch = msr.epoch(); @@ -585,7 +586,7 @@ where nominal_state, &msr.observation(), &computed_meas.observation(), - device.measurement_noise(epoch)?, + device.measurement_covar(epoch)?, self.resid_crit, ) { Ok((estimate, mut residual)) => { @@ -639,7 +640,7 @@ where if !reported[msr_prct] { let num_rejected = msr_cnt - msr_accepted_cnt.saturating_sub(1); let msg = format!( - "{:>3}% done ({msr_accepted_cnt:.0} measurements accepted, {:.0} rejected)", + "{:>3}% done - {msr_accepted_cnt:.0} measurements accepted, {:.0} rejected", 10 * msr_prct, num_rejected ); if msr_accepted_cnt < num_rejected { @@ -671,8 +672,9 @@ where // Always report the 100% mark if !reported[10] { + let tock_time = Epoch::now().unwrap() - tick; info!( - "100% done ({msr_accepted_cnt:.0} measurements accepted, {:.0} rejected)", + "100% done - {msr_accepted_cnt:.0} measurements accepted, {:.0} rejected (done in {tock_time})", num_msrs - msr_accepted_cnt ); } diff --git a/src/od/simulator/trackdata.rs b/src/od/simulator/trackdata.rs index 4e9b7dde..06b8f28c 100644 --- a/src/od/simulator/trackdata.rs +++ b/src/od/simulator/trackdata.rs @@ -75,7 +75,7 @@ where ) -> Result, ODError>; // Return the noise statistics of this tracking device at the requested epoch. - fn measurement_noise( + fn measurement_covar( &mut self, epoch: Epoch, ) -> Result, ODError>; diff --git a/src/od/snc.rs b/src/od/snc.rs index 10b37921..28d0e01e 100644 --- a/src/od/snc.rs +++ b/src/od/snc.rs @@ -38,7 +38,7 @@ where pub start_time: Option, /// Specify the frame of this SNC -- CURRENTLY UNIMPLEMENTED pub frame: Option, - /// Enables state noise compensation (process noise) only be applied if the time between measurements is less than the disable_time amount in seconds + /// Enables state noise compensation (process noise) only be applied if the time between measurements is less than the disable_time pub disable_time: Duration, // Stores the initial epoch when the SNC is requested, needed for decay. Kalman filter will edit this automatically. pub init_epoch: Option, diff --git a/tests/orbit_determination/mod.rs b/tests/orbit_determination/mod.rs index bf8e79cc..d2b6fdb3 100644 --- a/tests/orbit_determination/mod.rs +++ b/tests/orbit_determination/mod.rs @@ -71,7 +71,7 @@ fn filter_errors() { ) { Ok(_) => panic!("expected the measurement update to fail"), Err(e) => { - assert_eq!(e, ODError::SingularKalmanGain); + assert_eq!(e, ODError::SingularNoiseRk); } } } diff --git a/tests/orbit_determination/resid_reject.rs b/tests/orbit_determination/resid_reject.rs index d672eccf..ba7c8288 100644 --- a/tests/orbit_determination/resid_reject.rs +++ b/tests/orbit_determination/resid_reject.rs @@ -243,7 +243,7 @@ fn od_resid_reject_inflated_snc_ckf_two_way( .filter(|residual| residual.rejected) .count(); - assert!(num_rejections < 10, "oddly high rejections"); + assert!(dbg!(num_rejections) < 20, "oddly high rejections"); // Check that the final post-fit residual isn't too bad, and definitely much better than the prefit. let est = &odp.estimates.last().unwrap(); diff --git a/tests/orbit_determination/spacecraft.rs b/tests/orbit_determination/spacecraft.rs index c38b5929..512d2db8 100644 --- a/tests/orbit_determination/spacecraft.rs +++ b/tests/orbit_determination/spacecraft.rs @@ -402,7 +402,6 @@ fn od_val_sc_srp_estimation( // Define the initial orbit estimate let initial_estimate = sc.to_estimate().unwrap(); - // let ckf = KF::no_snc(initial_estimate); let ckf = KF::new( initial_estimate, SNC3::from_diagonal(2 * Unit::Minute, &[1e-15, 1e-15, 1e-15]), diff --git a/tests/orbit_determination/two_body.rs b/tests/orbit_determination/two_body.rs index 53d7fd5a..9ee2c56b 100644 --- a/tests/orbit_determination/two_body.rs +++ b/tests/orbit_determination/two_body.rs @@ -829,13 +829,15 @@ fn od_tb_ckf_fixed_step_perfect_stations_snc_covar_map( ); } - for i in 0..6 { - assert!( - est.covar[(i, i)] >= sigma_q, - "covar diagonal less than SNC value @ {} = {:.3e}", - no, - est.covar[(i, i)] - ); + if est.predicted() { + for i in 0..6 { + assert!( + est.covar[(i, i)] >= sigma_q, + "covar diagonal less than SNC value @ {} = {:.3e}", + no, + est.covar[(i, i)] + ); + } } }