From 710d193af20c8cdb5cf954ca53594c356ec691b4 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 20:27:48 -0400 Subject: [PATCH 01/26] Prune the book to the pages v1.0 will actually carry (#383) Removes the six loose design and setup notes in book/ (RBPF.md, EKF_USAGE.md, EKF_GEONAV.md, PROJECT_SUMMARY.md, QUICK_REFERENCE.md, GITHUB_PAGES_SETUP.md). Each described APIs or binaries that no longer exist; the reusable parts are merged into src/ pages later in this branch. Also removes the one-line placeholder pages that the new outline folds together: the eight api/ stubs become one page pointing at rustdoc, the three GNSS scenario stubs become one reference page, and the three geonav stubs become one. open-loop.md is retired because dead reckoning is dr, and ol is unimplemented. The new SUMMARY lists only pages that the rest of this branch fills. Co-Authored-By: Claude Opus 5.5 --- book/EKF_GEONAV.md | 212 --------- book/EKF_USAGE.md | 419 ----------------- book/GITHUB_PAGES_SETUP.md | 305 ------------ book/PROJECT_SUMMARY.md | 346 -------------- book/QUICK_REFERENCE.md | 166 ------- book/RBPF.md | 715 ----------------------------- book/src/SUMMARY.md | 61 +-- book/src/api/core.md | 1 - book/src/api/earth.md | 1 - book/src/api/geonav.md | 1 - book/src/api/kalman.md | 1 - book/src/api/measurements.md | 1 - book/src/api/particles.md | 1 - book/src/api/sim-binary.md | 1 - book/src/api/sim.md | 1 - book/src/chapter_1.md | 1 - book/src/development/structure.md | 1 - book/src/geonav/data-sources.md | 1 - book/src/geonav/gravity.md | 1 - book/src/geonav/magnetic.md | 1 - book/src/gnss/corruption.md | 1 - book/src/gnss/dropouts.md | 1 - book/src/gnss/reduced-rates.md | 1 - book/src/installation/building.md | 5 - book/src/installation/crates-io.md | 5 - book/src/resources/links.md | 1 - book/src/user-guide/open-loop.md | 1 - 27 files changed, 25 insertions(+), 2227 deletions(-) delete mode 100644 book/EKF_GEONAV.md delete mode 100644 book/EKF_USAGE.md delete mode 100644 book/GITHUB_PAGES_SETUP.md delete mode 100644 book/PROJECT_SUMMARY.md delete mode 100644 book/QUICK_REFERENCE.md delete mode 100644 book/RBPF.md delete mode 100644 book/src/api/core.md delete mode 100644 book/src/api/earth.md delete mode 100644 book/src/api/geonav.md delete mode 100644 book/src/api/kalman.md delete mode 100644 book/src/api/measurements.md delete mode 100644 book/src/api/particles.md delete mode 100644 book/src/api/sim-binary.md delete mode 100644 book/src/api/sim.md delete mode 100644 book/src/chapter_1.md delete mode 100644 book/src/development/structure.md delete mode 100644 book/src/geonav/data-sources.md delete mode 100644 book/src/geonav/gravity.md delete mode 100644 book/src/geonav/magnetic.md delete mode 100644 book/src/gnss/corruption.md delete mode 100644 book/src/gnss/dropouts.md delete mode 100644 book/src/gnss/reduced-rates.md delete mode 100644 book/src/installation/building.md delete mode 100644 book/src/installation/crates-io.md delete mode 100644 book/src/resources/links.md delete mode 100644 book/src/user-guide/open-loop.md diff --git a/book/EKF_GEONAV.md b/book/EKF_GEONAV.md deleted file mode 100644 index 983eb55f..00000000 --- a/book/EKF_GEONAV.md +++ /dev/null @@ -1,212 +0,0 @@ -# EKF-based Geophysical Navigation - -This document describes the Extended Kalman Filter (EKF) implementation for geophysical navigation in the `strapdown-rs` project. - -## Overview - -The EKF-based geophysical INS provides an alternative to the Unscented Kalman Filter (UKF) for integrating inertial navigation with geophysical measurements (gravity and magnetic anomalies). While the UKF uses sigma points to propagate uncertainty through nonlinear transformations, the EKF uses first-order Taylor series approximations (Jacobians) to linearize the system dynamics and measurement models. - -## Features - -- **15-state EKF**: Position (3), velocity (3), attitude (3), and IMU biases (6) -- **Geophysical measurements**: Gravity and magnetic anomaly measurements from geophysical maps -- **Numerical Jacobians**: Map gradients computed using finite differences for measurement updates -- **Health monitoring**: Integrated with existing health check framework -- **Compatible with UKF**: Drop-in replacement via CLI flag - -## Implementation Details - -### State Vector - -The EKF uses a 15-dimensional state vector (when biases are enabled): - -``` -x = [lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw, b_ax, b_ay, b_az, b_gx, b_gy, b_gz] -``` - -Where: -- `lat, lon, alt`: Position in geodetic coordinates (radians, radians, meters) -- `v_n, v_e, v_d`: Velocity in local-level frame (m/s) -- `roll, pitch, yaw`: Attitude Euler angles (radians) -- `b_ax, b_ay, b_az`: Accelerometer biases (m/s²) -- `b_gx, b_gy, b_gz`: Gyroscope biases (rad/s) - -### Predict Step - -The predict step propagates the state and covariance using: - -1. **Nonlinear state propagation**: Uses the `forward()` function to integrate IMU measurements through the strapdown mechanization equations -2. **Linearized covariance propagation**: $\bar{P} = F P F^T + Q$ where: - - $F$ is the state transition Jacobian (computed in `core/src/linearize.rs`) - - $Q$ is the process noise covariance - -### Update Step - -The update step incorporates measurements to correct the state estimate: - -1. **Measurement prediction**: $\hat{z} = h(\bar{x})$ -2. **Innovation covariance**: $S = H \bar{P} H^T + R$ -3. **Kalman gain**: $K = \bar{P} H^T S^{-1}$ -4. **State correction**: $x = \bar{x} + K(z - \hat{z})$ -5. **Covariance update**: $P = (I - KH)\bar{P}(I - KH)^T + KRK^T$ (Joseph form) - -Where: -- $H$ is the measurement Jacobian -- $R$ is the measurement noise covariance - -### Geophysical Measurements - -#### Gravity Anomaly Measurements - -Gravity measurements depend on: -- Position (lat, lon) for map lookup -- Velocity (for Eotvos correction) -- Altitude (for free-air correction) - -The measurement Jacobian is computed numerically using finite differences on the geophysical map: - -```rust -let (dlat, dlon) = map.get_gradient(&lat, &lon, epsilon); -``` - -#### Magnetic Anomaly Measurements - -Magnetic anomaly measurements depend on: -- Position (lat, lon) for map lookup -- Altitude (for reference field computation via WMM) -- Date (for WMM temporal variation) - -Similar to gravity, the Jacobian is computed numerically from the map gradient. - -### Numerical Gradient Computation - -The `GeoMap::get_gradient()` method computes map gradients using central finite differences: - -``` -∂z/∂lat ≈ (z(lat+ε) - z(lat-ε)) / (2ε) -∂z/∂lon ≈ (z(lon+ε) - z(lon-ε)) / (2ε) -``` - -With automatic fallback to forward/backward differences at map boundaries. - -## Usage - -### Command Line - -```bash -# Run with EKF (instead of the default ESKF) -strapdown-geonav \ - --input sensor_data.csv \ - --output navigation_solution.csv \ - --filter ekf \ - --geo-type gravity \ - --geo-noise-std 100.0 - -# Run with magnetic anomalies -strapdown-geonav \ - --input sensor_data.csv \ - --output navigation_solution.csv \ - --filter ekf \ - --geo-type magnetic \ - --geo-noise-std 50.0 -``` - -### Programmatic API - -```rust -use std::rc::Rc; -use strapdown::kalman::{ExtendedKalmanFilter, InitialState}; -use strapdown::sim::run_closed_loop; -use geonav::{GeoMap, build_event_stream}; - -// Initialize EKF -let initial_state = InitialState { /* ... */ }; -let mut ekf = ExtendedKalmanFilter::new( - initial_state, - vec![0.0; 6], // IMU biases - cov_diagonal, - process_noise, - true, // use 15-state with biases -); - -// Load geophysical map -let geomap = GeoMap::load_geomap(map_path, measurement_type)?; - -// Build event stream -// `?`: an empty record slice cannot supply the stream's start time or altitude reference. -let events = build_event_stream( - &records, - &config, - Some(Rc::new(geomap)), - Some(noise_std), - None, // no magnetic map - None, - frequency, -)?; - -// Run EKF navigation. The geonav-specific `geo_closed_loop_*` drivers were removed in -// favour of the one driver in `strapdown-core`, which takes any `NavigationFilter`. -let results = run_closed_loop(&mut ekf, events, None, None)?; -``` - -## Performance Comparison: EKF vs UKF - -| Aspect | EKF | UKF | -|--------|-----|-----| -| **Computational Cost** | Lower (linear algebra only) | Higher (sigma point propagation) | -| **Memory Usage** | Lower | Higher (stores sigma points) | -| **Accuracy** | First-order approximation | Second-order approximation | -| **Nonlinearity Handling** | Good for mildly nonlinear systems | Better for highly nonlinear systems | -| **Deterministic** | Yes | Yes | -| **Implementation Complexity** | Requires Jacobian computation | Requires careful sigma point selection | - -### When to Use EKF - -- Computational resources are limited -- System dynamics are mildly nonlinear -- Jacobians can be computed efficiently -- Faster iteration cycles are needed for development - -### When to Use UKF - -- High accuracy is paramount -- System has significant nonlinearities -- Jacobians are difficult to compute or verify -- Computational resources are available - -## Testing - -The EKF geophysical navigation includes comprehensive tests: - -```bash -# Run all core EKF tests (31 tests) -cargo test --package strapdown-core --lib kalman - -# Run geophysical EKF tests (17 tests including Jacobian tests) -cargo test --package strapdown-geonav --lib -``` - -Key test coverage: -- EKF construction (9-state and 15-state) -- Predict step with various motion profiles -- Update step with different measurement types -- Geophysical measurement Jacobians -- Map gradient computation -- Angle wrapping and covariance reduction - -## References - -1. Groves, P. D. "Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd Edition", Chapter 14.2 -2. Bar-Shalom, Y., et al. "Estimation with Applications to Tracking and Navigation", Chapter 5 -3. Julier, S. & Uhlmann, J. "Unscented Filtering and Nonlinear Estimation" (for UKF comparison) - -## Future Enhancements - -Potential improvements to the EKF geophysical navigation: - -1. **Analytic Jacobians**: Compute analytic gradients for bilinear map interpolation -2. **Adaptive Process Noise**: Adjust Q based on motion dynamics -3. **Map Uncertainty**: Incorporate geophysical map uncertainty into R -4. **Multi-resolution Maps**: Support hierarchical map representations -5. **Terrain-aided Navigation**: Add terrain elevation as additional measurement -6. **Error-State Formulation**: Implement error-state EKF for improved numerical stability diff --git a/book/EKF_USAGE.md b/book/EKF_USAGE.md deleted file mode 100644 index b0649e11..00000000 --- a/book/EKF_USAGE.md +++ /dev/null @@ -1,419 +0,0 @@ -# Extended Kalman Filter (EKF) Usage Guide - -## Overview - -The Extended Kalman Filter (EKF) has been added to the strapdown-rs library as an alternative to the Unscented Kalman Filter (UKF). The EKF provides a linearized Gaussian approximation for navigation state estimation using analytic Jacobians for efficient uncertainty propagation. - -## Features - -- **9-state and 15-state configurations**: Support for navigation-only (9-state) or navigation + IMU biases (15-state) -- **Analytic Jacobians**: Uses pre-computed Jacobians from the `linearize` module for efficient computation -- **Multiple measurement types**: Supports GPS position, GPS velocity, combined position+velocity, and barometric altitude -- **Comprehensive testing**: 16 unit tests and 5 integration tests validate correctness -- **Well-documented**: Extensive LaTeX/KaTeX doccomments explain the mathematical foundations - -## API Usage - -### Basic Initialization - -```rust -use strapdown::kalman::{ExtendedKalmanFilter, InitialState, NavigationFilter}; -use strapdown::measurements::GPSPositionMeasurement; -use strapdown::IMUData; -use nalgebra::{DMatrix, DVector, Vector3}; - -// Define initial state -let initial_state = InitialState { - latitude: 45.0, - longitude: -122.0, - altitude: 100.0, - northward_velocity: 0.0, - eastward_velocity: 0.0, - vertical_velocity: 0.0, - roll: 0.0, - pitch: 0.0, - yaw: 0.0, - in_degrees: true, - is_enu: true, -}; - -// Initial covariance diagonal. -// -// Latitude and longitude are held in RADIANS and altitude in metres, so the three position -// entries are not in the same unit and cannot be filled from one literal. `vec![1e-6; 15]` -// -- what this example used to show -- reads as a 6.4 km initial horizontal uncertainty, not -// as a small number; see "Default Parameters" below and issue #308. Write the uncertainty in -// metres once and convert where it is used. -let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; // a 10 m GNSS fix -let mut initial_covariance = vec![ - horizontal_std_rad.powi(2), // latitude, rad^2 - horizontal_std_rad.powi(2), // longitude, rad^2 - 10.0_f64.powi(2), // altitude, m^2 -]; -initial_covariance.extend([0.25; 3]); // velocity, (m/s)^2 -- a 0.5 m/s fix -initial_covariance.extend([1e-4; 3]); // attitude, rad^2 -- ~0.6 deg -initial_covariance.extend([1e-3; 3]); // accel bias, (m/s^2)^2 -initial_covariance.extend([1e-8; 3]); // gyro bias, (rad/s)^2 - -// Initialize 15-state EKF with biases -let mut ekf = ExtendedKalmanFilter::new( - initial_state, - vec![0.0; 6], // IMU biases (3 accel + 3 gyro) - initial_covariance.clone(), // cloned so the 9-state example below can reuse it - // Process noise. The crate's own default, which is built the same way -- one metric - // constant converted once. `vec![1e-9; 15]` is a 201 m horizontal term. - DMatrix::from_diagonal(&DVector::from_vec( - strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY.to_vec(), - )), - true, // use_biases = true for 15-state -); - -// Predict step with IMU data -let imu_data = IMUData { - accel: Vector3::new(0.0, 0.0, 9.81), - gyro: Vector3::zeros(), -}; -ekf.predict(imu_data, 0.01); // dt = 0.01 seconds - -// Update step with GPS measurement -let gps_meas = GPSPositionMeasurement { - latitude: 45.0, - longitude: -122.0, - altitude: 100.0, - horizontal_noise_std: 5.0, - vertical_noise_std: 2.0, -}; -ekf.update(&gps_meas); - -// Get state estimate -let state = ekf.get_estimate(); -let covariance = ekf.get_certainty(); -``` - -### 9-State Configuration (No Biases) - -```rust -// The leading nine entries of the 15-state diagonal built above -- the same unit caveat -// applies, and for the same reason. -let initial_covariance_9 = initial_covariance[0..9].to_vec(); - -// Initialize 9-state EKF without biases -let mut ekf = ExtendedKalmanFilter::new( - initial_state, - vec![0.0; 6], // Biases ignored when use_biases = false - initial_covariance_9, // Initial covariance for 9 states - DMatrix::from_diagonal(&DVector::from_vec( - strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY[0..9].to_vec(), - )), // Process noise for 9 states - false, // use_biases = false for 9-state -); -``` - -### Using `initialize_ekf` Helper Function - -For simulation workflows, use the `initialize_ekf` helper: - -```rust -use strapdown::sim::{EkfConfig, initialize_ekf, TestDataRecord}; - -// Assuming you have TestDataRecord from sensor data -let initial_pose = TestDataRecord::default(); - -// `EkfConfig::default()` is the 15-state filter in NED, with the crate's default -// covariances, biases and process noise. Override only what you need. -let ekf = initialize_ekf(&initial_pose, EkfConfig::default()); - -// A Sensor Logger recording is ENU, so say so -- a `TestDataRecord` carries no frame -// tag, and declaring the wrong one is rejected rather than integrated at 2 g. -// `EkfConfig` is `#[non_exhaustive]`: outside `strapdown-core` it can be neither written as a -// struct literal nor updated with `..default()`, so it is built from `default()` and assigned. -let mut enu_config = EkfConfig::default(); -enu_config.is_enu = true; -let ekf_enu = initialize_ekf(&initial_pose, enu_config); -``` - -## Measurement Types - -The EKF supports all measurement types from the `measurements` module: - -### GPS Position -```rust -use strapdown::measurements::GPSPositionMeasurement; - -let meas = GPSPositionMeasurement { - latitude: 45.0, // degrees - longitude: -122.0, // degrees - altitude: 100.0, // meters - horizontal_noise_std: 5.0, // meters - vertical_noise_std: 2.0, // meters -}; -ekf.update(&meas); -``` - -### GPS Velocity -```rust -use strapdown::measurements::GPSVelocityMeasurement; - -let meas = GPSVelocityMeasurement { - northward_velocity: 10.0, // m/s - eastward_velocity: 5.0, // m/s - vertical_velocity: 0.0, // m/s - horizontal_noise_std: 0.5, // m/s - vertical_noise_std: 0.5, // m/s -}; -ekf.update(&meas); -``` - -### Combined GPS Position + Velocity -```rust -use strapdown::measurements::GPSPositionAndVelocityMeasurement; - -let meas = GPSPositionAndVelocityMeasurement { - latitude: 45.0, - longitude: -122.0, - altitude: 100.0, - northward_velocity: 10.0, - eastward_velocity: 5.0, - horizontal_noise_std: 5.0, - vertical_noise_std: 2.0, - velocity_noise_std: 0.5, -}; -ekf.update(&meas); -``` - -### Barometric Altitude -```rust -use strapdown::measurements::RelativeAltitudeMeasurement; - -let meas = RelativeAltitudeMeasurement { - relative_altitude: 5.0, // meters above reference - reference_altitude: 95.0, // meters (reference altitude) -}; -ekf.update(&meas); -``` - -## Command Line Usage (Simulator) - -The EKF is now fully integrated into the simulator and can be used via command line: - -```bash -# Run closed-loop simulation with EKF (linearized Jacobians) -strapdown-sim cl --filter ekf --input data.csv --output results.csv - -# Run with UKF (sigma point propagation) -strapdown-sim cl --filter ukf --input data.csv --output results.csv - -# Run with the ESKF (the default; omitting --filter selects it) -strapdown-sim cl --filter eskf --input data.csv --output results.csv - -# With a scenario config. `--config` supplies the whole run, so `--filter`, `--input` and -# `--output` are ignored alongside it -- set `closed_loop.filter`, `input` and `output` -# inside the file and invoke the config on its own. -strapdown-sim --config gnss_config.toml - -# View available options -strapdown-sim cl --help -``` - -The `--filter` option accepts `eskf`, `ukf` or `ekf`. Since #258 the default is `eskf`, the -15-state error-state filter: it estimates the IMU biases online rather than carrying whatever -turn-on bias the sensor happened to have, which the 9-state UKF and EKF cannot observe. - -## Performance Characteristics - -### Advantages of EKF -- **Computational efficiency**: No sigma point generation/propagation -- **Deterministic**: Produces identical results on repeated runs -- **Lower memory**: Smaller footprint than UKF -- **Well-understood**: Decades of theory and applications - -### Limitations -- **Linearization errors**: First-order Taylor approximation may introduce errors for highly nonlinear systems -- **Potential divergence**: Can diverge if linearization is poor or process noise underestimated -- **Gaussian assumption**: Like UKF, assumes Gaussian distributions - -### EKF vs UKF Comparison - -Based on integration tests with real data: -- **Similar accuracy**: EKF and UKF achieve comparable RMS horizontal errors (typically within 50% of each other) -- **Speed**: EKF is generally faster due to no sigma point propagation -- **Stability**: Both filters maintain bounded errors over extended periods -- **GNSS outages**: Both filters handle intermittent GNSS well with appropriate process noise tuning - -## Testing - -### Unit Tests -Run EKF-specific unit tests: -```bash -cargo test -p strapdown-core --lib kalman::tests::ekf -``` - -All 16 EKF unit tests cover: -- Construction (9-state and 15-state) -- Predict/update steps -- All measurement types -- Motion profiles (free fall, hover, horizontal motion) -- Covariance reduction -- Angle wrapping - -### Integration Tests -Run EKF integration tests: -```bash -cargo test -p strapdown-core --test integration_tests test_ekf -``` - -Integration tests include: -- End-to-end 15-state EKF simulation -- 9-state configuration verification -- EKF vs UKF performance comparison -- GNSS outage handling -- Bounded drift vs dead reckoning - -**Note**: Integration tests are marked `#[ignore]` as they require test data files. Remove the ignore attribute and provide data to run them. - -## Default Parameters - -### Process Noise -Default process noise diagonal (for 15-state). Note that the three position entries are -**not** in the same unit as one another: the filter holds latitude and longitude in radians -and altitude in metres, so a horizontal uncertainty written in metres has to be converted -before it can go on the diagonal. The crate does that once, from a single metric constant: - -```rust -// strapdown::sim -pub const POSITION_PROCESS_NOISE_M_PER_ROOT_S: f64 = 0.1; // metres per root-second - -const HORIZONTAL: f64 = { - let radians = POSITION_PROCESS_NOISE_M_PER_ROOT_S * strapdown::earth::METERS_TO_RADIANS; - radians * radians // 2.467e-16 rad^2 -}; - -const DEFAULT_PROCESS_NOISE_DENSITY: [f64; 15] = [ - HORIZONTAL, // latitude, rad^2 - HORIZONTAL, // longitude, rad^2 - 1e-2, // altitude, m^2 -- its own constant, see below - 1e-3, // velocity north - 1e-3, // velocity east - 1e-3, // velocity down - 1e-5, // roll - 1e-5, // pitch - 1e-5, // yaw - 1e-6, // accel bias x - 1e-6, // accel bias y - 1e-6, // accel bias z - 1e-8, // gyro bias x - 1e-8, // gyro bias y - 1e-8, // gyro bias z -]; -``` - -The altitude entry keeps its own constant, `VERTICAL_POSITION_PROCESS_NOISE_M2_PER_S`, rather -than being spelled as `POSITION_PROCESS_NOISE_M_PER_ROOT_S` squared -- though since #328 that is -exactly what it evaluates to, `1e-2 m^2/s`, a 10 cm standard deviation per root-second. - -It was `1e-4` until then. #308 deliberately left it alone, on the grounds -that a units fix is not the place to retune the vertical channel; #328 did the retune, on -evidence that it was the only one of three candidate knobs that moved three-sigma altitude -containment the right way. It is still short of where it should be -- see #372 -- so treat -`1e-2` as the current best value rather than a settled one. - -If you write the horizontal entries directly in rad^2, be aware what the numbers mean: -`1e-6 rad^2/s` is a **6.4 km** standard deviation per root-second, not a small number. Writing it -next to an altitude term of `1e-2 m^2` (10 cm) is the defect issue #308 fixed -- the filter -is told its own prediction is worthless, so it discards it and lands on each fix instead of -filtering, and innovation gating cannot function. `1e-9 rad^2` is the same mistake three -orders of magnitude smaller: a 201 m standard deviation. - -### Initial Covariance -Everything above is about $Q$, and every word of it applies to $P_0$: latitude and longitude -are radians there too. `1e-6` as an initial variance is a 6.4 km initial horizontal -uncertainty, which is how #303 first noticed the problem. - -Recommended initial covariance based on sensor accuracy. Convert metres to **radians**, not -degrees -- `METERS_TO_DEGREES` alone leaves the value 57.3x too large as a standard deviation -and 3283x in variance: - -- Position: `(horizontal_accuracy * METERS_TO_RADIANS)²` for latitude and longitude, - `vertical_accuracy²` for altitude -- Velocity: `(speed_accuracy)²` -- Attitude: `1e-9` (radians²) is what the `initialize_*` helpers use, which assumes the - record's own attitude is trusted. Seeding from a coarse alignment instead, use something - like the `1e-4` rad² (~0.6°) in the worked example above -- the two differ by five orders - of magnitude because they describe different situations, not because one is wrong -- Accelerometer and gyroscope biases: `1e-3`, all six entries - -`sim::initialize_ekf` and `sim::initialize_ukf` build the position and velocity blocks from a -`TestDataRecord`'s own reported accuracies and the rest from those defaults, and `sim::initialize_eskf` and -`engine::InsEngine` use `sim::DEFAULT_INITIAL_POSITION_UNCERTAINTY_M` (10 m) when there is no -record to read. For a derivation from an IMU grade rather than a hand-picked constant, see -`IMUQuality::auto_covariance`. - -## Mathematical Background - -The EKF implementation follows the standard formulation: - -### Predict Step -$$ -\begin{aligned} -\bar{x}_{k+1} &= f(x_k, u_k) \\ -\bar{P}_{k+1} &= F_k P_k F_k^T + G_k Q_k G_k^T -\end{aligned} -$$ - -where: -- $F_k$ is the state transition Jacobian computed using `linearize::state_transition_jacobian` -- $G_k$ is the process noise Jacobian computed using `linearize::process_noise_jacobian` -- $Q_k$ is the process noise covariance - -### Update Step -$$ -\begin{aligned} -K_k &= \bar{P}_k H_k^T (H_k \bar{P}_k H_k^T + R_k)^{-1} \\ -x_k &= \bar{x}_k + K_k (z_k - h(\bar{x}_k)) \\ -P_k &= (I - K_k H_k) \bar{P}_k -\end{aligned} -$$ - -where: -- $H_k$ is the measurement Jacobian (e.g., `linearize::gps_position_jacobian`) -- $R_k$ is the measurement noise covariance -- $K_k$ is the Kalman gain - -For detailed mathematical derivations, see: -- Groves, P. D. "Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd Edition", Chapter 14.2 -- In-code documentation with LaTeX/KaTeX equations - -## Troubleshooting - -### Filter Divergence -If the EKF diverges (unbounded errors): -1. Increase process noise (Q matrix) -2. Check IMU bias initialization -3. Verify coordinate frame consistency (ENU vs NED) -4. Ensure measurement noise is realistic - -### Poor Performance vs UKF -If EKF significantly underperforms UKF: -1. Check for highly nonlinear motion (e.g., aggressive maneuvers) -2. Verify Jacobian computations are appropriate for your scenario -3. Consider using UKF for highly nonlinear regimes - -### Numerical Issues -If experiencing numerical instability: -1. Covariance regularization is applied automatically (eps = 1e-9) -2. Joseph form covariance update is used for numerical stability -3. Robust SPD solver is used for Kalman gain computation - -## Future Work - -- CLI integration once particle filter stubs are resolved -- Example configurations and datasets -- Performance benchmarking suite -- Extended documentation with real-world examples -- Support for additional measurement types (pseudorange, carrier phase) - -## References - -1. Groves, P. D. (2013). "Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd Edition" -2. Bar-Shalom, Y., et al. (2001). "Estimation with Applications to Tracking and Navigation" -3. Simon, D. (2006). "Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches" diff --git a/book/GITHUB_PAGES_SETUP.md b/book/GITHUB_PAGES_SETUP.md deleted file mode 100644 index e86aaf2d..00000000 --- a/book/GITHUB_PAGES_SETUP.md +++ /dev/null @@ -1,305 +0,0 @@ -# GitHub Pages Setup Guide for Strapdown-rs - -This guide provides step-by-step instructions for setting up and deploying the Strapdown-rs documentation website using GitHub Pages and mdBook. - -## Overview - -The strapdown-rs project now includes a comprehensive documentation website built with mdBook. This guide covers: - -1. Local development and testing -2. GitHub Pages configuration -3. Automatic deployment via GitHub Actions -4. Custom domain setup - -## Project Structure - -``` -book/ -├── book.toml # mdBook configuration -├── custom.css # Custom styling -├── src/ # Documentation source files -│ ├── SUMMARY.md # Table of contents -│ ├── introduction.md -│ ├── quick-start.md -│ ├── installation/ -│ ├── user-guide/ -│ ├── filters/ -│ ├── geonav/ -│ ├── gnss/ -│ ├── api/ -│ ├── examples/ -│ ├── development/ -│ ├── resources/ -│ └── faq.md -└── book/ # Generated HTML (gitignored) -``` - -## Local Development - -### Prerequisites - -1. **Install Rust and Cargo** (if not already installed): - ```bash - curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh - ``` - -2. **Install mdBook**: - ```bash - cargo install mdbook - ``` - -### Building and Serving Locally - -1. **Build the book**: - ```bash - cd book - mdbook build - ``` - The generated HTML will be in `book/book/`. - -2. **Serve the book locally** (with live reload): - ```bash - cd book - mdbook serve - ``` - Open http://localhost:3000 in your browser. - -3. **Watch for changes** (automatic rebuild): - ```bash - cd book - mdbook watch - ``` - -### Adding New Content - -1. **Create a new markdown file** in the appropriate directory under `book/src/`. - -2. **Add the file to SUMMARY.md**: - ```markdown - # Summary - - - [New Chapter](./path/to/new-chapter.md) - ``` - -3. **Build and test** locally before committing. - -## GitHub Pages Configuration - -### Step 1: Enable GitHub Pages - -1. Go to your repository on GitHub: https://github.com/jbrodovsky/strapdown-rs - -2. Click **Settings** → **Pages** - -3. Under "Build and deployment": - - **Source**: Select "GitHub Actions" - - This enables the workflow-based deployment - -4. Save the settings. - -### Step 2: Verify Workflow Permissions - -1. Go to **Settings** → **Actions** → **General** - -2. Under "Workflow permissions": - - Select "Read and write permissions" - - Check "Allow GitHub Actions to create and approve pull requests" - -3. Save the settings. - -### Step 3: Deploy - -The GitHub Actions workflow (`.github/workflows/deploy-book.yml`) automatically: -- Builds the book when you push to `main` -- Deploys it to GitHub Pages -- Makes it available at: https://jbrodovsky.github.io/strapdown-rs/ - -**First deployment**: After merging the PR, the site should be live within 2-5 minutes. - -## Custom Domain Setup - -To use your custom domain (www.strapdown.rs): - -### Step 1: Configure DNS - -Add these DNS records with your domain registrar: - -**For apex domain (strapdown.rs)**: -``` -Type: A -Name: @ -Value: 185.199.108.153 - 185.199.109.153 - 185.199.110.153 - 185.199.111.153 -``` - -**For www subdomain (www.strapdown.rs)**: -``` -Type: CNAME -Name: www -Value: jbrodovsky.github.io -``` - -### Step 2: Configure GitHub Pages - -1. Go to **Settings** → **Pages** - -2. Under "Custom domain": - - Enter: `www.strapdown.rs` - - Click **Save** - -3. Check **Enforce HTTPS** (wait a few minutes for the certificate) - -### Step 3: Update book.toml - -The `book.toml` already includes: -```toml -[output.html] -cname = "www.strapdown.rs" -site-url = "/" -``` - -This creates a `CNAME` file in the build output. - -### Step 4: Verify - -After DNS propagation (can take up to 48 hours but usually minutes): -- Visit https://www.strapdown.rs -- Verify HTTPS works -- Check that redirects work correctly - -## Workflow Details - -The GitHub Actions workflow (`.github/workflows/deploy-book.yml`) performs these steps: - -1. **Checkout**: Gets the repository code -2. **Setup Rust**: Installs the Rust toolchain -3. **Install mdBook**: Installs mdBook via cargo -4. **Build**: Runs `mdbook build` in the `book/` directory -5. **Upload**: Uploads the build artifact -6. **Deploy**: Deploys to GitHub Pages (only on `main` branch) - -### Triggering Deployment - -Deployment happens automatically when: -- You push to the `main` branch -- A PR is merged into `main` -- You manually trigger via "Run workflow" in the Actions tab - -### Monitoring Deployment - -1. Go to **Actions** tab in GitHub -2. Click on the latest "Deploy mdBook to GitHub Pages" workflow -3. Check the build and deploy steps -4. If successful, visit the site URL - -## Troubleshooting - -### Build Fails Locally - -**Error**: "command not found: mdbook" -- **Solution**: Install mdBook: `cargo install mdbook` - -**Error**: "Invalid configuration file" -- **Solution**: Check `book.toml` syntax with `mdbook build --verbose` - -### Build Fails on GitHub - -**Error**: Permission denied -- **Solution**: Check workflow permissions in Settings → Actions - -**Error**: mdBook installation fails -- **Solution**: The workflow should install mdBook automatically. Check the workflow logs. - -### Site Not Updating - -1. **Check workflow status**: Actions tab should show successful deployment -2. **Clear browser cache**: Use Ctrl+Shift+R or Cmd+Shift+R -3. **Wait for propagation**: Can take 1-2 minutes after deployment -4. **Check GitHub Pages settings**: Ensure source is set to "GitHub Actions" - -### Custom Domain Not Working - -1. **Verify DNS records**: Use `dig www.strapdown.rs` or https://dnschecker.org -2. **Check CNAME file**: Should be present in the deployed site -3. **Wait for DNS propagation**: Can take up to 48 hours -4. **Check HTTPS**: May need time to provision certificate - -## Customization - -### Styling - -Edit `book/custom.css` to customize the appearance: -- Colors and themes -- Typography -- Layout and spacing -- Mobile responsiveness - -### Configuration - -Edit `book/book.toml` to change: -- Site title and description -- Theme preferences -- Search settings -- MathJax support -- Git integration - -### Theme - -mdBook includes several built-in themes: -- Light (default) -- Navy -- Coal -- Rust -- Ayu - -Users can switch themes using the theme selector in the top menu. - -## Maintenance - -### Updating Content - -1. Edit markdown files in `book/src/` -2. Test locally with `mdbook serve` -3. Commit and push to `main` -4. GitHub Actions automatically deploys - -### Updating mdBook - -To update to the latest mdBook version: - -```bash -cargo install mdbook --force -``` - -Update the version in `.github/workflows/deploy-book.yml` if needed. - -### Monitoring - -- Check GitHub Actions for build/deployment status -- Monitor GitHub Pages analytics (if enabled) -- Review issues/feedback from users - -## Best Practices - -1. **Always test locally** before pushing to main -2. **Use descriptive commit messages** for documentation changes -3. **Keep navigation organized** in SUMMARY.md -4. **Cross-reference pages** using relative links -5. **Include code examples** where appropriate -6. **Update the table of contents** when adding pages -7. **Use proper markdown formatting** for consistency - -## Additional Resources - -- [mdBook Documentation](https://rust-lang.github.io/mdBook/) -- [GitHub Pages Documentation](https://docs.github.com/en/pages) -- [GitHub Actions Documentation](https://docs.github.com/en/actions) -- [Markdown Guide](https://www.markdownguide.org/) - -## Support - -For issues or questions: -- Open an issue on [GitHub](https://github.com/jbrodovsky/strapdown-rs/issues) -- Contact the maintainer: jbrodovsky@temple.edu diff --git a/book/PROJECT_SUMMARY.md b/book/PROJECT_SUMMARY.md deleted file mode 100644 index 94751597..00000000 --- a/book/PROJECT_SUMMARY.md +++ /dev/null @@ -1,346 +0,0 @@ -# Strapdown-rs Documentation Website - Implementation Summary - -## Overview - -A comprehensive GitHub Pages website has been successfully developed for the strapdown-rs project using mdBook, a modern documentation tool built with Rust. The website provides easy-to-navigate documentation, tutorials, API references, and usage instructions. - -## What Has Been Implemented - -### 1. Documentation Structure - -A complete book structure with the following sections: - -- **Introduction**: Project overview, features, and getting started -- **Installation**: System requirements, dependencies, and installation methods -- **Quick Start**: Fast-track guide to running first simulations -- **User Guide**: Comprehensive usage documentation - - Core concepts and theory - - Simulation modes (open-loop, closed-loop, particle filter) - - Data formats and configuration - - Logging and debugging -- **Navigation Filters**: Detailed filter documentation - - Extended Kalman Filter (EKF) - - Unscented Kalman Filter (UKF) - - Particle Filters and RBPF - - Comparison and selection guide -- **Geophysical Navigation**: Gravity and magnetic anomaly navigation -- **GNSS Degradation**: Fault simulation scenarios -- **API Reference**: Module-level documentation structure -- **Examples**: Tutorials and configuration examples -- **Development**: Contributing guidelines and architecture -- **FAQ**: Common questions and troubleshooting -- **Resources**: Publications, links, and glossary - -### 2. Technical Implementation - -#### mdBook Configuration (`book/book.toml`) - -Configured with: -- MathJax support for mathematical equations -- Custom CSS for improved styling -- GitHub repository integration -- Edit links to source files -- Search functionality -- Responsive design -- Custom domain support (www.strapdown.rs) - -#### Custom Styling (`book/custom.css`) - -Custom CSS providing: -- Improved code block styling -- Warning/info/note boxes -- Better mobile responsiveness -- Enhanced table formatting -- Consistent link styling - -#### GitHub Actions Workflow (`.github/workflows/deploy-book.yml`) - -Automated deployment workflow that: -- Builds the book on every push to main -- Installs mdBook automatically -- Deploys to GitHub Pages -- Supports manual triggering -- Uses proper GitHub Pages deployment action - -### 3. Key Content Pages Created - -The following pages have substantial content: - -1. **Introduction** (`book/src/introduction.md`) - - Project overview - - Key features - - Target audience - - Getting help - -2. **Quick Start** (`book/src/quick-start.md`) - - Installation commands - - First simulation examples - - Configuration file usage - - Next steps - -3. **Installation Guide** (`book/src/installation/installation.md`) - - System requirements - - Installation methods - - Platform-specific instructions - - Troubleshooting - -4. **System Requirements** (`book/src/installation/requirements.md`) - - Hardware requirements - - Software requirements - - Platform notes - - Development tools - -5. **User Guide Overview** (`book/src/user-guide/overview.md`) - - Guide structure - - Simulation modes - - State models - - Common workflows - -6. **EKF Documentation** (`book/src/filters/ekf.md`) - - Mathematical foundation - - Usage examples - - Configuration guide - - Performance characteristics - -7. **FAQ** (`book/src/faq.md`) - - General questions - - Installation help - - Usage guidance - - Troubleshooting - -### 4. Setup Documentation - -#### GITHUB_PAGES_SETUP.md - -Comprehensive guide covering: -- Local development workflow -- GitHub Pages configuration -- Custom domain setup (DNS, CNAME) -- Workflow details and monitoring -- Troubleshooting common issues -- Maintenance and best practices - -## File Structure - -``` -book/ -├── book.toml # mdBook configuration -├── custom.css # Custom styling -├── .gitignore # Ignore build output -└── src/ - ├── SUMMARY.md # Table of contents - ├── introduction.md # Main introduction - ├── quick-start.md # Quick start guide - ├── faq.md # FAQ - ├── installation/ # Installation docs - ├── user-guide/ # User guide pages - ├── filters/ # Filter documentation - ├── geonav/ # Geophysical navigation - ├── gnss/ # GNSS scenarios - ├── api/ # API reference - ├── examples/ # Tutorials - ├── development/ # Contributing - └── resources/ # Additional resources -``` - -## How to Use - -### Local Development - -1. Install mdBook: - ```bash - cargo install mdbook - ``` - -2. Build and serve locally: - ```bash - cd book - mdbook serve - ``` - -3. View at http://localhost:3000 - -### Adding Content - -1. Create or edit markdown files in `book/src/` -2. Update `SUMMARY.md` if adding new pages -3. Build locally to test -4. Commit and push to trigger deployment - -### Deployment - -**Automatic**: Push to `main` branch triggers GitHub Actions workflow - -**Manual**: -1. Go to Actions tab on GitHub -2. Select "Deploy mdBook to GitHub Pages" -3. Click "Run workflow" - -## GitHub Pages Setup Required - -After merging this PR, you'll need to: - -1. **Enable GitHub Pages**: - - Go to Settings → Pages - - Set source to "GitHub Actions" - - Save - -2. **Verify Permissions**: - - Settings → Actions → General - - Enable "Read and write permissions" - -3. **Wait for Deployment**: - - First deployment takes 2-5 minutes - - Site will be available at: https://jbrodovsky.github.io/strapdown-rs/ - -4. **Optional - Custom Domain**: - - Configure DNS records (see GITHUB_PAGES_SETUP.md) - - Add domain in Settings → Pages - - Enable HTTPS - -## Features - -### For Users - -- **Easy Navigation**: Sidebar with hierarchical structure -- **Search**: Full-text search across all pages -- **Responsive**: Works on desktop and mobile -- **Dark Mode**: Built-in theme switcher -- **Printable**: Print-friendly styling -- **MathJax**: Mathematical equation rendering -- **Code Highlighting**: Syntax highlighting for Rust and other languages - -### For Maintainers - -- **Automatic Deployment**: Push to main = instant updates -- **Edit Links**: Each page has "Edit on GitHub" link -- **Version Control**: All docs in Git -- **Easy Updates**: Markdown-based authoring -- **No Build Dependencies**: GitHub Actions handles everything - -## What's Next - -### Content to Expand - -Many pages are currently placeholders. Priority areas to fill: - -1. **API Reference**: Detailed module documentation -2. **Tutorials**: Step-by-step examples -3. **Core Concepts**: INS theory and implementation -4. **Examples**: More configuration examples -5. **Measurement Models**: Detailed measurement documentation - -### Enhancements - -Possible future improvements: - -1. **Search Optimization**: Tune search indexing -2. **More Examples**: Add code examples to more pages -3. **Diagrams**: Add visual diagrams and flowcharts -4. **Videos**: Embed tutorial videos -5. **Interactive Demos**: WebAssembly demos - -## Integration with Existing Docs - -The website complements existing documentation: - -- **docs/**: Technical documentation (can be moved to book) -- **README.md**: Updated with link to full documentation -- **API Docs**: docs.rs remains the Rust API reference -- **Examples**: Configuration examples remain in `examples/` - -## Custom Domain Setup - -The book is pre-configured for www.strapdown.rs: - -1. **DNS Records**: - - A records pointing to GitHub Pages IPs - - CNAME for www subdomain - -2. **Configuration**: - - `cname = "www.strapdown.rs"` in book.toml - - Automatic CNAME file generation - -3. **Verification**: - - Visit https://www.strapdown.rs after DNS propagation - - HTTPS automatically provisioned - -See GITHUB_PAGES_SETUP.md for detailed DNS instructions. - -## Maintenance - -### Regular Updates - -1. Keep content current with code changes -2. Update examples when APIs change -3. Add new sections as features are added -4. Review and address user feedback - -### Monitoring - -1. Check GitHub Actions for build failures -2. Review GitHub Pages analytics (if enabled) -3. Monitor issues for documentation questions -4. Keep mdBook updated - -### Contributing - -Users can contribute documentation: - -1. Fork the repository -2. Edit files in `book/src/` -3. Submit pull request -4. Preview builds automatically - -## Benefits - -### For the Project - -- **Professional presentation** -- **Better discoverability** -- **Reduced support burden** (better docs = fewer questions) -- **Easier onboarding** for new users -- **Research credibility** (important for academic projects) - -### For Users - -- **Centralized documentation** -- **Easy to search and navigate** -- **Always up-to-date** -- **Mobile-friendly** -- **Offline capable** (can download HTML) - -## Conclusion - -A complete, production-ready documentation website has been implemented for strapdown-rs. The site is: - -- ✅ Built with modern tools (mdBook) -- ✅ Automatically deployed via GitHub Actions -- ✅ Well-structured with comprehensive content outline -- ✅ Styled for readability and professionalism -- ✅ Configured for custom domain -- ✅ Ready for immediate use -- ✅ Easy to maintain and extend - -The foundation is solid. Content can be expanded incrementally as the project evolves. The setup guide ensures that anyone can contribute or maintain the documentation going forward. - -## Files Changed - -### Created: -- `book/` - Complete mdBook project -- `.github/workflows/deploy-book.yml` - Deployment workflow -- `GITHUB_PAGES_SETUP.md` - Setup instructions -- `PROJECT_SUMMARY.md` - This file - -### Modified: -- `.gitignore` - Added book output exclusion -- `README.md` - Updated documentation link - -### Total: -- 60+ markdown files -- 1 GitHub Actions workflow -- 2 configuration files -- 1 CSS stylesheet -- 2 guide documents - -All ready for immediate deployment upon merge! diff --git a/book/QUICK_REFERENCE.md b/book/QUICK_REFERENCE.md deleted file mode 100644 index f262a65e..00000000 --- a/book/QUICK_REFERENCE.md +++ /dev/null @@ -1,166 +0,0 @@ -# Quick Reference - GitHub Pages Deployment - -## After Merge: Immediate Next Steps - -### 1. Enable GitHub Pages (2 minutes) - -1. Go to https://github.com/jbrodovsky/strapdown-rs/settings/pages -2. Under "Build and deployment": - - **Source**: Select "GitHub Actions" -3. Click **Save** - -### 2. Verify Permissions (1 minute) - -1. Go to https://github.com/jbrodovsky/strapdown-rs/settings/actions -2. Under "Workflow permissions": - - Select **"Read and write permissions"** - - Check **"Allow GitHub Actions to create and approve pull requests"** -3. Click **Save** - -### 3. Trigger First Deployment (automatic) - -The merge to `main` will automatically trigger the deployment workflow. - -Or manually trigger: -1. Go to https://github.com/jbrodovsky/strapdown-rs/actions -2. Click "Deploy mdBook to GitHub Pages" -3. Click "Run workflow" → "Run workflow" - -### 4. Verify Deployment (2-5 minutes) - -1. Wait for the workflow to complete (green checkmark) -2. Visit: https://jbrodovsky.github.io/strapdown-rs/ -3. Verify the site loads correctly - -## Custom Domain Setup (Optional) - -### DNS Configuration - -Add these records to your domain registrar: - -**For strapdown.rs:** -``` -Type: A -Name: @ -Values: - 185.199.108.153 - 185.199.109.153 - 185.199.110.153 - 185.199.111.153 -``` - -**For www.strapdown.rs:** -``` -Type: CNAME -Name: www -Value: jbrodovsky.github.io -``` - -### GitHub Configuration - -1. Go to https://github.com/jbrodovsky/strapdown-rs/settings/pages -2. Under "Custom domain": - - Enter: `www.strapdown.rs` - - Click **Save** -3. Wait a few minutes, then check **"Enforce HTTPS"** - -### Verify - -After DNS propagation (up to 48 hours, usually minutes): -- Visit https://www.strapdown.rs -- Verify HTTPS works -- Check redirect from http:// to https:// - -## Quick Commands - -### Build locally: -```bash -cd book -mdbook build -``` - -### Serve locally (with live reload): -```bash -cd book -mdbook serve -``` - -### Check for errors: -```bash -cd book -mdbook build --verbose -``` - -## File Structure - -``` -book/ -├── book.toml # Configuration -├── custom.css # Styling -└── src/ - ├── SUMMARY.md # Table of contents (edit to add pages) - └── *.md # Content pages -``` - -## Common Tasks - -### Add a new page: -1. Create `book/src/path/to/page.md` -2. Add to `book/src/SUMMARY.md`: - ```markdown - - [Page Title](./path/to/page.md) - ``` -3. Build and test locally -4. Commit and push - -### Update existing page: -1. Edit the markdown file -2. Test locally with `mdbook serve` -3. Commit and push -4. Deployment happens automatically - -### Change styling: -1. Edit `book/custom.css` -2. Test locally -3. Commit and push - -## Monitoring - -### Check deployment status: -https://github.com/jbrodovsky/strapdown-rs/actions - -### View deployed site: -https://jbrodovsky.github.io/strapdown-rs/ - -### Check workflow logs: -Actions tab → Latest "Deploy mdBook to GitHub Pages" run - -## Troubleshooting - -### Build fails: -```bash -cd book -mdbook build --verbose -``` -Check error messages, fix markdown syntax - -### Site not updating: -1. Check GitHub Actions (should show green checkmark) -2. Clear browser cache (Ctrl+Shift+R) -3. Wait 1-2 minutes for CDN propagation - -### Custom domain not working: -1. Verify DNS: `dig www.strapdown.rs` -2. Check CNAME file is present in deployment -3. Wait for DNS propagation (up to 48 hours) - -## Support - -- **Setup Guide**: See GITHUB_PAGES_SETUP.md for detailed instructions -- **Project Summary**: See PROJECT_SUMMARY.md for implementation details -- **mdBook Docs**: https://rust-lang.github.io/mdBook/ -- **GitHub Pages**: https://docs.github.com/en/pages - -## Contact - -Questions? Open an issue or contact: jbrodovsky@temple.edu diff --git a/book/RBPF.md b/book/RBPF.md deleted file mode 100644 index 4cfa1c58..00000000 --- a/book/RBPF.md +++ /dev/null @@ -1,715 +0,0 @@ -# Rao-Blackwellized Particle Filter Implementation - -## Overview - -The Rao-Blackwellized Particle Filter (RBPF) partitions the 15-state INS into nonlinear states (position: 3) estimated via particles and conditionally linear states (velocity: 3, attitude: 3, biases: 6) estimated via per-particle **EKF** filters. This reduces computational burden while improving bias estimation and maintaining the existing 2.5D vertical channel approach. - -**Note:** The current implementation uses an **Extended Kalman Filter (EKF)** instead of an Unscented Kalman Filter (UKF) for improved computational efficiency. The EKF uses Jacobian-based linearization which is faster than the UKF's sigma point approach, providing approximately 3-5x speedup while maintaining acceptable accuracy for the conditionally-linear states. - -## State Partitioning - -**Nonlinear states (particles):** -- Latitude (rad) -- Longitude (rad) -- Altitude (m) - -**Linear/Conditionally-Gaussian states (per-particle EKF):** -- Velocity: v_north, v_east, v_vertical (m/s) -- Attitude: roll, pitch, yaw (rad) -- Accelerometer biases: b_ax, b_ay, b_az (m/s²) -- Gyroscope biases: b_gx, b_gy, b_gz (rad/s) - -## Implementation Steps - -### Step 1: Create new structs for RBPF state representation - -**Location:** `core/src/particle.rs` - -#### 1.1 Define `RBParticle` struct - -```rust -pub struct RBParticle { - /// Position states: [latitude (rad), longitude (rad), altitude (m)] - pub position: Vector3, - - /// Per-particle UKF for linear states (9-state: velocity, attitude, biases) - pub kalman_filter: PerParticleUKF, - - /// Importance weight (unnormalized likelihood) - pub weight: f64, -} -``` - -**Methods to implement:** -- `to_state_vector(&self) -> DVector` - Convert to 15-state vector for compatibility -- `from_state_vector(state: &DVector) -> Self` - Create from state vector -- `get_position(&self) -> Vector3` - Access position -- `get_linear_states(&self) -> DVector` - Access velocity, attitude, biases from UKF - -#### 1.2 Define `PerParticleEKF` struct - -```rust -pub struct PerParticleEKF { - /// Mean state: [v_n, v_e, v_v, roll, pitch, yaw, b_ax, b_ay, b_az, b_gx, b_gy, b_gz] - pub mean_state: DVector, - - /// Covariance matrix (12×12) - pub covariance: DMatrix, -} -``` - -**Methods to implement:** -- `new(initial_state: DVector, initial_cov: DMatrix) -> Self` -- `predict(&mut self, position: &Vector3, imu_data: IMUData, process_noise: &DMatrix, dt: f64, is_enu: bool)` - EKF predict step conditioned on particle position, uses Jacobian computation -- `update(&mut self, position: &Vector3, measurement: &M) -> f64` - EKF update, returns marginal likelihood for particle weighting -- `get_estimate(&self) -> DVector` - Return mean state -- `get_covariance(&self) -> DMatrix` - Return covariance -- `compute_state_transition_jacobian(...)` - Compute F matrix for linearization -- `compute_measurement_jacobian(...)` - Compute H matrix for measurement update - -#### 1.3 Define `RBProcessNoise` struct - -```rust -pub struct RBProcessNoise { - /// Position noise standard deviations [σ_lat, σ_lon, σ_h] (rad, rad, m) - pub position_std: Vector3, - - /// Linear state noise covariance matrix (9×9) for per-particle UKF - /// Includes: velocity (3), attitude (3), accel biases (3), gyro biases (3) - pub linear_states_covariance: DMatrix, -} -``` - -**Default implementation:** -```rust -impl Default for RBProcessNoise { - fn default() -> Self { - // Position noise (for particle diffusion) - let position_std = Vector3::new(1e-3, 1e-3, 5e-2); - - // Linear state noise (for UKF predict) - let linear_noise_diag = vec![ - 1e-2, 1e-2, 1e-1, // velocity (2.5D: large v_v) - 1e-3, 1e-3, 1e-3, // attitude - 1e-3, 1e-3, 1e-3, // accel biases - 1e-4, 1e-4, 1e-4, // gyro biases - ]; - let linear_states_covariance = DMatrix::from_diagonal(&DVector::from_vec(linear_noise_diag)); - - RBProcessNoise { - position_std, - linear_states_covariance, - } - } -} -``` - -### Step 2: Implement `RaoBlackwellizedParticleFilter` struct - -**Location:** `core/src/particle.rs` - -#### 2.1 Main struct definition - -```rust -pub struct RaoBlackwellizedParticleFilter { - /// Ensemble of RB particles - particles: Vec, - - /// Number of particles - num_particles: usize, - - /// Process noise parameters - process_noise: RBProcessNoise, - - /// Vertical channel mode (2.5D simplified) - vertical_channel_mode: VerticalChannelMode, - - /// Resampling strategy - resampling_strategy: ResamplingStrategy, - - /// Effective particle threshold for resampling - effective_particle_threshold: f64, - - /// Random number generator (seeded) - rng: StdRng, - - /// Coordinate frame flag - is_enu: bool, - - /// UKF parameters for per-particle filters - ukf_alpha: f64, - ukf_beta: f64, - ukf_kappa: f64, -} -``` - -#### 2.2 Constructor - -```rust -impl RaoBlackwellizedParticleFilter { - pub fn new( - initial_state: InitialState, - imu_biases: Vec, - covariance_diagonal: Vec, // 15-element: 3 position + 9 linear + 3 biases - process_noise: RBProcessNoise, - num_particles: usize, - vertical_mode: VerticalChannelMode, - resampling_strategy: ResamplingStrategy, - ukf_alpha: f64, - ukf_beta: f64, - ukf_kappa: f64, - seed: Option, - ) -> Self { - // 1. Initialize RNG - // 2. Sample initial positions from Gaussian (first 3 elements of covariance_diagonal) - // 3. Initialize identical per-particle UKFs with velocity, attitude, biases - // (elements 3-14 of covariance_diagonal) - // 4. Set uniform weights - // ... - } -} -``` - -#### 2.3 Implement `NavigationFilter` trait - -```rust -impl NavigationFilter for RaoBlackwellizedParticleFilter { - fn predict(&mut self, imu_data: IMUData, dt: f64) { - // For each particle: - for particle in &mut self.particles { - // 1. Get current velocity and attitude from UKF - let linear_states = particle.kalman_filter.get_estimate(); - let velocity = linear_states.fixed_rows::<3>(0); - let attitude = linear_states.fixed_rows::<3>(3); - - // 2. Propagate position using forward_2_5d_rbpf (attitude, velocity given) - forward_2_5d_rbpf( - &mut particle.position, - &velocity, - &attitude, - dt - ); - - // 3. Add process noise to position - add_position_noise(&mut particle.position, &self.process_noise.position_std, dt, &mut self.rng); - - // 4. UKF predict for linear states (conditioned on new position) - let biases = linear_states.fixed_rows::<6>(3); - let bias_corrected_imu = IMUData { - accel: imu_data.accel - biases.fixed_rows::<3>(0), - gyro: imu_data.gyro - biases.fixed_rows::<3>(3), - }; - particle.kalman_filter.predict( - &particle.position, - bias_corrected_imu, - &self.process_noise.linear_states_covariance, - dt - ); - } - } - - fn update(&mut self, measurement: &M) { - // 1. Compute marginal likelihood for each particle - for particle in &mut self.particles { - // UKF update returns marginal likelihood p(z|position) - let log_likelihood = particle.kalman_filter.update( - &particle.position, - measurement - ); - particle.weight *= log_likelihood.exp(); - } - - // 2. Normalize weights - self.normalize_weights(); - - // 3. Check ESS and resample if needed - let ess = self.effective_particle_count(); - if ess < self.effective_particle_threshold { - self.resample(); - } - } - - fn get_estimate(&self) -> DVector { - // 1. Weighted average of positions - let mean_position = self.weighted_position_mean(); - - // 2. Weighted average of per-particle UKF means - let mean_linear_states = self.weighted_linear_states_mean(); - - // 3. Concatenate into 15-state vector - let mut state = DVector::zeros(15); - state.fixed_rows_mut::<3>(0).copy_from(&mean_position); - state.fixed_rows_mut::<12>(3).copy_from(&mean_linear_states); - state - } - - fn get_certainty(&self) -> DMatrix { - // Combined covariance from: - // 1. Particle position covariance (empirical from weighted samples) - // 2. Weighted average of per-particle UKF covariances - // 3. Cross terms (position-linear states correlation) - self.compute_combined_covariance() - } -} -``` - -### Step 3: Add RBPF-specific propagation functions - -**Location:** `core/src/particle.rs` - -#### 3.1 Position propagation function - -```rust -/// Propagate position states for RBPF given velocity and attitude from UKF. -/// -/// This is the position-only component of 2.5D navigation. The velocity and -/// attitude are provided as inputs (from per-particle UKF) rather than being -/// propagated together with position. -/// -/// # Arguments -/// * `position` - Mutable reference to position [lat, lon, alt] -/// * `velocity` - Velocity vector [v_n, v_e, v_v] from UKF -/// * `attitude` - Attitude angles [roll, pitch, yaw] from UKF -/// * `dt` - Time step -pub fn forward_2_5d_rbpf( - position: &mut Vector3, - velocity: &Vector3, - attitude: &Vector3, - dt: f64, -) { - // 1. Update horizontal position (lat, lon) using geodetic equations - // Similar to position_update() but only using horizontal velocities - - // 2. Update altitude using vertical velocity (2.5D: simple integration) - // alt(t+dt) = alt(t) + v_v * dt - - // Uses earth model and geodetic calculations from crate::earth -} -``` - -#### 3.2 UKF predict with position conditioning - -```rust -impl PerParticleUKF { - /// UKF predict step conditioned on particle position. - /// - /// Propagates the 9-state linear system: velocity, attitude, biases. - /// The nonlinear coupling with position comes through gravity, Earth rate, - /// and transport rate terms which are evaluated at the given position. - pub fn predict( - &mut self, - position: &Vector3, // Given by particle - imu_data: IMUData, - process_noise: &DMatrix, - dt: f64, - ) { - // 1. Generate sigma points for 9-state UKF - let sigma_points = self.get_sigma_points(); - - // 2. Propagate each sigma point - // - Velocity: use velocity_update_horizontal and vertical velocity dynamics - // - Attitude: use attitude_update with gyro measurements - // - Biases: random walk - let propagated_sigma_points = self.propagate_sigma_points( - &sigma_points, - position, // Earth parameters evaluated here - imu_data, - dt - ); - - // 3. Compute predicted mean and covariance - let mut mean_predicted = DVector::zeros(9); - for (i, sigma_point) in propagated_sigma_points.column_iter().enumerate() { - mean_predicted += self.weights_mean[i] * sigma_point; - } - - let mut cov_predicted = DMatrix::zeros(9, 9); - for (i, sigma_point) in propagated_sigma_points.column_iter().enumerate() { - let diff = sigma_point - &mean_predicted; - cov_predicted += self.weights_cov[i] * (&diff * diff.transpose()); - } - cov_predicted += process_noise; - - self.mean_state = mean_predicted; - self.covariance = symmetrize(&cov_predicted); - } -} -``` - -#### 3.3 UKF update with marginal likelihood - -```rust -impl PerParticleUKF { - /// UKF update step, returns marginal log-likelihood for particle weighting. - /// - /// Performs standard UKF measurement update on the linear states and - /// computes the marginal likelihood p(z | position) which is used to - /// update the particle weight. - pub fn update( - &mut self, - position: &Vector3, - measurement: &M, - ) -> f64 { - // 1. Generate measurement sigma points - // 2. Compute predicted measurement and innovation covariance - // 3. Compute Kalman gain - // 4. Update mean and covariance - // 5. Compute marginal likelihood: - // log p(z|position) = -0.5 * (innovation^T * S^-1 * innovation + log|S| + k*log(2π)) - // where S is the innovation covariance - - // ... (Standard UKF update equations) - - // Return log-likelihood for particle weighting - let log_likelihood = -0.5 * ( - innovation.dot(&s_inv_innovation) + - s.determinant().ln() + - innovation.len() as f64 * (2.0 * std::f64::consts::PI).ln() - ); - - log_likelihood - } -} -``` - -### Step 4: Extend sim.rs with initialization helper - -**Location:** `core/src/sim.rs` - -```rust -/// Initialize a Rao-Blackwellized particle filter for simulation. -/// -/// This function creates and initializes a `RaoBlackwellizedParticleFilter` with the -/// given parameters, using UKF for per-particle linear state estimation. -/// -/// # Arguments -/// -/// * `initial_pose` - A `TestDataRecord` containing the initial pose information. -/// * `num_particles` - Number of particles in the ensemble (recommend 50-200 for RBPF). -/// * `vertical_mode` - Vertical channel mode (Simplified for 2.5D). -/// * `attitude_covariance` - Optional initial attitude covariance. -/// * `imu_biases` - Optional initial IMU biases. -/// * `imu_biases_covariance` - Optional IMU bias covariance. -/// * `process_noise` - Optional `RBProcessNoise` struct. -/// * `resampling_strategy` - Resampling algorithm (default: Systematic). -/// * `ukf_alpha` - UKF spread parameter (default: 1e-3). -/// * `ukf_beta` - UKF distribution parameter (default: 2.0). -/// * `ukf_kappa` - UKF secondary scaling (default: 0.0). -/// * `seed` - Optional random seed for reproducibility. -/// -/// # Returns -/// -/// * `RaoBlackwellizedParticleFilter` - Initialized RBPF instance. -#[allow(clippy::too_many_arguments)] -pub fn initialize_rbpf( - initial_pose: &TestDataRecord, - num_particles: usize, - vertical_mode: VerticalChannelMode, - attitude_covariance: Option>, - imu_biases: Option>, - imu_biases_covariance: Option>, - process_noise: Option, - resampling_strategy: Option, - ukf_alpha: Option, - ukf_beta: Option, - ukf_kappa: Option, - seed: Option, -) -> RaoBlackwellizedParticleFilter { - // Implementation parallel to initialize_particle_filter - // but using RBProcessNoise and RBPF constructor - // ... -} -``` - -### Step 5: Add integration tests - -**Location:** `core/tests/integration_tests.rs` - -#### 5.1 Basic closed-loop test - -```rust -#[test] -fn test_rbpf_closed_loop_on_real_data() { - // Load test data - let manifest_dir = env!("CARGO_MANIFEST_DIR"); - let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv"); - let records = load_test_data(&test_data_path); - assert!(!records.is_empty()); - - // Initialize RBPF with 100 particles (fewer than standard PF) - let mut rbpf = initialize_rbpf( - &records[0], - 100, // Reduced particle count vs standard PF - VerticalChannelMode::Simplified, - None, - None, - None, - None, - Some(ResamplingStrategy::Systematic), - Some(1e-3), // UKF alpha - Some(2.0), // UKF beta - Some(0.0), // UKF kappa - Some(42), // Seed - ); - - // Build event stream with passthrough GNSS. - // - // `AidingConfig` is `#[non_exhaustive]`, so it is built from `default()` rather than - // written as a struct literal; everything not set keeps its default, including the - // barometer and magnetometer schedules. The fault field is `fault`, and - // `build_event_stream` takes the navigation frame explicitly -- `false` is NED. - let mut cfg = AidingConfig::default(); - cfg.scheduler = MeasurementScheduler::PassThrough; - cfg.fault = GnssFaultModel::None; - let stream = - build_event_stream(&records, &cfg, false).expect("records must not be empty"); - - // Run closed-loop - let results = run_closed_loop(&mut rbpf, stream, None, None) - .expect("RBPF closed-loop should complete"); - - // Compute error metrics - let stats = compute_error_metrics(&results, &records); - - // Print statistics - println!("\n=== RBPF Closed-Loop Error Statistics ==="); - println!("Horizontal Error: mean={:.2}m, max={:.2}m, rms={:.2}m", - stats.mean_horizontal_error, - stats.max_horizontal_error, - stats.rms_horizontal_error - ); - - // Assert reasonable error bounds (should match or beat UKF) - assert!( - stats.rms_horizontal_error < 50.0, - "RBPF RMS horizontal error too large: {:.2}m", - stats.rms_horizontal_error - ); -} -``` - -#### 5.2 GNSS dropout test - -```rust -#[test] -fn test_rbpf_with_gnss_dropout() { - // Similar to particle filter dropout test - // Use periodic scheduler with 60s on / 30s off - // Verify particle diversity maintained during outages - // ... -} -``` - -#### 5.3 Comparison test - -```rust -#[test] -fn test_rbpf_vs_standard_pf() { - // Load same data - // Run RBPF with 100 particles - // Run standard PF with 500 particles - // Compare: - // - Accuracy (RBPF should match or beat) - // - Bias estimation (RBPF should be better) - // - Computation time (not directly tested, but note particle count) - // ... -} -``` - -### Step 6: Documentation - -#### 6.1 Design document - -**Location:** `docs/PARTICLE_FILTER_DESIGN.md` - -Add new section: - -```markdown -## Rao-Blackwellized Particle Filter (RBPF) - -### Overview - -The RBPF exploits the conditional linear structure of the INS equations to reduce -computational burden while maintaining or improving estimation performance. It -partitions the 15-state vector into: - -- **Nonlinear states (3)**: Position [lat, lon, alt] → represented by particles -- **Linear states (12)**: Velocity, attitude, biases → per-particle UKF - -### Key Advantage - -By marginalizing the linear states analytically (via Kalman filter), the RBPF -requires significantly fewer particles than a standard PF: - -- Standard PF: 500-1000 particles typical -- RBPF: 50-200 particles sufficient - -This 5-10× reduction in particles translates directly to computational savings -while often improving bias estimation quality. - -### State Partitioning Rationale - -**Why position as nonlinear?** -- Coupled with Earth model (geodetic coordinates) -- Subject to GPS outages → multimodal posteriors -- Nonlinear transformation from body to navigation frame - -**Why velocity/attitude/biases as linear?** -- Conditionally Gaussian given position -- Biases evolve as random walks (Gaussian) -- Attitude dynamics approximately linear over short timesteps -- UKF handles residual nonlinearity efficiently - -### 2.5D Integration - -The RBPF maintains the 2.5D vertical channel approach: -- Vertical velocity in per-particle UKF with large process noise -- Position altitude propagated via simple integration -- Horizontal dynamics use full strapdown mechanization - -### Algorithm Summary - -**Predict:** -``` -For each particle i: - 1. Get velocity, attitude from particle_i.ukf - 2. Propagate position using forward_2_5d_rbpf(position, velocity, attitude, dt) - 3. Add position process noise ~ N(0, Q_position) - 4. UKF predict for linear states: ukf.predict(new_position, imu, Q_linear, dt) -``` - -**Update:** -``` -For each particle i: - 1. Compute marginal likelihood: p(z | position_i) from UKF innovation - 2. Update particle weight: w_i *= p(z | position_i) - 3. UKF update linear states: ukf.update(position_i, z) - -Normalize weights -If ESS < threshold: resample particles -``` - -**Estimate:** -``` -position_estimate = Σ(w_i * position_i) -linear_estimate = Σ(w_i * ukf_i.mean) -covariance = particle_cov(position) + Σ(w_i * ukf_i.cov) + cross_terms -``` - -### When to Use RBPF vs Standard PF - -**Use RBPF when:** -- Computational resources limited (EKF version is 3-5x faster than UKF) -- Bias estimation critical (e.g., low-cost IMU) -- GNSS intermittent but not completely denied -- Need real-time performance -- The conditionally-linear states have moderate nonlinearity - -**Use Standard PF when:** -- Highly nonlinear dynamics (aggressive maneuvers) -- Extreme non-Gaussian posteriors expected -- Computational cost not a concern -- Research/benchmarking (simpler algorithm) - -### Performance Characteristics (EKF Implementation) - -The current EKF-based RBPF implementation offers: -- **3-5x faster** than the previous UKF version -- **5-10x fewer particles needed** compared to standard PF for similar accuracy -- Suitable for real-time applications on embedded systems -- Maintains good accuracy for typical navigation scenarios - -**Computational Complexity:** -- EKF linearization: O(n²) for n-state system -- UKF sigma points: O(n³) for n-state system -- For 12-state linear subspace: EKF ~144 operations vs UKF ~1728 operations per particle -``` - -#### 6.2 User guide - -**Location:** `docs/USER_GUIDE.md` - -Add RBPF example: - -```markdown -### Rao-Blackwellized Particle Filter (EKF-based) - -For improved efficiency with similar accuracy: - -```rust -use strapdown::particle::{RaoBlackwellizedParticleFilter, RBProcessNoise, VerticalChannelMode, ResamplingStrategy}; -use strapdown::kalman::InitialState; - -let initial_state = InitialState { /* ... */ }; -let process_noise = RBProcessNoise::default(); - -let rbpf = RaoBlackwellizedParticleFilter::new( - initial_state, - vec![0.0; 6], // IMU biases - vec![1e-3; 15], // Initial covariance diagonal - process_noise, - 100, // Fewer particles than standard PF (uses EKF internally) - VerticalChannelMode::Simplified, - ResamplingStrategy::Systematic, - Some(42), // Random seed -); -``` - -The RBPF uses per-particle EKF for velocity, attitude, and biases, requiring -only ~100 particles compared to 500-1000 for standard PF. The EKF implementation -provides 3-5x faster performance than the previous UKF version. -``` - -## Implementation Notes - -### Measurement Routing - -Different measurements update different components: - -- **GPS Position** → Updates particle weights via marginal likelihood, UKF updates velocity correlation -- **GPS Velocity** → UKF update only (linear in velocity space) -- **GPS Position+Velocity** → Combined: position for weights, velocity for UKF -- **Barometric Altitude** → Updates particle altitude weights, UKF vertical velocity - -### Numerical Stability - -1. **Log-likelihood computation**: Use log-space for particle weights to avoid underflow -2. **Covariance symmetrization**: Apply `symmetrize()` to UKF covariance matrices -3. **Robust SPD solve**: Use `robust_spd_solve()` for Kalman gain computation -4. **Weight normalization**: Subtract max log-weight before exp() to prevent overflow - -### Performance Considerations - -**Expected particle counts:** -- Continuous GNSS: 50 particles sufficient -- Intermittent GNSS (5-10s updates): 100 particles -- Long outages (>30s): 200 particles -- Extreme scenarios: 500 particles (but standard PF may be better) - -**Computational complexity per timestep:** -- RBPF: O(N_particles × [position_prop + UKF_prop + UKF_update]) -- Standard PF: O(N_particles × full_state_prop) -- UKF speedup: ~2-3× per particle, but RBPF UKF is 9-state vs 15-state -- Overall: RBPF with 100 particles ≈ Standard PF with 200-300 particles in cost - -### Compatibility - -The RBPF implements the `NavigationFilter` trait, making it a drop-in replacement for UKF or standard PF in: -- `run_closed_loop()` simulation -- `dead_reckoning()` comparison -- Event stream processing -- All measurement models - -## Future Enhancements - -1. **Adaptive particle count**: Increase/decrease N based on ESS or GPS availability -2. **Regularization**: Add roughening/jitter to prevent particle collapse -3. **Parallel UKF**: Use Rayon to parallelize per-particle UKF operations -4. **Alternative partitioning**: Position+Attitude as nonlinear (4D particles, 6D UKF) -5. **Auxiliary particle filter**: Use auxiliary variable for improved proposal - -## References - -1. Doucet, A., et al. "Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks." UAI 2000. -2. Schön, T., et al. "Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models." IEEE Trans. Signal Processing, 2005. -3. Gustafsson, F., et al. "Particle filters for positioning, navigation, and tracking." IEEE Trans. Signal Processing, 2002. diff --git a/book/src/SUMMARY.md b/book/src/SUMMARY.md index ec2987e4..4d66343d 100644 --- a/book/src/SUMMARY.md +++ b/book/src/SUMMARY.md @@ -6,66 +6,56 @@ - [Installation](./installation/installation.md) - [System Requirements](./installation/requirements.md) - - [Installing from Crates.io](./installation/crates-io.md) - - [Building from Source](./installation/building.md) - [Quick Start](./quick-start.md) # User Guide - [Overview](./user-guide/overview.md) -- [Core Concepts](./user-guide/concepts.md) - - [Strapdown Mechanization](./user-guide/strapdown-mechanization.md) +- [The Navigation Model](./user-guide/concepts.md) - [Coordinate Frames](./user-guide/coordinate-frames.md) - [State Representation](./user-guide/state-representation.md) - [Running Simulations](./user-guide/simulations.md) - - [Open-Loop (Dead Reckoning)](./user-guide/open-loop.md) - - [Closed-Loop (UKF/EKF)](./user-guide/closed-loop.md) + - [Dead Reckoning](./user-guide/dead-reckoning.md) + - [Closed Loop (Kalman Filters)](./user-guide/closed-loop.md) - [Particle Filter](./user-guide/particle-filter.md) + - [Synthetic Trajectories](./user-guide/synthetic.md) - [Input Data Format](./user-guide/data-format.md) +- [Output Format](./user-guide/output-format.md) - [Configuration Files](./user-guide/configuration.md) - [Logging](./user-guide/logging.md) +- [Using the Library](./user-guide/library.md) # Navigation Filters - [Kalman Filters](./filters/kalman.md) + - [Error-State Kalman Filter (ESKF)](./filters/eskf.md) - [Extended Kalman Filter (EKF)](./filters/ekf.md) - [Unscented Kalman Filter (UKF)](./filters/ukf.md) - - [Comparison: EKF vs UKF](./filters/comparison.md) -- [Particle Filters](./filters/particle-filter.md) - - [Rao-Blackwellized Particle Filter](./filters/rbpf.md) -- [Measurement Models](./filters/measurements.md) + - [Comparison](./filters/comparison.md) +- [Rao-Blackwellized Particle Filter](./filters/rbpf.md) + - [Particle Building Blocks](./filters/particle-filter.md) +- [Measurement Models and Integrity](./filters/measurements.md) -# Geophysical Navigation +# GNSS Degradation -- [Overview](./geonav/overview.md) -- [Gravity Anomaly Navigation](./geonav/gravity.md) -- [Magnetic Anomaly Navigation](./geonav/magnetic.md) -- [Data Sources and Maps](./geonav/data-sources.md) +- [Fault Simulation](./gnss/fault-simulation.md) +- [Schedulers and Faults Reference](./gnss/scenarios.md) -# GNSS Degradation Scenarios +# Geophysical Navigation (Experimental) -- [Fault Simulation](./gnss/fault-simulation.md) -- [Dropout Scenarios](./gnss/dropouts.md) -- [Reduced Update Rates](./gnss/reduced-rates.md) -- [Measurement Corruption](./gnss/corruption.md) +- [Overview](./geonav/overview.md) +- [Maps and Measurement Models](./geonav/maps.md) -# API Reference +# Reference -- [strapdown-core](./api/core.md) - - [earth Module](./api/earth.md) - - [kalman Module](./api/kalman.md) - - [measurements Module](./api/measurements.md) - - [particles Module](./api/particles.md) - - [sim Module](./api/sim.md) -- [strapdown-sim](./api/sim-binary.md) -- [strapdown-geonav](./api/geonav.md) +- [API Documentation](./api/index.md) # Examples and Tutorials - [Example Configurations](./examples/configurations.md) -- [Tutorial: Basic INS Simulation](./examples/tutorial-basic.md) -- [Tutorial: GPS Degradation](./examples/tutorial-gps-degradation.md) -- [Tutorial: Using the Particle Filter](./examples/tutorial-particle-filter.md) +- [Tutorial: Basic INS](./examples/tutorial-basic.md) +- [Tutorial: GNSS Degradation](./examples/tutorial-gps-degradation.md) +- [Tutorial: Particle Filter](./examples/tutorial-particle-filter.md) # Development @@ -73,14 +63,13 @@ - [Building and Testing](./development/building.md) - [Performance Baselines](./development/performance.md) - [Architecture](./development/architecture.md) -- [Project Structure](./development/structure.md) +- [Python Analysis Tooling](./development/analysis.md) # FAQ - [Frequently Asked Questions](./faq.md) -# Additional Resources +# Resources -- [Publications](./resources/publications.md) -- [External Links](./resources/links.md) +- [Publications and Links](./resources/publications.md) - [Glossary](./resources/glossary.md) diff --git a/book/src/api/core.md b/book/src/api/core.md deleted file mode 100644 index b0d5c88f..00000000 --- a/book/src/api/core.md +++ /dev/null @@ -1 +0,0 @@ -# API Reference diff --git a/book/src/api/earth.md b/book/src/api/earth.md deleted file mode 100644 index 554be786..00000000 --- a/book/src/api/earth.md +++ /dev/null @@ -1 +0,0 @@ -# earth Module diff --git a/book/src/api/geonav.md b/book/src/api/geonav.md deleted file mode 100644 index 64a140fd..00000000 --- a/book/src/api/geonav.md +++ /dev/null @@ -1 +0,0 @@ -# strapdown-geonav diff --git a/book/src/api/kalman.md b/book/src/api/kalman.md deleted file mode 100644 index 775bb7a1..00000000 --- a/book/src/api/kalman.md +++ /dev/null @@ -1 +0,0 @@ -# kalman Module diff --git a/book/src/api/measurements.md b/book/src/api/measurements.md deleted file mode 100644 index 902e0d50..00000000 --- a/book/src/api/measurements.md +++ /dev/null @@ -1 +0,0 @@ -# measurements Module diff --git a/book/src/api/particles.md b/book/src/api/particles.md deleted file mode 100644 index c6526136..00000000 --- a/book/src/api/particles.md +++ /dev/null @@ -1 +0,0 @@ -# particles Module diff --git a/book/src/api/sim-binary.md b/book/src/api/sim-binary.md deleted file mode 100644 index 77a4627d..00000000 --- a/book/src/api/sim-binary.md +++ /dev/null @@ -1 +0,0 @@ -# strapdown-sim Binary diff --git a/book/src/api/sim.md b/book/src/api/sim.md deleted file mode 100644 index a22a9db4..00000000 --- a/book/src/api/sim.md +++ /dev/null @@ -1 +0,0 @@ -# sim Module diff --git a/book/src/chapter_1.md b/book/src/chapter_1.md deleted file mode 100644 index b743fda3..00000000 --- a/book/src/chapter_1.md +++ /dev/null @@ -1 +0,0 @@ -# Chapter 1 diff --git a/book/src/development/structure.md b/book/src/development/structure.md deleted file mode 100644 index 782fdf4d..00000000 --- a/book/src/development/structure.md +++ /dev/null @@ -1 +0,0 @@ -# Project Structure diff --git a/book/src/geonav/data-sources.md b/book/src/geonav/data-sources.md deleted file mode 100644 index 570d8c3a..00000000 --- a/book/src/geonav/data-sources.md +++ /dev/null @@ -1 +0,0 @@ -# Data Sources and Maps diff --git a/book/src/geonav/gravity.md b/book/src/geonav/gravity.md deleted file mode 100644 index 27f4d4cf..00000000 --- a/book/src/geonav/gravity.md +++ /dev/null @@ -1 +0,0 @@ -# Gravity Anomaly Navigation diff --git a/book/src/geonav/magnetic.md b/book/src/geonav/magnetic.md deleted file mode 100644 index fbc2b683..00000000 --- a/book/src/geonav/magnetic.md +++ /dev/null @@ -1 +0,0 @@ -# Magnetic Anomaly Navigation diff --git a/book/src/gnss/corruption.md b/book/src/gnss/corruption.md deleted file mode 100644 index 959d4542..00000000 --- a/book/src/gnss/corruption.md +++ /dev/null @@ -1 +0,0 @@ -# Measurement Corruption diff --git a/book/src/gnss/dropouts.md b/book/src/gnss/dropouts.md deleted file mode 100644 index 9a75f8aa..00000000 --- a/book/src/gnss/dropouts.md +++ /dev/null @@ -1 +0,0 @@ -# Dropout Scenarios diff --git a/book/src/gnss/reduced-rates.md b/book/src/gnss/reduced-rates.md deleted file mode 100644 index e843f1dd..00000000 --- a/book/src/gnss/reduced-rates.md +++ /dev/null @@ -1 +0,0 @@ -# Reduced Update Rates diff --git a/book/src/installation/building.md b/book/src/installation/building.md deleted file mode 100644 index c02da5c3..00000000 --- a/book/src/installation/building.md +++ /dev/null @@ -1,5 +0,0 @@ -# Building from Source - -Detailed instructions for building from source. - -*Content coming soon.* diff --git a/book/src/installation/crates-io.md b/book/src/installation/crates-io.md deleted file mode 100644 index 94dba775..00000000 --- a/book/src/installation/crates-io.md +++ /dev/null @@ -1,5 +0,0 @@ -# Installing from Crates.io - -Detailed instructions for installing from Crates.io. - -*Content coming soon.* diff --git a/book/src/resources/links.md b/book/src/resources/links.md deleted file mode 100644 index aa7d3db1..00000000 --- a/book/src/resources/links.md +++ /dev/null @@ -1 +0,0 @@ -# External Links diff --git a/book/src/user-guide/open-loop.md b/book/src/user-guide/open-loop.md deleted file mode 100644 index e614eafd..00000000 --- a/book/src/user-guide/open-loop.md +++ /dev/null @@ -1 +0,0 @@ -# Open-Loop Mode From 5b4e7bc9822c5322f05db7168c7b3ea171b89f40 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 20:31:12 -0400 Subject: [PATCH 02/26] Make the combo GNSS fault readable from a configuration file GnssFaultModel is internally tagged (#[serde(tag = "kind")]), and serde cannot put a tag on a newtype holding a sequence. So Combo(Vec) could be built in code but never deserialized: kind = "combo" in TOML or YAML failed with "invalid type: map, expected a sequence", while configuration.md advertised it as a fault kind. The CLI's --fault offers no combo either, so the variant was unreachable outside Rust. It is now the struct variant Combo { faults: Vec }, written kind = "combo" with the members under faults. Its documentation states that members share one FaultState and should be of different kinds. apply_fault's doc no longer says composition still needs wiring up. It has been wired since the fault interval moved out of apply_fault. The old test ran None twice, which would pass even if combo applied nothing. It is replaced by a test that two hijacks on different axes both land, and by a TOML/YAML round-trip test. Co-Authored-By: Claude Opus 5.5 --- core/src/messages.rs | 128 ++++++++++++++++++++++++++++++---- core/tests/example_configs.rs | 2 +- 2 files changed, 115 insertions(+), 15 deletions(-) diff --git a/core/src/messages.rs b/core/src/messages.rs index 44ed95c7..640349a8 100644 --- a/core/src/messages.rs +++ b/core/src/messages.rs @@ -152,10 +152,12 @@ pub enum MeasurementScheduler { /// }; /// /// // Combo: first drift slowly, then add hijack window -/// let fault = GnssFaultModel::Combo(vec![ -/// GnssFaultModel::SlowBias { drift_n_mps: 0.02, drift_e_mps: 0.0, q_bias: 1e-6, rotate_omega_rps: 0.0 }, -/// GnssFaultModel::Hijack { offset_n_m: 50.0, offset_e_m: 0.0, start_s: 120.0, duration_s: 60.0 }, -/// ]); +/// let fault = GnssFaultModel::Combo { +/// faults: vec![ +/// GnssFaultModel::SlowBias { drift_n_mps: 0.02, drift_e_mps: 0.0, q_bias: 1e-6, rotate_omega_rps: 0.0 }, +/// GnssFaultModel::Hijack { offset_n_m: 50.0, offset_e_m: 0.0, start_s: 120.0, duration_s: 60.0 }, +/// ], +/// }; /// ``` #[derive(Clone, Debug, Default, Serialize, Deserialize)] #[serde(tag = "kind", rename_all = "snake_case")] @@ -250,7 +252,37 @@ pub enum GnssFaultModel { /// /// The output of one model is fed as the input to the next. This allows /// combining e.g. `SlowBias` with a `Hijack` to simulate multi-stage spoofing. - Combo(Vec), + /// + /// In a configuration file this is `kind = "combo"` with the members under `faults`: + /// + /// ```toml + /// [aiding.fault] + /// kind = "combo" + /// [[aiding.fault.faults]] + /// kind = "slow_bias" + /// drift_n_mps = 0.02 + /// drift_e_mps = 0.0 + /// q_bias = 1e-6 + /// rotate_omega_rps = 0.0 + /// [[aiding.fault.faults]] + /// kind = "hijack" + /// offset_n_m = 50.0 + /// offset_e_m = 0.0 + /// start_s = 120.0 + /// duration_s = 60.0 + /// ``` + /// + /// It is a struct variant rather than a newtype around the list because the enum is + /// internally tagged: serde cannot put a `kind` tag on a sequence, so `Combo(Vec)` + /// could be built in code but never read from a file. + /// + /// The members share one [`FaultState`], so two members of the same kind (two + /// `Degraded`, say) step the same error states twice per fix rather than adding two + /// independent errors. Combine different kinds. + Combo { + /// The models to apply, in order. + faults: Vec, + }, } /// Default seed value for reproducible simulations @@ -859,9 +891,8 @@ fn ar1_step(x: &mut f64, rho: f64, sigma: f64, rng: &mut rand::rngs::StdRng) { /// window, measurements pass through unchanged. /// /// - **`GnssFaultModel::Combo`**\ -/// Intended to compose multiple effects by feeding the output of one model as -/// the input to the next. (Wire up the call loop to `apply_fault` for each -/// sub-model if composition is desired.) +/// Composes multiple effects by feeding the output of one model as the input to +/// the next, in order. /// /// # Units & conventions /// - Inputs/outputs for latitude and longitude are **degrees**; internal small-angle @@ -1031,7 +1062,7 @@ pub fn apply_fault( } } - GnssFaultModel::Combo(models) => { + GnssFaultModel::Combo { faults: models } => { let mut out = ( lat_deg, lon_deg, @@ -1224,7 +1255,7 @@ fn duty_cycle_is_on(elapsed_s: f64, on_s: f64, off_s: f64, start_phase_s: f64) - /// velocity sigmas scaled by `r_scale`. /// - `SlowBias`: integrates a drifting N/E bias (with optional rotation/random walk). /// - `Hijack`: applies a constant N/E offset within a time window. -/// - `Combo`: intended for sequential composition (hook up as needed). +/// - `Combo`: applies its members in order, each feeding the next. /// /// > **Note:** The current implementation passes `vertical_noise_std` through /// > unchanged. If you also want to degrade vertical accuracy, extend the @@ -2387,21 +2418,90 @@ mod tests { #[test] fn test_combo_fault_model() { - // Test that the combo fault model functionality exists - // Note: Due to commented code in apply_fault for Combo, this is a minimal test let records = create_test_records(10, 0.1); let config = AidingConfig { scheduler: MeasurementScheduler::PassThrough, - fault: GnssFaultModel::Combo(vec![GnssFaultModel::None, GnssFaultModel::None]), + fault: GnssFaultModel::Combo { + faults: vec![GnssFaultModel::None, GnssFaultModel::None], + }, ..Default::default() }; - // This should at least not crash let events = build_event_stream(&records, &config, false).unwrap(); assert!(!events.events.is_empty()); } + /// A combo applies every member: two hijacks with disjoint axes add their offsets. + /// + /// The old test only ran `None` twice, which a combo that applied nothing would also pass. + #[test] + fn combo_applies_each_member_in_turn() { + let combo = GnssFaultModel::Combo { + faults: vec![ + GnssFaultModel::Hijack { + offset_n_m: 100.0, + offset_e_m: 0.0, + start_s: 0.0, + duration_s: 10.0, + }, + GnssFaultModel::Hijack { + offset_n_m: 0.0, + offset_e_m: 100.0, + start_s: 0.0, + duration_s: 10.0, + }, + ], + }; + let mut state = FaultState::new(7); + let (lat, lon, ..) = apply_fault( + &combo, &mut state, 1.0, 1.0, 40.0, -75.0, 0.0, 0.0, 0.0, 3.0, 0.1, + ); + assert!(lat > 40.0 + 5e-4, "north offset missing: {lat}"); + assert!(lon > -75.0 + 5e-4, "east offset missing: {lon}"); + } + + /// `kind = "combo"` reads from TOML and YAML, members and all. + /// + /// As `Combo(Vec)` under `#[serde(tag = "kind")]` this failed with "invalid type: + /// map, expected a sequence": an internally tagged enum cannot tag a newtype holding a + /// sequence, so the variant the configuration docs advertised could never be configured. + #[test] + fn combo_round_trips_through_configuration_files() { + let toml_text = r#" + kind = "combo" + [[faults]] + kind = "slow_bias" + drift_n_mps = 0.02 + drift_e_mps = 0.0 + q_bias = 1e-6 + rotate_omega_rps = 0.0 + [[faults]] + kind = "hijack" + offset_n_m = 50.0 + offset_e_m = 0.0 + start_s = 120.0 + duration_s = 60.0 + "#; + let yaml_text = "kind: combo +faults: + - {kind: slow_bias, drift_n_mps: 0.02, drift_e_mps: 0.0, q_bias: 1.0e-6, rotate_omega_rps: 0.0} + - {kind: hijack, offset_n_m: 50.0, offset_e_m: 0.0, start_s: 120.0, duration_s: 60.0} +"; + let from_toml: GnssFaultModel = toml::from_str(toml_text).unwrap(); + let from_yaml: GnssFaultModel = serde_yaml::from_str(yaml_text).unwrap(); + for parsed in [&from_toml, &from_yaml] { + let GnssFaultModel::Combo { faults } = parsed else { + panic!("expected a combo, got {parsed:?}"); + }; + assert!(matches!(faults[0], GnssFaultModel::SlowBias { .. })); + assert!(matches!(faults[1], GnssFaultModel::Hijack { .. })); + } + let reparsed: GnssFaultModel = + toml::from_str(&toml::to_string(&from_toml).unwrap()).unwrap(); + assert!(matches!(reparsed, GnssFaultModel::Combo { ref faults } if faults.len() == 2)); + } + /// Without a time constant, nothing about the AR(1) step changes. /// /// The guarantee that lets `tau_pos_s` be additive: every configuration written before it diff --git a/core/tests/example_configs.rs b/core/tests/example_configs.rs index 8c14cee7..a4491398 100644 --- a/core/tests/example_configs.rs +++ b/core/tests/example_configs.rs @@ -141,7 +141,7 @@ const fn fault_variant(fault: &GnssFaultModel) -> &'static str { GnssFaultModel::Degraded { .. } => "Degraded", GnssFaultModel::SlowBias { .. } => "SlowBias", GnssFaultModel::Hijack { .. } => "Hijack", - GnssFaultModel::Combo(_) => "Combo", + GnssFaultModel::Combo { .. } => "Combo", } } From e6a4fe544a91edbbdc439cddcbcde278742e9160 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 20:37:20 -0400 Subject: [PATCH 03/26] Correct the mechanization equations in the crate docs, and the stale CLI help The crate-level rustdoc got three of Groves' signs wrong. The attitude update showed -(Omega_ie - Omega_en) for -(Omega_ie + Omega_en) (eq. 5.46). The velocity update showed -(Omega_en - Omega_ie) v for -(Omega_en + 2 Omega_ie) v (eq. 5.54). The height update added the down velocity instead of subtracting it (eq. 5.56). The code is right and tested (velocity_update_coriolis_term_is_a_bare_cross_product); only the documentation was wrong. The docs also called the IMU data pre-filtered four paragraphs before saying it must not be, gave positions in degrees where StrapdownState holds radians, and listed nine of the sixteen modules. strapdown-sim --help described three modes, called dead reckoning open loop, said closed loop ran the UKF or EKF, and marked the particle filter 'CURRENTLY IN DEVELOPMENT'. ol's help described a working Kalman mode it does not have. These now describe dr, cl (ESKF by default), pf, syn and config, and say plainly that ol writes nothing. ParticleFilterType's doc described per-particle Kalman filters with IMU biases, which the Canciani & Raquet restructure removed. cargo doc --workspace --all-features now passes with -D warnings. The 21 public docs that linked to private items now name them in plain code spans, and four redundant explicit link targets are dropped. Co-Authored-By: Claude Opus 5.5 --- core/src/kalman.rs | 6 +-- core/src/lib.rs | 98 ++++++++++++++++++++++++---------------- core/src/linearize.rs | 8 ++-- core/src/measurements.rs | 4 +- core/src/messages.rs | 4 +- core/src/metrics.rs | 4 +- core/src/sim.rs | 17 +++---- geonav/src/lib.rs | 4 +- sim/src/main.rs | 58 ++++++++++++++---------- 9 files changed, 117 insertions(+), 86 deletions(-) diff --git a/core/src/kalman.rs b/core/src/kalman.rs index dd987f26..69686667 100644 --- a/core/src/kalman.rs +++ b/core/src/kalman.rs @@ -717,7 +717,7 @@ pub(crate) fn imu_sample_from_input( impl NavigationFilter for UnscentedKalmanFilter { /// See [`NavigationFilter::baro_bias_index`]. Reports what - /// [`Self::set_baro_bias_index`] was told, so a filter built without one says `None`. + /// `Self::set_baro_bias_index` was told, so a filter built without one says `None`. fn baro_bias_index(&self) -> Option { self.baro_bias_index } @@ -758,7 +758,7 @@ impl NavigationFilter for UnscentedKalmanFilter { /// `[-pi, pi]` *per point*, because that is what `Rotation3::euler_angles` returns. At a /// southerly heading the set straddles the cut, so a linear mean of the numbers is not /// the mean attitude. They are put back on sigma point 0's branch by - /// [`unwrap_attitude_onto_reference_branch`] before the weighted sum, which is both + /// `unwrap_attitude_onto_reference_branch` before the weighted sum, which is both /// where the mean and the covariance become meaningful again (#336). fn predict( &mut self, @@ -1341,7 +1341,7 @@ impl ExtendedKalmanFilter { impl NavigationFilter for ExtendedKalmanFilter { /// See [`NavigationFilter::baro_bias_index`]. Reports what - /// [`Self::set_baro_bias_index`] was told, so a filter built without one says `None`. + /// `Self::set_baro_bias_index` was told, so a filter built without one says `None`. fn baro_bias_index(&self) -> Option { self.baro_bias_index } diff --git a/core/src/lib.rs b/core/src/lib.rs index 546e9203..c4e4aedc 100644 --- a/core/src/lib.rs +++ b/core/src/lib.rs @@ -1,13 +1,18 @@ -//! Strapdown navigation toolbox for various navigation filters +//! Strapdown inertial navigation and GNSS-degradation simulation //! -//! This crate provides a set of tools for implementing navigation filters in Rust. The filters are implemented -//! as structs that can be initialized and updated with new sensor data. The filters are designed to be used in -//! a strapdown navigation system, where the orientation of the sensor is known and the sensor data can be used -//! to estimate the position and velocity of the sensor. While utilities exist for IMU data, this crate does -//! not currently support IMU output directly and should not be thought of as a full inertial navigation system -//! (INS). This crate is designed to be used to test the filters that would be used in an INS. It does not -//! provide utilities for reading raw output from the IMU or act as IMU firmware or driver. As such the IMU data -//! is assumed to be pre-filtered and contain the total accelerations and relative rotations. +//! This crate implements local-level strapdown mechanization and four loosely coupled navigation +//! filters behind one [`NavigationFilter`] trait: an error-state Kalman filter +//! ([`kalman::ErrorStateKalmanFilter`], the default), an extended Kalman filter, an unscented +//! Kalman filter, and a Rao-Blackwellized particle filter ([`rbpf`]). It also provides the +//! measurement models that aid them and the event-stream engine ([`messages`]) that replays +//! recorded or synthetic data with GNSS withheld or corrupted. +//! +//! It is not IMU firmware or a driver: it consumes specific force and angular rate that are +//! already in physical units, and does not read raw sensor output. Those measurements are +//! **not** gravity-compensated: an accelerometer at rest reads the reaction to gravity, and +//! the mechanization removes gravity itself (see below). The +//! [user guide](https://jbrodovsky.github.io/strapdown-rs/) covers the concepts and the +//! `strapdown-sim` command-line tool. //! //! This crate is primarily built off of three additional dependencies: //! - [`nav-types`](https://crates.io/crates/nav-types): Provides basic coordinate types and conversions. @@ -24,16 +29,24 @@ //! //! ## Crate overview //! -//! This crate is organized into several modules: -//! - [earth]: Contains functions and constants related to Earth models, coordinate transformations, and geodetic calculations. -//! - [engine]: Contains the high-level [`InsEngine`] builder API, the user-facing entry point. -//! - [kalman]: Contains the implementation of Kalman-style navigation filters (including nonlinear variants) -//! - [linalg]: Contains linear algebra utilities and helper functions. -//! - [linearize]: Contains analytic Jacobians for strapdown mechanization and measurement models (for EKF/ESKF/RBPF-EKF). -//! - [measurements]: Contains measurement models and utilities for processing sensor data in the context of navigation filters. -//! - [messages]: Contains message definitions for sensor data and filter outputs used in constructing simulations. -//! - [particle]: Contains the implementation of particle filter navigation methods. -//! - [sim]: Contains simulation utilities for running and testing filters. +//! The mechanization itself lives in this crate root ([`forward`], [`attitude_update`], +//! [`position_update`], [`StrapdownState`]). The modules are: +//! - [alignment]: coarse alignment -- leveling and gyrocompassing -- for initialization. +//! - [calibration]: IMU calibration and sensor error models applied to raw samples. +//! - [earth]: the WGS84 ellipsoid, gravity, Earth rate, transport rate and magnetic models. +//! - [engine]: the [`InsEngine`] builder, the high-level entry point for applications. +//! - [error]: [`StrapdownError`], returned instead of panicking throughout the library. +//! - [gating]: chi-squared innovation gating and gate recovery. +//! - [kalman]: the ESKF, EKF and UKF, and their configuration. +//! - [linalg]: linear algebra helpers. +//! - [linearize]: analytic Jacobians of the mechanization and measurement models. +//! - [measurements]: the [`measurements::MeasurementModel`] trait and its implementations. +//! - [messages]: the event stream, GNSS schedulers and GNSS fault models. +//! - [metrics]: accuracy and consistency metrics (RMSE, CEP, NEES, NIS). +//! - [particle]: particle-filter building blocks (resampling and averaging strategies). +//! - [rbpf]: the Rao-Blackwellized particle filter after Canciani & Raquet (2017). +//! - [sim]: data records, results, configuration and the simulation drivers. +//! - [stationary]: the stationarity detector behind zero-velocity and zero-rate updates. //! //! ## Strapdown mechanization data and equations //! @@ -52,7 +65,9 @@ //! $$ //! //! Where: -//! - $p_n$, $p_e$, and $p_d$ are the WGS84 geodetic positions (degrees latitude, degrees longitude, meters relative to the ellipsoid). +//! - $p_n$, $p_e$, and $p_d$ are the WGS84 geodetic positions: latitude and longitude (radians in +//! [`StrapdownState`]; degrees in CSV records and results) and height above the ellipsoid +//! (metres, positive up in both frames). //! - $v_n$, $v_e$, and $v_v$ are the local level frame (NED/ENU) velocities (m/s) along the north axis, east axis, and vertical axis. //! - $\phi$, $\theta$, and $\psi$ are the Euler angles (radians) representing the orientation of the body frame relative to the local level frame (XYZ Euler rotation). //! @@ -117,50 +132,55 @@ //! x = \begin{bmatrix} a \\\\ b \\\\ c \end{bmatrix} \rightarrow X = \begin{bmatrix} 0 & -c & b \\\\ c & 0 & -a \\\\ -b & a & 0 \end{bmatrix} = \begin{bmatrix} x & \wedge \end{bmatrix} //! $$ //! +//! The equations below are written in NED, as Groves writes them. An ENU state is reflected into +//! NED, propagated, and reflected back, so there is one implementation of each equation. +//! //! #### Attitude update //! //! Given a direction-cosine matrix $C_b^n$ representing the orientation (attitude, rotation) of the platform's body frame ($b$) //! with respect to the local level frame ($n$), the transport rate $\Omega_{en}^n$ representing the rotation of the local level frame -//! with respect to the Earth-fixed frame ($e$), the Earth's rotation rate $\Omega_{ie}^e$, and the angular rate $\Omega_{ib}^b$ -//! representing the rotation of the body frame with respect to the inertial frame ($i$), the attitude update equation is given by: +//! with respect to the Earth-fixed frame ($e$), the Earth's rotation rate $\Omega_{ie}^n$ resolved in the local level frame, and +//! the angular rate $\Omega_{ib}^b$ representing the rotation of the body frame with respect to the inertial frame ($i$), the +//! attitude update (Groves eq. 5.46) is: //! //! $$ -//! C_b^n(+) \approx C_b^n(-) \left( I + \Omega_{ib}^b t \right) - \left( \Omega_{ie}^e - \Omega_{en}^n \right) C_b^n(-) t +//! C_b^n(+) \approx C_b^n(-) \left( I + \Omega_{ib}^b \tau \right) - \left( \Omega_{ie}^n + \Omega_{en}^n \right) C_b^n(-) \tau //! $$ //! -//! where $t$ is the time differential and $C(-)$ is the prior attitude. These attitude matrices are then used to transform the -//! specific forces from the IMU: +//! where $\tau$ is the propagation interval and $C(-)$ is the prior attitude. The specific force is resolved into the local level +//! frame with the attitude averaged over the interval (eq. 5.47): //! //! $$ //! f_{ib}^n \approx \frac{1}{2} \left( C_b^n(+) + C_b^n(-) \right) f_{ib}^b //! $$ //! -//! #### Velocity Update +//! #### Velocity update //! -//! The velocity update equation is given by: +//! The velocity update (eq. 5.54) adds the specific force and gravity and removes the Coriolis and transport-rate terms: //! //! $$ -//! v(+) \approx v(-) + \left( f_{ib}^n + g_{b}^n - \left( \Omega_{en}^n - \Omega_{ie}^e \right) v(-) \right) t +//! v(+) \approx v(-) + \left( f_{ib}^n + g_b^n - \left( \Omega_{en}^n + 2 \Omega_{ie}^n \right) v(-) \right) \tau //! $$ //! //! #### Position update //! -//! Finally, we update the base position states in three steps. First we update the altitude: +//! Finally, the position is updated in three steps (eq. 5.56), each using the trapezoidal average of the old and new +//! velocity. With $h$ the height above the ellipsoid (positive up) and $v_D$ the down velocity, the height is updated first: //! //! $$ -//! p_d(+) = p_d(-) + \frac{1}{2} \left( v_d(-) + v_d(+) \right) t +//! h(+) = h(-) - \frac{1}{2} \left( v_D(-) + v_D(+) \right) \tau //! $$ //! -//! Next we update the latitude: +//! then the latitude $L$, using the meridian radius of curvature $R_N$: //! //! $$ -//! p_n(+) = p_n(-) + \frac{1}{2} \left( \frac{v_n(-)}{R_n + p_d(-)} + \frac{v_n(+)}{R_n + p_d(+) } \right) t +//! L(+) = L(-) + \frac{1}{2} \left( \frac{v_N(-)}{R_N(L(-)) + h(-)} + \frac{v_N(+)}{R_N(L(-)) + h(+)} \right) \tau //! $$ //! -//! Finally, we update the longitude: +//! and finally the longitude $\lambda$, using the transverse radius of curvature $R_E$ at the old and new latitude: //! //! $$ -//! p_e = p_e(-) + \frac{1}{2} \left( \frac{v_e(-)}{R_e + p_d(-) \cos(p_n(-))} + \frac{v_e(+)}{R_e + p_d(+) \cos(p_n(+))} \right) t +//! \lambda(+) = \lambda(-) + \frac{1}{2} \left( \frac{v_E(-)}{\left( R_E(L(-)) + h(-) \right) \cos L(-)} + \frac{v_E(+)}{\left( R_E(L(+)) + h(+) \right) \cos L(+)} \right) \tau //! $$ //! //! This top-level module provides a public API for each step of the forward mechanization equations, allowing users to @@ -1913,7 +1933,7 @@ fn reduce_onto_interval(value: f64, lower: f64, upper: f64, period: f64) -> f64 /// /// # Cost and non-finite inputs /// -/// The reduction is constant time; see [`reduce_onto_interval`] for what that replaces +/// The reduction is constant time; see `reduce_onto_interval` for what that replaces /// and why it agrees with the loop it replaces on every finite input. pub fn wrap_to_180(angle: T) -> T where @@ -1939,7 +1959,7 @@ where /// /// # Cost and non-finite inputs /// -/// The reduction is constant time; see [`reduce_onto_interval`] for what that replaces +/// The reduction is constant time; see `reduce_onto_interval` for what that replaces /// and why it agrees with the loop it replaces on every finite input. pub fn wrap_to_360(angle: T) -> T where @@ -1973,7 +1993,7 @@ where /// ([`kalman::wrap_attitude_onto_principal_branch`](crate::kalman)), so a diverging /// attitude used to make the diagnostic path progressively more expensive exactly when it /// was least affordable. An input already on `[-π, π]`, and any non-finite input, is -/// returned unchanged; see [`reduce_onto_interval`] for why the result is identical to the +/// returned unchanged; see `reduce_onto_interval` for why the result is identical to the /// loop's on every finite input, `wrap_to_pi(3π) == +π` included. pub fn wrap_to_pi(angle: T) -> T where @@ -2011,7 +2031,7 @@ where /// /// # Cost and non-finite inputs /// -/// The reduction is constant time; see [`reduce_onto_interval`] for what that replaces +/// The reduction is constant time; see `reduce_onto_interval` for what that replaces /// and why it agrees with the loop it replaces on every finite input. pub fn wrap_to_2pi(angle: T) -> T where @@ -2045,7 +2065,7 @@ where /// /// # Cost and non-finite inputs /// -/// The reduction is constant time; see [`reduce_onto_interval`] for what that replaces +/// The reduction is constant time; see `reduce_onto_interval` for what that replaces /// and why it agrees with the loop it replaces on every finite input. pub fn wrap_latitude(latitude: T) -> T where diff --git a/core/src/linearize.rs b/core/src/linearize.rs index 576d2520..6f6a145e 100644 --- a/core/src/linearize.rs +++ b/core/src/linearize.rs @@ -171,13 +171,13 @@ fn euler_rate_matrix(roll: f64, pitch: f64, yaw: f64) -> nalgebra::Matrix3 /// **Attitude.** This is where the two filters genuinely differ, and it is not a sign. /// [`AttitudeParametrization::RotationVector`] here means the **navigation-frame** /// perturbation $\tilde C = (I + [\delta\theta\times]) C$ that the rest of -/// [`transition_jacobian`] is written in. A body-frame rate perturbation reaches it through +/// `transition_jacobian` is written in. A body-frame rate perturbation reaches it through /// the attitude, so /// /// $$ \frac{\partial \theta^+_n}{\partial b_g} = -C_b^n \, \Delta t $$ /// /// and for a state holding Euler angles the row converts out of rotation-vector space the -/// same way [`transition_jacobian`]'s attitude block does, through $E(\Phi^+)^{-1}$: +/// same way `transition_jacobian`'s attitude block does, through $E(\Phi^+)^{-1}$: /// /// $$ \frac{\partial \Phi^+}{\partial b_g} = -E(\Phi^+)^{-1} C_b^n \, \Delta t $$ /// @@ -192,7 +192,7 @@ fn euler_rate_matrix(roll: f64, pitch: f64, yaw: f64) -> nalgebra::Matrix3 /// /// Near gimbal lock $E$ is singular, and the rotation-vector form is kept -- wrong but /// bounded -- rather than inverting a near-singular matrix. Same policy, and same reason, as -/// [`transition_jacobian`]. +/// `transition_jacobian`. #[must_use] pub fn bias_coupling_blocks( state: &StrapdownState, @@ -452,7 +452,7 @@ pub fn attitude_reset_jacobian(delta_theta: &Vector3) -> nalgebra::Matrix3< /// # Construction /// /// $\omega^b = (C_b^n)^\top \omega^n$, so $E_b(\Phi) = (C_b^n)^\top E(\Phi)$ and therefore -/// $E_b^{-1} = E^{-1} C_b^n$. Built from [`euler_rate_matrix_inverse`] rather than from +/// $E_b^{-1} = E^{-1} C_b^n$. Built from `euler_rate_matrix_inverse` rather than from /// fresh trigonometry, so the two charts cannot drift apart. /// /// The amplification guard is then re-applied **to the product**, not inherited from diff --git a/core/src/measurements.rs b/core/src/measurements.rs index 9d520445..9e106fb4 100644 --- a/core/src/measurements.rs +++ b/core/src/measurements.rs @@ -244,7 +244,7 @@ pub trait MeasurementModel: Any { /// # The attitude columns are ∂h/∂Euler, and that is a promise (#349) /// /// `state` holds attitude as the roll, pitch and yaw - /// [`StrapdownState`](crate::StrapdownState) stores, so columns 6, 7 and 8 differentiate + /// [`StrapdownState`] stores, so columns 6, 7 and 8 differentiate /// with respect to *those three numbers*. An implementation must not write them against /// any other attitude parametrisation, and a filter must not read them as if they were. /// @@ -592,7 +592,7 @@ pub struct RelativeAltitudeMeasurement { /// the thing that declares it. /// /// A `Some(index)` past the end of the state is an error, not a silent zero: see - /// [`Self::require_bias_state`]. That matters more than it looks. A bias column the filter + /// `Self::require_bias_state`. That matters more than it looks. A bias column the filter /// never observes is #394's failure mode -- `expand_measurement_jacobian` pads on the right /// and would put a zero there with no complaint, leaving the state unobservable and the /// covariance growing on process noise alone. diff --git a/core/src/messages.rs b/core/src/messages.rs index 640349a8..dc5ec27d 100644 --- a/core/src/messages.rs +++ b/core/src/messages.rs @@ -384,7 +384,7 @@ pub struct AidingConfig { /// Scheduler that determines when barometric altitude measurements are emitted. /// - /// Defaults to one per second (see [`default_aiding_scheduler`]), *not* to + /// Defaults to one per second (see `default_aiding_scheduler`), *not* to /// [`MeasurementScheduler::PassThrough`]. [`MeasurementScheduler::DutyCycle`] gives a barometer outage /// the same way it gives a GNSS one. #[serde(default = "default_aiding_scheduler")] @@ -392,7 +392,7 @@ pub struct AidingConfig { /// Scheduler that determines when magnetometer heading measurements are emitted. /// - /// Defaults to one per second (see [`default_aiding_scheduler`]), *not* to + /// Defaults to one per second (see `default_aiding_scheduler`), *not* to /// [`MeasurementScheduler::PassThrough`]. The heading a magnetometer yields is derived from a /// field vector, so re-reading it faster than the field changes adds no information while /// adding weight. diff --git a/core/src/metrics.rs b/core/src/metrics.rs index 420ec2be..bdb18fb2 100644 --- a/core/src/metrics.rs +++ b/core/src/metrics.rs @@ -270,7 +270,7 @@ pub enum MetricId { /// /// [`Self::NpesPosition`] is the same quantity computed as though $P$ were diagonal, which /// is what this crate could measure before the off-diagonals reached - /// [`NavigationResult`](crate::sim::NavigationResult). That form is **optimistic**: it + /// [`NavigationResult`]. That form is **optimistic**: it /// equals the NEES only when the position states are genuinely uncorrelated, and after a /// GNSS update they are not. Where the two disagree, this one is right. NeesPosition, @@ -888,7 +888,7 @@ fn mean(values: &[f64]) -> Option { /// /// Public because the test targets each grew their own copy of this, which is what /// [`crate::metrics`] exists to stop (#368). `None` rather than zero for the same reason -/// [`mean`] returns `None`: a zero is indistinguishable from a perfect run. +/// `mean` returns `None`: a zero is indistinguishable from a perfect run. /// /// ``` /// use strapdown::metrics::root_mean_square; diff --git a/core/src/sim.rs b/core/src/sim.rs index 8f6f0d19..a398fabd 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -298,7 +298,7 @@ const VERTICAL_POSITION_PROCESS_NOISE_M2_PER_S: f64 = /// picked as though they were, which made the horizontal terms a 6.4 km per-step standard /// deviation sitting next to a 1 cm one. /// -/// #308 changed only the horizontal pair, leaving [`VERTICAL_POSITION_PROCESS_NOISE_M2_PER_S`] at +/// #308 changed only the horizontal pair, leaving `VERTICAL_POSITION_PROCESS_NOISE_M2_PER_S` at /// its historical `1e-4` because altitude never carried the units defect and a units fix is /// not the place to retune a channel. That retune is now done, separately and on its own /// evidence: the altitude entry is `1e-2`, derived from the same @@ -2575,7 +2575,7 @@ impl /// [`run_closed_loop_with_geo`], which carries the layout. /// /// The **barometric** bias is different, and is read here. Since #372 the filter answers -/// [`NavigationFilter::baro_bias_index`](crate::NavigationFilter::baro_bias_index) for itself, +/// [`NavigationFilter::baro_bias_index`] for itself, /// so the one question this conversion could not previously answer -- which extra state is /// which -- now has an answer for that state. Writing `None` regardless would drop an estimate /// the filter demonstrably holds. @@ -2639,7 +2639,7 @@ impl From<(&DateTime, &UnscentedKalmanFilter)> for NavigationResult { /// [`run_closed_loop_with_geo`], which carries the layout. /// /// The **barometric** bias is different, and is read here. Since #372 the filter answers -/// [`NavigationFilter::baro_bias_index`](crate::NavigationFilter::baro_bias_index) for itself, +/// [`NavigationFilter::baro_bias_index`] for itself, /// so the one question this conversion could not previously answer -- which extra state is /// which -- now has an answer for that state. Writing `None` regardless would drop an estimate /// the filter demonstrably holds. @@ -4390,7 +4390,7 @@ fn position_rms_meters( /// deviations $\sqrt{P_{ii}}$ (degrees, degrees, metres) at `debug` level -- despite the name, /// nothing is written to stdout, so the message appears only when the logger is configured for /// [`LogLevel::Debug`] or finer. The horizontal sigmas are also converted to metres (see -/// [`position_rms_meters`]) so they can be root-sum-squared with the (already-metric) altitude +/// `position_rms_meters`) so they can be root-sum-squared with the (already-metric) altitude /// sigma into a single, dimensionally meaningful RMS distance. /// /// The filter must expose at least the three position states; any 9- or 15-state filter in this @@ -5151,9 +5151,10 @@ pub enum FilterType { #[serde(rename_all = "kebab-case")] #[derive(Default)] pub enum ParticleFilterType { - /// Rao-Blackwellized particle filter after Canciani & Raquet (horizontal position as - /// particles; altitude, velocity, tilt, optional IMU biases and the map biases as a - /// Kalman filter per particle). The default, and currently the only variant. + /// Rao-Blackwellized particle filter after Canciani & Raquet (horizontal position error + /// as particles; altitude, velocity, tilt, the barometer loop's two states and the map + /// biases as one Kalman filter shared by every particle, with no IMU bias states). The + /// default, and currently the only variant. #[default] RaoBlackwellized, } @@ -6256,7 +6257,7 @@ pub struct SyntheticConfig { pub baro_noise_std_pa: f64, /// Magnetometer noise standard deviation per axis, microtesla. /// - /// See [`default_mag_noise_std_ut`]. Sensor noise only. + /// See `default_mag_noise_std_ut`. Sensor noise only. #[serde(default = "default_mag_noise_std_ut")] pub mag_noise_std_ut: f64, /// Hard-iron offset magnitude, microtesla, drawn once per trajectory and held constant. diff --git a/geonav/src/lib.rs b/geonav/src/lib.rs index f4f5749b..90d86a07 100644 --- a/geonav/src/lib.rs +++ b/geonav/src/lib.rs @@ -922,13 +922,13 @@ pub trait GeophysicalAnomalyMeasurementModel: MeasurementModel { /// therefore just $-h$ to five significant figures. It never tripped a gate, because a /// tens-of-milligal innovation against the default 100 mGal noise is a NIS of about 0.16. /// This is the same defect the magnetic channel had and the same fix; see -/// [`MICROTESLA_TO_NANOTESLA`]. +/// `MICROTESLA_TO_NANOTESLA`. /// /// # Latitude units /// /// [`gravity_anomaly`] takes **degrees**, while [`StrapdownState`] stores radians, so both /// of this type's anomaly paths convert: [`GeophysicalAnomalyMeasurementModel::set_state`] -/// and the per-particle path through [`Self::extract_state_inputs`]. Neither did before +/// and the per-particle path through `Self::extract_state_inputs`. Neither did before /// #330, which evaluated normal gravity near the equator whatever the true latitude -- a /// -2136 mGal error at 40 deg N, against map anomalies of tens of mGal. (That figure was /// itself written while the anomaly was still $m/s^2$; it is only literally true in milligal diff --git a/sim/src/main.rs b/sim/src/main.rs index c6486d11..c6b0226a 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -1,22 +1,27 @@ //! STRAPDOWN SIM: A simulation and analysis tool for strapdown inertial navigation systems. //! -//! This program can operate in three modes: open-loop, closed-loop, and particle-filter. +//! This program runs strapdown INS simulations on recorded or synthetic data: //! -//! - Open-loop mode: Relies solely on inertial measurements (IMU) and an initial position estimate -//! for dead reckoning. Useful for high-accuracy IMUs with drift rates ≤1 nm per 24 hours. +//! - dr: dead reckoning. Propagates the IMU from the initial state with no aiding at all. //! -//! - Closed-loop mode: Incorporates GNSS measurements to correct IMU drift using either an -//! Unscented Kalman Filter (UKF) or Extended Kalman Filter (EKF). Supports GNSS degradation -//! scenarios including jamming, reduced update rates, and spoofing. +//! - cl: closed loop. Corrects the INS with GNSS, barometric and magnetometer measurements +//! through the 15-state error-state Kalman filter (ESKF) by default, or the EKF or UKF with +//! --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). //! -//! - Particle-filter mode: Uses particle-based state estimation, supporting both standard and -//! Rao-Blackwellized implementations. +//! - pf: the Rao-Blackwellized particle filter, with the same GNSS degradation options. +//! +//! - syn: generates a synthetic trajectory: noisy sensor records for the modes above, or the +//! kinematic truth with --no-noise. +//! +//! - config: writes a template configuration file. +//! +//! `ol` (open loop) is reserved and not implemented; it writes no output. //! //! You can run simulations either by: -//! 1. Loading all parameters from a configuration file (TOML/JSON/YAML) +//! 1. Loading all parameters from a configuration file (TOML/JSON/YAML) with --config //! 2. Specifying parameters via command-line flags //! -//! For dataset format details, see the documentation or use --help with specific subcommands. +//! Results are written as CSV. User guide: https://jbrodovsky.github.io/strapdown-rs/ mod common; #[cfg(feature = "plotting")] @@ -78,23 +83,28 @@ const DEFAULT_LOG_LEVEL: &str = "info"; const LONG_ABOUT: &str = "STRAPDOWN SIM: A simulation and analysis tool for strapdown inertial navigation systems. -This program can operate in three modes: open-loop, closed-loop, and particle-filter. +This program runs strapdown INS simulations on recorded or synthetic data: + +- dr: dead reckoning. Propagates the IMU from the initial state with no aiding at all. + +- cl: closed loop. Corrects the INS with GNSS, barometric and magnetometer measurements + through the 15-state error-state Kalman filter (ESKF) by default, or the EKF or UKF with + --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). + +- pf: the Rao-Blackwellized particle filter, with the same GNSS degradation options. -- Open-loop mode: Relies solely on inertial measurements (IMU) and an initial position estimate - for dead reckoning. Useful for high-accuracy IMUs with drift rates ≤1 nm per 24 hours. +- syn: generates a synthetic trajectory: noisy sensor records for the modes above, or the + kinematic truth with --no-noise. -- Closed-loop mode: Incorporates GNSS measurements to correct IMU drift using either an - Unscented Kalman Filter (UKF) or Extended Kalman Filter (EKF). Supports GNSS degradation - scenarios including jamming, reduced update rates, and spoofing. +- config: writes a template configuration file. -- Particle-filter mode: Uses particle-based state estimation, supporting both standard and - Rao-Blackwellized implementations. CURRENTLY IN DEVELOPMENT!!! +`ol` (open loop) is reserved and not implemented; it writes no output. You can run simulations either by: - 1. Loading all parameters from a configuration file (TOML/JSON/YAML) + 1. Loading all parameters from a configuration file (TOML/JSON/YAML) with --config 2. Specifying parameters via command-line flags -For dataset format details, see the documentation or use --help with specific subcommands."; +Results are written as CSV. User guide: https://jbrodovsky.github.io/strapdown-rs/"; /// Command line arguments #[derive(Parser)] @@ -137,21 +147,21 @@ enum Command { DeadReckoning(SimArgs), #[command( name = "ol", - about = "Run simulation in open-loop mode", - long_about = "Run INS simulation in an open-loop (feed-forward) mode. In this mode, an initial position estimate and inertial measurements (IMU) are used to propagate the navigation solution. A Kalman filter (EKF or UKF) is used to estimate the errors to the navigation solution from GNSS measurements and apply the correction. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." + about = "Open-loop (feed-forward) mode -- not implemented; writes no output", + long_about = "Reserved for an open-loop (feed-forward) mode, in which a filter would estimate the navigation errors from GNSS without feeding the corrections back into the mechanization. It is not implemented: the command validates its paths and exits without writing a result. For dead reckoning use `dr`; for GNSS-aided navigation use `cl` or `pf`." )] OpenLoop(SimArgs), #[command( name = "cl", about = "Run simulation in closed-loop mode", - long_about = "Run INS simulation in a closed-loop (feedback) mode. In this mode, GNSS measurements are incorporated to correct for IMU drift and directly reset or update the navigation states using either an Unscented Kalman Filter (UKF) or Extended Kalman Filter (EKF). Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." + long_about = "Run INS simulation in a closed-loop (feedback) mode. In this mode, GNSS measurements are incorporated to correct for IMU drift and directly reset or update the navigation states. The filter is the 15-state error-state Kalman filter (ESKF) by default; --filter selects the extended (EKF) or unscented (UKF) Kalman filter instead. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." )] ClosedLoop(Box), #[command( name = "pf", about = "Run simulation using particle filter.", - long_about = "Run INS simulation using a particle filter for state estimation. This mode supports both standard and Rao-Blackwellized particle filter implementations. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." + long_about = "Run INS simulation using the Rao-Blackwellized particle filter after Canciani & Raquet (2017): horizontal position error as particles, the remaining error states as one Kalman filter shared by every particle. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." )] ParticleFilter(Box), From a4d2e3144d069d48266cd044ae1bab45b0aa913a Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 20:38:50 -0400 Subject: [PATCH 04/26] Build the book and its API docs on pull requests, and publish rustdoc with it deploy-book.yml ran only on pushes to main, so a page that failed to build, or a link into a deleted page, was first seen on the live site. It now builds on every pull request that touches the book or a crate, and deploys only from main. The build also runs cargo doc for the whole workspace with -D warnings and publishes the output under /api/. The book can then link to the documentation of the commit it was built from; docs.rs has none for 1.0.0 until the release. An offline lychee pass checks every link between pages and into the API docs. Smaller fixes in the same area: - The workspace homepage pointed at www.strapdown.rs, a domain that was never configured. It now points at the GitHub Pages book, and book.toml sets the matching site-url. - examples/magnetometer-config.{yaml,json} described a magnetometer block that matches no struct, and nothing referenced them. - examples/configs/README.md linked a docs/USER_GUIDE.md that does not exist. - CLAUDE.md listed pseudorange and carrier-phase models that were never implemented, linked a LOGGING.md that is gone, and counted 21 conf recipes where there are 15. Co-Authored-By: Claude Opus 5.5 --- .github/workflows/deploy-book.yml | 69 ++++++++++++++++++++++++------- CLAUDE.md | 11 ++--- Cargo.toml | 2 +- book/book.toml | 3 +- examples/configs/README.md | 2 +- examples/magnetometer-config.json | 20 --------- examples/magnetometer-config.yaml | 50 ---------------------- 7 files changed, 62 insertions(+), 95 deletions(-) delete mode 100644 examples/magnetometer-config.json delete mode 100644 examples/magnetometer-config.yaml diff --git a/.github/workflows/deploy-book.yml b/.github/workflows/deploy-book.yml index f4297507..acdc5da0 100644 --- a/.github/workflows/deploy-book.yml +++ b/.github/workflows/deploy-book.yml @@ -1,10 +1,21 @@ name: Deploy mdBook to GitHub Pages +# The book and the API documentation it links to are built on every pull request that could +# change them, and deployed only from `main`. Until #383 the build ran on `main` alone, so a +# page that failed to build -- or a link into a page that had been deleted -- was first seen +# on the live site. on: push: branches: [ "main" ] -# pull_request: -# branches: [ "main" ] + pull_request: + paths: + - "book/**" + - "core/**" + - "sim/**" + - "geonav/**" + - "CONTRIBUTING.md" + - "katex-header.html" + - ".github/workflows/deploy-book.yml" # Sets permissions of the GITHUB_TOKEN to allow deployment to GitHub Pages permissions: @@ -12,38 +23,64 @@ permissions: pages: write id-token: write -# Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. -# However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. +# One deployment at a time, never cancelling one in progress. Pull-request builds get their +# own group so they neither queue behind nor cancel a deployment. concurrency: - group: "pages" - cancel-in-progress: false + group: ${{ github.event_name == 'pull_request' && format('book-pr-{0}', github.event.pull_request.number) || 'pages' }} + cancel-in-progress: ${{ github.event_name == 'pull_request' }} jobs: build: runs-on: ubuntu-latest - timeout-minutes: 20 + timeout-minutes: 45 steps: - name: Checkout repository uses: actions/checkout@v4 - - name: Setup Rust - uses: dtolnay/rust-toolchain@stable - - # `+stable` is deliberate. `rust-toolchain.toml` pins the workspace to 1.91 for lint - # reproducibility, and that pin would otherwise apply here too -- building a + # `+stable` for mdBook is deliberate. `rust-toolchain.toml` pins the workspace to 1.91 + # for lint reproducibility, and that pin would otherwise apply here too -- building a # third-party tool against an older toolchain than it supports is the classic way to # break a docs deploy for a reason that has nothing to do with the docs. + - name: Setup Rust (stable, for mdBook) + uses: dtolnay/rust-toolchain@stable + + - name: Setup Rust (pinned, for rustdoc) + uses: dtolnay/rust-toolchain@1.91 + + - uses: Swatinem/rust-cache@v2 + - name: Install mdBook run: | - cargo +stable install mdbook --version 0.4.40 + cargo +stable install mdbook --version 0.4.40 --locked mdbook --version - name: Build the book + run: mdbook build book + + # The API reference is published inside the book, under /api/, so the book can link to + # the documentation of the exact commit it was built from. docs.rs only has a crate's + # documentation once that version is on crates.io. + # + # `-D warnings` makes a broken intra-doc link fail the build. RUSTDOCFLAGS replaces the + # `rustdocflags` in .cargo/config.toml rather than adding to it, so the KaTeX header is + # repeated here, with an absolute path because rustdoc runs from each crate's directory. + - name: Build the API documentation + env: + RUSTDOCFLAGS: -D warnings --html-in-header ${{ github.workspace }}/katex-header.html run: | - cd book - mdbook build + cargo doc --workspace --no-deps --all-features --locked + cp -r target/doc/. book/book/api/ + + # Offline: checks every relative link between book pages and into the API docs, + # without depending on external sites being up. + - name: Check links + uses: lycheeverse/lychee-action@v2 + with: + args: --offline --no-progress --exclude-path book/book/api 'book/book/**/*.html' + fail: true - name: Upload artifact + if: github.event_name == 'push' uses: actions/upload-pages-artifact@v3 with: path: ./book/book @@ -55,7 +92,7 @@ jobs: runs-on: ubuntu-latest timeout-minutes: 10 needs: build - if: github.ref == 'refs/heads/main' + if: github.event_name == 'push' && github.ref == 'refs/heads/main' steps: - name: Deploy to GitHub Pages id: deployment diff --git a/CLAUDE.md b/CLAUDE.md index a21d5236..977c4304 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -22,7 +22,7 @@ The core library implementing strapdown INS algorithms and simulation framework: - **kalman.rs**: Kalman-style navigation filters including Unscented Kalman Filter (UKF) for nonlinear state estimation - **particle.rs**: Particle-filter building blocks (the `Particle` trait, resampling and averaging strategies); not a filter on its own - **rbpf.rs**: The Rao-Blackwellized particle filter, after Canciani & Raquet (2017) -- the one concrete particle filter -- **measurements.rs**: Measurement models (GPS position/velocity, barometric altitude, pseudorange, carrier phase) implementing the `MeasurementModel` trait +- **measurements.rs**: Measurement models (GPS position/velocity, barometric altitude, magnetometer yaw, ZUPT/ZARU) implementing the `MeasurementModel` trait. Loosely coupled only: there are no pseudorange or carrier-phase models - **messages.rs**: Event stream handling for GNSS scheduling and fault injection scenarios - **sim.rs**: Simulation utilities, CSV data loading (Sensor Logger format), dead reckoning and closed-loop functions - **linalg.rs**: Linear algebra utilities for matrix operations @@ -48,7 +48,7 @@ Command-line tool for running INS simulations with GNSS degradation: `to_mcap` exist as **library** writers on `NavigationResult`, reachable from Rust but not from this binary. There is no Parquet writer at all - Configuration: TOML/YAML/JSON scenario files or command-line arguments -- Built-in logging: Use `--log-level` and `--log-file` flags (see LOGGING.md for details) +- Built-in logging: Use `--log-level` and `--log-file` flags (see `book/src/user-guide/logging.md`) **Free Core scope**: Basic GNSS degradation (outages, noise, reduced availability) **Future Pro scope**: Advanced faults (spoofing, jamming, multipath, terrain masking) @@ -58,7 +58,7 @@ Command-line tool for running INS simulations with GNSS degradation: - Loads NetCDF geophysical maps (gravity/magnetic anomaly grids) - Integrates geophysical measurements with INS/GNSS filters - Provides alternative PNT in GNSS-denied environments -- Built-in logging: Use `--log-level` and `--log-file` flags (see LOGGING.md for details) +- Built-in logging: Use `--log-level` and `--log-file` flags (see `book/src/user-guide/logging.md`) - Status: Experimental feature for research, may be commercialized in future roadmap ### 4. `analysis` (/analysis, Python) @@ -68,7 +68,7 @@ Post-processing and experiment tooling, exposed as the `analyze` CLI: `_magnetic.nc` maps beside each trajectory. `just preprocess` runs it at **10 Hz**, which gives 10 Hz inertial propagation against the ~1 Hz the GNSS was actually recorded at (the GNSS columns stay NaN in nine rows out of ten; do not interpolate them up). `data/input` is - the one directory every consumer reads -- all 21 `conf/*.toml`, `geo-stats`, `postprocess` + the one directory every consumer reads -- all 15 `conf/*.toml`, `geo-stats`, `postprocess` and `geoperf-*` -- so change the rate, not the path. `just preprocess-1hz` is the 1 Hz variant, which is the rate every result before the geophysical fixes used - **geostats.py**: characterises the geophysical measurements against those maps -- per-field @@ -254,7 +254,8 @@ The Free Core implementation must achieve the following capabilities: - **Forward propagation**: Uses `StrapdownState::propagate()` with strapdown equations (Chapter 5.4-5.5) - **Measurement models**: Implement the `MeasurementModel` trait for update step - Trait provides `predict_measurement()` and `innovation_covariance()` methods - - Implemented models: GPS position, GPS velocity, barometric altitude, pseudorange, carrier phase + - Implemented models: GPS position, velocity, and position+velocity; relative (barometric) + altitude; magnetometer yaw; ZUPT and ZARU. Geophysical models live in `strapdown-geonav` - **UKF implementation**: - Uses unscented transform with sigma points for nonlinear state estimation - Handles full 9-state navigation solution diff --git a/Cargo.toml b/Cargo.toml index c05d2dd5..8cb6a655 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,7 +14,7 @@ resolver = "3" authors = ["James Brodovsky"] edition = "2024" license = "MIT" -homepage = "https://www.strapdown.rs" +homepage = "https://jbrodovsky.github.io/strapdown-rs/" repository = "https://github.com/jbrodovsky/strapdown-rs" # Minimum supported Rust version. Keep this in sync with `channel` in # rust-toolchain.toml, `msrv` in clippy.toml and the CI toolchain pins; raising diff --git a/book/book.toml b/book/book.toml index 6a11c960..156b8ac0 100644 --- a/book/book.toml +++ b/book/book.toml @@ -13,8 +13,7 @@ create-missing = true mathjax-support = true git-repository-url = "https://github.com/jbrodovsky/strapdown-rs" edit-url-template = "https://github.com/jbrodovsky/strapdown-rs/edit/main/book/{path}" -#site-url = "/strapdown-rs/" -#cname = "www.strapdown.rs" +site-url = "/strapdown-rs/" default-theme = "light" preferred-dark-theme = "navy" no-section-label = false diff --git a/examples/configs/README.md b/examples/configs/README.md index 673e9a93..ae431a4c 100644 --- a/examples/configs/README.md +++ b/examples/configs/README.md @@ -40,7 +40,7 @@ and `--seed` arguments are ignored when it is present -- the CLI says as much in `output`, so point a scenario at your own data by adding those two keys to the file (or copying it), not by passing paths on the command line. -See the [User Guide](../../docs/USER_GUIDE.md) for detailed documentation. +See the [configuration guide](https://jbrodovsky.github.io/strapdown-rs/user-guide/configuration.html) for every key. --- diff --git a/examples/magnetometer-config.json b/examples/magnetometer-config.json deleted file mode 100644 index 90fdfe48..00000000 --- a/examples/magnetometer-config.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "scheduler": { - "kind": "pass_through" - }, - "fault": { - "kind": "none" - }, - "seed": 42, - "magnetometer": { - "enabled": true, - "reference_field_ned": [25.0, 0.0, 40.0], - "hard_iron_offset": [0.0, 0.0, 0.0], - "soft_iron_matrix": [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - ], - "noise_std": 5.0 - } -} diff --git a/examples/magnetometer-config.yaml b/examples/magnetometer-config.yaml deleted file mode 100644 index 19e9ea21..00000000 --- a/examples/magnetometer-config.yaml +++ /dev/null @@ -1,50 +0,0 @@ -# Example GNSS degradation configuration with magnetometer aiding enabled -# -# This configuration demonstrates how to enable and configure magnetometer aiding -# for improved heading/yaw estimation during GNSS outages. - -# GNSS scheduler - controls when GNSS measurements are available -scheduler: - kind: "pass_through" # Options: pass_through, fixed_interval, duty_cycle - -# GNSS fault model - controls how GNSS measurements are corrupted -fault: - kind: "none" # Options: none, degraded, hijack, slow_bias, combo - -# Random seed for reproducibility -seed: 42 - -# Magnetometer aiding configuration -magnetometer: - # Enable magnetometer measurements - enabled: true - - # Reference magnetic field vector in NED frame (microteslas) - # This should be set based on your geographic location using WMM or IGRF - # Examples: - # - Mid-latitude Northern Hemisphere (e.g., USA): [25.0, 0.0, 40.0] - # - Mid-latitude Southern Hemisphere (e.g., Australia): [25.0, 0.0, -40.0] - # - Equator: [30.0, 0.0, 0.0] - # - North Pole: [0.0, 0.0, 60.0] - reference_field_ned: [25.0, 0.0, 40.0] - - # Hard-iron offset calibration (microteslas) - # Constant bias in each axis from ferromagnetic materials on vehicle - # Determined during calibration procedure - hard_iron_offset: [0.0, 0.0, 0.0] - - # Soft-iron correction matrix (dimensionless, 3x3 row-major) - # Corrects for scale factors and cross-axis misalignment - # Identity matrix = no correction - soft_iron_matrix: [ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - ] - - # Measurement noise standard deviation (microteslas) - # Typical values: 3-10 μT depending on sensor quality - noise_std: 5.0 - -# Usage: -# strapdown-sim closed-loop --input data.csv --output results.csv --config magnetometer-config.yaml From 5fbdcacc9aa33735e0f26768d77f49a28f6fee2e Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:00:26 -0400 Subject: [PATCH 05/26] Fill the book: every page in SUMMARY now documents v1.0 as it is (#383) 36 of the 54 pages were one-line placeholders, and four filled pages described APIs that do not exist. filters/particle-filter.md was a pre-implementation design for types never written; ekf.md called ExtendedKalmanFilter::new with the wrong arguments; data-format.md imported a load_test_data that never existed; faq.md documented config keys that were never read. Every page in the pruned outline is now written against the code: - Getting started: installation from git until v1.0.0 reaches crates.io, with per-feature build requirements, and a quick start run end to end on a syn trajectory. - User guide: the navigation model (Groves eqs. 5.46, 5.47, 5.54 and 5.56, written from the code), frames, state layouts, a reference page per subcommand, the input and output schemas, every configuration section, and library use through InsEngine. - Filters: the NavigationFilter trait, ESKF, EKF, UKF and RBPF, the particle building blocks, every measurement model, and gating. - GNSS degradation, experimental geonav, an API page pointing at the rustdoc that the deploy now publishes, architecture, analysis tooling, FAQ, publications and glossary. Rust in the book is pulled from compiled examples by anchor, so a signature change breaks cargo build --examples rather than silently staling a page. Five new examples back the filter and library pages (kalman_filters, eskf, aiding, lever_arm, score_run), and basic_ins/gnss_outage gain anchors. Every CLI command shown was run. Accuracy figures come only from the generated baseline tables. theme/head.hbs enables inline $...$ math. mdBook 0.4's MathJax 2 renders only $$ and \( \) by default, so the inline math would have shown as raw dollar signs. Spacing commands markdown would strip (\,) are written as \ instead. Co-Authored-By: Claude Opus 5.5 --- book/src/api/index.md | 88 +++ book/src/development/analysis.md | 80 ++ book/src/development/architecture.md | 168 +++++ book/src/development/building.md | 376 ++++------ book/src/development/contributing.md | 10 +- book/src/development/performance.md | 540 ++++---------- book/src/examples/configurations.md | 175 +++-- book/src/examples/tutorial-particle-filter.md | 238 ++++++ book/src/faq.md | 220 +++--- book/src/filters/comparison.md | 68 +- book/src/filters/ekf.md | 309 ++++---- book/src/filters/eskf.md | 199 +++++ book/src/filters/kalman.md | 271 +++++++ book/src/filters/measurements.md | 230 +++++- book/src/filters/particle-filter.md | 596 ++------------- book/src/filters/rbpf.md | 15 +- book/src/filters/ukf.md | 149 ++++ book/src/geonav/maps.md | 219 ++++++ book/src/geonav/overview.md | 196 +++++ book/src/gnss/fault-simulation.md | 172 +++-- book/src/gnss/scenarios.md | 274 +++++++ book/src/installation/installation.md | 200 ++--- book/src/installation/requirements.md | 161 ++-- book/src/introduction.md | 154 ++-- book/src/quick-start.md | 282 +++++-- book/src/resources/glossary.md | 72 ++ book/src/resources/publications.md | 54 +- book/src/user-guide/closed-loop.md | 237 +++++- book/src/user-guide/concepts.md | 256 ++++++- book/src/user-guide/configuration.md | 691 ++++++++++++------ book/src/user-guide/coordinate-frames.md | 144 ++++ book/src/user-guide/data-format.md | 229 +++--- book/src/user-guide/dead-reckoning.md | 102 +++ book/src/user-guide/library.md | 304 ++++++++ book/src/user-guide/logging.md | 58 +- book/src/user-guide/output-format.md | 114 +++ book/src/user-guide/overview.md | 256 +++---- book/src/user-guide/particle-filter.md | 165 ++++- book/src/user-guide/simulations.md | 214 ++++++ book/src/user-guide/state-representation.md | 186 +++++ book/src/user-guide/synthetic.md | 237 ++++++ book/theme/head.hbs | 13 + core/examples/aiding.rs | 236 ++++++ core/examples/basic_ins.rs | 8 +- core/examples/eskf.rs | 162 ++++ core/examples/gnss_outage.rs | 4 + core/examples/kalman_filters.rs | 202 +++++ core/examples/lever_arm.rs | 215 ++++++ core/examples/score_run.rs | 68 ++ 49 files changed, 7228 insertions(+), 2389 deletions(-) create mode 100644 book/src/api/index.md create mode 100644 book/src/development/analysis.md create mode 100644 book/src/filters/eskf.md create mode 100644 book/src/geonav/maps.md create mode 100644 book/src/gnss/scenarios.md create mode 100644 book/src/user-guide/dead-reckoning.md create mode 100644 book/src/user-guide/library.md create mode 100644 book/src/user-guide/output-format.md create mode 100644 book/src/user-guide/synthetic.md create mode 100644 book/theme/head.hbs create mode 100644 core/examples/aiding.rs create mode 100644 core/examples/eskf.rs create mode 100644 core/examples/kalman_filters.rs create mode 100644 core/examples/lever_arm.rs create mode 100644 core/examples/score_run.rs diff --git a/book/src/api/index.md b/book/src/api/index.md new file mode 100644 index 00000000..7a3a9c0f --- /dev/null +++ b/book/src/api/index.md @@ -0,0 +1,88 @@ +# API Documentation + +The complete API reference is the rustdoc generated from the source. This page maps the +workspace so you know which crate and module to open. + +## Where to read it + +| crate | imported as | rustdoc in this book | docs.rs | +|---|---|---|---| +| `strapdown-core` | `strapdown` | [api/strapdown](strapdown/index.html) | [docs.rs/strapdown-core](https://docs.rs/strapdown-core) | +| `strapdown-sim` | (binary) | [api/strapdown_sim](strapdown_sim/index.html) | -- | +| `strapdown-geonav` (experimental) | `geonav` | [api/geonav](geonav/index.html) | [docs.rs/strapdown-geonav](https://docs.rs/strapdown-geonav) | + +The copy under `api/` is generated by `cargo doc` and published with this book, so it matches +the book's version of the code. The docs.rs pages appear once the crates are published to +crates.io with the v1.0.0 release; until then use the copy here or build your own. + +To build and open the documentation locally: + +```bash +cargo doc --workspace --no-deps --open # all three crates (alias: cargo docs) +cargo doc -p strapdown-core --all-features --no-deps --open # include the hdf5/netcdf/mcap/clap items +``` + +Items behind a cargo feature only appear when that feature is enabled, which is why the second +command exists. Output goes to `target/doc/`, one directory per crate under its library name: +`strapdown`, `strapdown_sim` and `geonav`. + +## `strapdown-core` + +The library. Every module is public; `lib.rs` itself holds the 9-state strapdown mechanization +(`StrapdownState`, `forward`, the IMU input type) and the `NavigationFilter` trait. + +| module | purpose | +|---|---| +| `alignment` | coarse alignment (Groves §5.6.3): roll and pitch by levelling, heading by gyrocompassing or from velocity | +| `calibration` | deterministic IMU calibration, removing systematic sensor errors ahead of the mechanization | +| `earth` | WGS84 ellipsoid constants, radii of curvature, normal gravity, Earth rate, transport rate, Eötvös correction, gravity anomaly | +| `engine` | `InsEngine`: a high-level, builder-configured navigation engine for feeding samples in one at a time | +| `error` | `StrapdownError`, the single error type every fallible function returns | +| `gating` | innovation gating (chi-squared outlier rejection) and its recovery policies | +| `kalman` | the Kalman-family filters: ESKF, EKF and UKF | +| `linalg` | linear-algebra helpers, chiefly robust covariance square roots | +| `linearize` | analytic Jacobians of the mechanization for the EKF and ESKF | +| `measurements` | the `MeasurementModel` trait and its models: GNSS position, velocity and both together, barometric relative altitude, magnetometer yaw, ZUPT and ZARU | +| `messages` | event-stream construction: GNSS, barometer and magnetometer scheduling and GNSS fault injection | +| `metrics` | navigation-accuracy metrics (RMSE, CEP, NEES, containment) for scoring a solution against a reference | +| `particle` | particle-filter building blocks: the `Particle` trait, resampling and averaging strategies | +| `rbpf` | the Rao-Blackwellized particle filter after Canciani & Raquet (2017) | +| `sim` | input records (`TestDataRecord`), output rows (`NavigationResult`), dead reckoning, the closed-loop runner, synthetic trajectories, configuration types and health monitoring | +| `stationary` | variance-based detection of stationary periods, for zero-velocity and zero-angular-rate aiding | + +The crate root also re-exports `InsEngine` and its configuration types, `StrapdownError`, and +the gating types `InnovationGate`, `GateRecovery` and `UpdateOutcome`. + +Its cargo features are all off by default, so `strapdown-core` alone needs nothing but a Rust +toolchain: + +| feature | enables | +|---|---| +| `clap` | `clap` derives on the configuration enums, used by `strapdown-sim` | +| `hdf5` | `to_hdf5` / `from_hdf5` on `TestDataRecord` and `NavigationResult` (builds libhdf5 from source) | +| `netcdf` | `to_netcdf` / `from_netcdf` on the same two types (builds libnetcdf from source) | +| `mcap` | `to_mcap` / `from_mcap` on the same two types | +| `full` | all of the above | + +## `strapdown-sim` + +The command-line tool. It is a binary crate, so its rustdoc documents the program's internals +rather than an API to depend on; the user-facing reference is `strapdown-sim --help` and +[Running Simulations](../user-guide/simulations.md). + +| module | purpose | +|---|---| +| `main` | argument parsing, the `dr`/`cl`/`pf`/`syn`/`ol`/`config` subcommands, and configuration-file runs | +| `common` | logging setup, input and output path validation (CSV output only), file discovery, interactive prompts | +| `plotting` | the optional `--plot` performance figure (feature `plotting`, on by default) | + +Its `geonav` feature adds the `--geo` flags and depends on `strapdown-geonav`. + +## `strapdown-geonav` + +The experimental geophysical-navigation crate, a single module. Its main items are `GeoMap` +(NetCDF anomaly grids with interpolation and gradients), the measurement models +`GravityMeasurement`, `MagneticAnomalyMeasurement` and `CombinedGeophysicalMeasurement`, the +bias-state bookkeeping `GeoBiasLayout` and `BiasState`, and `build_event_stream`, which adds +geophysical measurements to core's event stream. See [Geophysical Navigation](../geonav/overview.md) +and [Maps and Measurement Models](../geonav/maps.md). diff --git a/book/src/development/analysis.md b/book/src/development/analysis.md new file mode 100644 index 00000000..499cb8dd --- /dev/null +++ b/book/src/development/analysis.md @@ -0,0 +1,80 @@ +# Python Analysis Tooling + +> **Experimental research tooling.** The `analysis` package is what the project's own +> experiments run on. It is tested in CI, but its interface is not covered by the 1.0 API +> commitments the Rust crates make. + +The repository is a two-language monorepo. Beside the Cargo workspace sits `analysis/`, a +Python package (Python 3.13 or newer) that preprocesses recordings into simulator input and +scores simulator output. It is a member of a **uv** workspace declared by the root +`pyproject.toml`; it is not a Cargo member, and nothing in the Rust crates depends on it. + +## Setup + +From the repository root: + +```bash +uv sync # creates .venv and installs analysis with its dependencies +uv run analyze --help # the CLI, through the `analyze` entry point +``` + +`just setup` runs `cargo fetch` and `uv sync` together. + +**GMT is optional.** PyGMT loads the GMT C library when it is imported, so every import of it +is inside the function that uses it, and the package, its CLI and its tests all work without +GMT installed (CI's `python.yml` deliberately installs none). GMT and network access are only +needed to download maps with `analyze preprocess --getmaps`. + +## The `analyze` command + +| subcommand | module | what it does | +|---|---|---| +| `preprocess` | `preprocess.py` | rebuilds simulator input from Sensor Logger exports: resampling to a fixed rate (`-f`), splitting recordings at IMU dropouts, and with `--getmaps` downloading each trajectory's `_gravity.nc`, `_magnetic.nc` and `_relief.nc` maps; `--synthetic` replaces the phone's gravity and magnetometer readings with simulated dedicated sensors (`synthetic.py`) | +| `dataset-summary` | `__init__.py` | distance travelled and duration of each trajectory | +| `performance` | `compare.py`, `plotting.py` | error statistics, LaTeX tables and plots for a directory of results against the GNSS reference | +| `geoperformance` | `compare.py`, `plotting.py` | the same for geophysically aided runs, against a degraded unaided baseline | +| `compare-filters` | `compare.py` | side-by-side comparison of several result directories | +| `geostats` | `geostats.py` | bias, noise, signal-to-noise and de-correlation length of the geophysical readings against their maps; `--apply-to conf` writes the measured values into the configs | + +`uv run analyze --help` lists each one's options. `geostats` mirrors the +anomaly computations in `core/src/earth.rs` and `geonav/src/lib.rs` term for term and checks +that it agrees with them before computing anything; see +[Maps and Measurement Models](../geonav/maps.md#the-python-mirror-of-these-models). + +## The `justfile` recipes + +The experiment pipeline is a set of [`just`](https://github.com/casey/just) recipes at the +repository root: + +| recipe | does | +|---|---| +| `setup` | `cargo fetch` and `uv sync` | +| `build` | `cargo build --release --workspace --all-features` | +| `check-python` | `ruff check`, `ruff format --check` and `pytest`, as CI runs them | +| `preprocess` | rebuilds `data/input` from `data/raw` at 10 Hz, with maps and synthetic geophysical sensors | +| `preprocess-1hz` | the same at 1 Hz | +| `geo-stats` | `analyze geostats` over `data/input`; writes a report and leaves `conf/` alone | +| `geo-adopt` | `analyze geostats --apply-to conf`: rewrites the geophysical configs with measured values | +| `truth`, `degraded`, `ukf-geo`, `ekf-geo`, `rbpf-sim` | run the `conf/` recipes through `strapdown-sim --config` | +| `postprocess` | `dataset-summary` and `performance` over every result directory | +| `geoperf-all`, `geoperf-rbpf` | `geoperformance` for each geophysically aided run against its baseline | +| `clean` | deletes `data/input`, `data/output` and `log` | +| `pipeline` | `clean build preprocess geo-stats truth degraded ukf-geo ekf-geo rbpf-sim postprocess geoperf-all`, in order | + +**`data/raw` is not distributed.** The recordings the pipeline starts from are not in the +repository (`data/` is ignored by git), so `just preprocess` and `just pipeline` only run where +those recordings exist. The other subcommands take any directory of simulator input and +output: `performance` and `dataset-summary` were run for this page on a trajectory from +`strapdown-sim syn` and its closed-loop result, matched by file name. `geostats` also needs +the `_gravity.nc` and `_magnetic.nc` maps beside each trajectory. + +## Checks + +```bash +uv run ruff check +uv run ruff format --check +uv run pytest -q +``` + +These are what `.github/workflows/python.yml` runs, and what `just check-python` wraps. Ruff +is configured in the root `pyproject.toml`. diff --git a/book/src/development/architecture.md b/book/src/development/architecture.md index c79bec1a..832d2c08 100644 --- a/book/src/development/architecture.md +++ b/book/src/development/architecture.md @@ -1 +1,169 @@ # Architecture + +This page describes how the workspace is put together: which crate depends on which, how a +simulation's data moves from a CSV file to a CSV file, and the rules every crate follows for +errors, determinism and optional features. For a module-by-module list, see +[API Documentation](../api/index.md). + +## Crates + +The repository is a Cargo workspace of three crates, plus a separate Python package. + +| crate | kind | imported as | version | role | +|---|---|---|---|---| +| `strapdown-core` | library | `strapdown` | 1.0.0 | mechanization, filters, measurement models, event streams, simulation framework | +| `strapdown-sim` | binary | -- | 1.0.0 | the `strapdown-sim` command-line tool | +| `strapdown-geonav` | library | `geonav` | 0.1.0, experimental | geophysical map aiding | +| `analysis` | Python package | `analyze` CLI | -- | preprocessing and post-processing; not a Cargo member ([Python Analysis Tooling](./analysis.md)) | + +```text + +--------------------+ + | strapdown-sim | binary: CLI, config files, CSV output, + | (bin) | parallel runs (rayon), plots (plotters) + +----+----------+----+ + | | optional, feature "geonav" + | v + | +------------------+ + | | strapdown-geonav | maps (netcdf), WMM, + | | (lib "geonav") | gravity / magnetic models + | +--------+---------+ + | | + v v + +--------------------+ + | strapdown-core | nalgebra, serde, rand, csv, WMM; + | (lib "strapdown") | optional hdf5 / netcdf / mcap / clap + +--------------------+ +``` + +`strapdown-core` depends on no other workspace crate and needs no C toolchain in its default +configuration. `strapdown-sim` enables core's `clap` feature for its argument enums. +`strapdown-geonav` always links libnetcdf (built from source), which is why `strapdown-sim` +only pulls it in behind the `geonav` feature. + +## Layers inside `strapdown-core` + +| layer | modules | what lives there | +|---|---|---| +| foundations | `earth`, `linalg`, `error` | WGS84 geometry and gravity, covariance square roots, `StrapdownError` | +| mechanization | crate root (`lib.rs`) | `StrapdownState`, `IMUData`, `forward`: the 9-state strapdown equations of Groves §5.4–5.5 | +| initialisation | `alignment`, `calibration`, `stationary` | coarse alignment, IMU calibration, stationary detection for ZUPT/ZARU | +| measurements | `measurements`, `gating` | the `MeasurementModel` trait and its models; innovation gating | +| estimation | `kalman`, `linearize`, `particle`, `rbpf` | ESKF, EKF, UKF, the RBPF, and the Jacobians they share | +| framework | `messages`, `sim`, `metrics`, `engine` | event streams, records and results, runners, scoring, the `InsEngine` facade | + +Lower layers do not know about higher ones: the mechanization knows nothing about filters, and +the filters know nothing about files or schedules. + +## Data flow + +Every simulation mode is the same pipeline with a different estimator in the middle: + +```text + input.csv + | TestDataRecord::from_csv (sim) + v + Vec one row per sample: IMU, GNSS, barometer, magnetometer, ... + | check_declared_frame(records, is_enu) (sim; rejects a wrong NED/ENU declaration) + | + | build_event_stream(records, &AidingConfig, is_enu) (messages) + | - MeasurementScheduler per channel: GNSS, barometer, magnetometer -> *when* + | - GnssFaultModel on each delivered GNSS fix -> *what* + | - seeded RNG from AidingConfig::seed + | geonav::build_event_stream adds gravity/magnetic events on top (geonav, optional) + v + EventStream { start_time, events: Vec } + | Event::Imu { dt_s, imu, elapsed_s } + | Event::Measurement { meas: Box, elapsed_s } + | + | run_closed_loop(&mut filter, stream, health_limits, execution_limits) (sim) + | for each event: Imu -> filter.predict(imu, dt) + | Measurement -> filter.update(meas) (gated, NIS reported) + | HealthMonitor and ExecutionMonitor can stop a diverging or stalled run + v + Vec one row per timestamp: state, IMU and map biases, covariance + | NavigationResult::to_csv (sim; to_hdf5 / to_netcdf / to_mcap in the library) + v + output.csv +``` + +The variations are: + +- **Dead reckoning** (`dr`) skips the event stream: `sim::dead_reckoning(records, is_enu)` + mechanizes the IMU rows directly and reports no covariance. +- **Closed loop** (`cl`) builds one of the Kalman filters and calls `run_closed_loop`; + with geophysical maps it calls `run_closed_loop_with_geo`, which also reports the map biases. +- **Particle filter** (`pf`) consumes the same `EventStream` through an event loop in + `strapdown-sim` that drives the RBPF. +- **Synthetic** (`syn`) runs the other way: `sim::generate_synthetic` produces both a truth + trajectory and the sensor records a vehicle on it would have logged, and writes the records + as a CSV that the other modes can read. +- **Online use** from Rust does not need a stream at all: `InsEngine` wraps any + `NavigationFilter` and accepts IMU samples and GNSS fixes one at a time. See + [Using the Library](../user-guide/library.md). + +### Where degradation sits + +Scheduling and fault injection happen **once, in `build_event_stream`**, before any filter +runs. The filter never knows a fix was dropped or corrupted; it sees only the events it is +given. That is what makes comparisons fair: every filter run on the same records with the same +`AidingConfig` sees an identical stream, and swapping the filter changes nothing upstream. It +is also why `strapdown-geonav` delegates to core's builder rather than reimplementing it, and +why `geonav/tests/builder_equivalence.rs` checks that the two produce the same non-geophysical +events. See [Schedulers and Faults Reference](../gnss/scenarios.md). + +## Trait boundaries + +| trait | defined in | implemented by | contract | +|---|---|---|---| +| `NavigationFilter` | crate root | ESKF, EKF, UKF, RBPF | `predict(&dyn InputModel, dt)`, `update(&dyn MeasurementModel)` returning an `UpdateOutcome`, `get_estimate`, `get_certainty`, and optional gating hooks | +| `InputModel` | crate root | `IMUData`, `ImuSample`, `VelocityData` | the control input to `predict`, taken as a trait object so that `NavigationFilter` is object-safe | +| `MeasurementModel` | `measurements` | GNSS, barometer, magnetometer, ZUPT, ZARU; geonav's gravity and magnetic models | the measurement, its prediction from a state, its noise and its Jacobian | +| `Particle` | `particle` | nothing in the workspace; it is for user-built particle filters | the hooks the generic resampling and averaging strategies in `particle` need | + +Adding a sensor means implementing `MeasurementModel` and emitting events for it; adding an +estimator means implementing `NavigationFilter`. Neither requires touching the other, the +event stream, or the runner. + +## Errors + +Library code never panics. `unwrap_used`, `expect_used` and `panic` are denied for library code +(tests are exempt), and every fallible function returns `Result<_, StrapdownError>`, documented +with an `# Errors` section. `StrapdownError` is one enum with variants for shape errors, +non-finite values, invalid configuration, map loading, divergence, sensor gaps and timeouts. + +`StrapdownError::is_recoverable` separates the conditions a run should skip from the ones that +should stop it. An estimate off a geophysical map, a measurement that cannot be evaluated, and +an external model (the World Magnetic Model) refusing a query are recoverable: the runner +skips that measurement and counts it. Everything else ends the run with the error. + +The lints that enforce this are workspace-wide and at `deny`, including `clippy::pedantic`, +`clippy::nursery` and `missing_docs`; see [Building and Testing](./building.md#linting-and-formatting). + +## Determinism + +A run is a function of its inputs and its seeds. There are no wall-clock or thread-dependent +inputs to the navigation solution: + +- the GNSS fault model draws from a `StdRng` seeded from `AidingConfig::seed` (`[aiding] seed` + in a config file, `--seed` on the command line); +- the RBPF seeds its own `StdRng` from its configuration's seed; +- `generate_synthetic` takes the seeded RNG it draws from. + +`core/tests/integration_tests.rs` checks that repeated runs are identical, and the +[performance baseline](./performance.md) is gated on all three CI platforms, which catches +results that depend on the platform. `--parallel` processes *files* concurrently; each file's +run is still sequential. + +## Feature gating + +| crate | feature | default | adds | +|---|---|---|---| +| `strapdown-core` | `clap` | off | `clap` derives on configuration enums | +| `strapdown-core` | `hdf5`, `netcdf`, `mcap` | off | binary I/O on `TestDataRecord` and `NavigationResult` | +| `strapdown-core` | `full` | off | all four | +| `strapdown-sim` | `plotting` | **on** | the `--plot` figure; loads libfontconfig at run time | +| `strapdown-sim` | `geonav` | off | `--geo` and the `[geophysical]` section | + +Because an item used only behind a feature is dead code without it, CI lints and tests both +`--all-features` and core's `--no-default-features`; see +[Building and Testing](./building.md#linting-and-formatting). diff --git a/book/src/development/building.md b/book/src/development/building.md index de5868f8..60ee6887 100644 --- a/book/src/development/building.md +++ b/book/src/development/building.md @@ -1,323 +1,255 @@ # Building and Testing -## Building the Project +## Building the project -### Build Dependencies +### Build dependencies There are no system libraries to install. libhdf5, libnetcdf, zlib and freetype are compiled from vendored sources that ship as cargo dependencies, so the only requirement beyond Rust is a toolchain to build them with: + - a C/C++ compiler -- **cmake 3.26 or newer** (more than Ubuntu 22.04 or Debian 12 ship -- see +- **cmake 3.26 or newer** (newer than Ubuntu 22.04 or Debian 12 ship; see [System Requirements](../installation/requirements.md)) -On Ubuntu/Debian systems: +On Ubuntu/Debian systems with a recent enough release: + ```bash sudo apt update sudo apt install -y build-essential cmake ``` Neither is needed for `cargo build -p strapdown-core`, which uses no C library by default. +Keep `HDF5_DIR` unset: when it is set, the HDF5 build looks for a system library even though a +vendored one was requested, and the netCDF build then fails against the wrong headers. -### Rust Toolchain -- Pinned to 1.91 by `rust-toolchain.toml`; rustup fetches it on the first cargo command -- Required components: `clippy`, `rustfmt` (also declared in `rust-toolchain.toml`) +### Rust toolchain -### Building +`rust-toolchain.toml` pins Rust 1.91 with the `clippy`, `rustfmt`, `rust-src` and +`rust-analyzer` components; rustup fetches it on the first cargo command. The pin matters for +linting, because `clippy::pedantic` and `clippy::nursery` gain lints with every release. -Build the entire workspace: -```bash -cargo build --workspace --all-features -``` - -Build a specific crate: -```bash -cargo build -p strapdown-core -cargo build -p strapdown-sim -cargo build -p strapdown-geonav -``` +### Building -Build with optimizations (release mode): ```bash -cargo build --workspace --all-features --release +cargo build --workspace --all-features # everything +cargo build -p strapdown-core # the library alone, no C toolchain needed +cargo build --release -p strapdown-sim # the CLI +cargo build --release -p strapdown-sim --features geonav # the CLI with --geo ``` -### Installing Binaries +### Installing the binary -Install the simulation binaries to your system: ```bash -# Install strapdown-sim -cargo install --path sim - -# ...with geophysical navigation. This is what pulls in libnetcdf, so it needs cmake. -cargo install --path sim --features geonav +cargo install --path sim # from a checkout +cargo install --path sim --features geonav # with geophysical navigation ``` ## Testing -### Overview - -The strapdown-rs project includes comprehensive unit tests, integration tests, and module-specific tests to validate the correctness of the navigation algorithms. - -### Running Tests - -Run all tests in the workspace: ```bash -cargo test --workspace --all-features --verbose +cargo test --workspace --all-features # the whole suite, as CI's `full` job runs it +cargo test-min # strapdown-core with no default features, as CI's `minimal` job runs it +cargo test -p strapdown-core # one crate ``` -Run tests for a specific crate: -```bash -cargo test -p strapdown-core -cargo test -p strapdown-sim -cargo test -p strapdown-geonav -``` +Both of the first two are gates in CI, and they test different code: an item used only behind +a feature is live in one configuration and dead in the other. -Run with output visible: -```bash -cargo test -- --nocapture -``` - -Run tests sequentially (single-threaded): -```bash -cargo test -- --test-threads=1 -``` +### Where the tests are -### Test Organization +Unit tests sit beside the code in `#[cfg(test)]` modules in every crate. The integration tests +are: -Tests are organized into: -- **Unit tests**: Inline in source files using `#[cfg(test)]` modules -- **Integration tests**: Located in `core/tests/integration_tests.rs` -- **Module tests**: Specific to individual modules +| file | what it checks | +|---|---| +| `core/tests/integration_tests.rs` | every filter end to end on the real recording `test_data.csv` (see below) | +| `core/tests/filter_comparison.rs` | the ESKF, EKF, UKF and RBPF driven through the same `NavigationFilter` interface on one shared scenario | +| `core/tests/perf_baseline.rs` | the navigation-accuracy regression gate against `perf_baseline.json`; see [Performance Baselines](./performance.md) | +| `core/tests/aiding.rs` | innovation gating and ZUPT/ZARU aiding over whole runs | +| `core/tests/baro_bias.rs` | that the barometric bias state is actually estimated, in all three Kalman filters | +| `core/tests/engine_lever_arm.rs` | `InsEngine` and antenna lever-arm compensation, on synthetic and real data | +| `core/tests/jacobian_agreement.rs` | that the analytic transition Jacobians agree with the mechanization they linearize | +| `core/tests/ukf_conditioning.rs` | that a one-ulp change to the UKF's input cannot move its answer by a metre | +| `core/tests/rbpf_depletion.rs` | that the RBPF's particle cloud does not collapse to a point when its weights degenerate | +| `core/tests/example_configs.rs` | that every config in `examples/configs/` and `conf/` deserializes into the scenario it describes, and that paired `conf/` recipes agree | +| `core/tests/config_serde_defaults.rs` | that each configuration type's `Default` and its serde defaults agree | +| `geonav/tests/geo_closed_loop.rs` | geophysically aided closed-loop runs | +| `geonav/tests/builder_equivalence.rs` | that `geonav::build_event_stream` produces the same non-geophysical events as core's | -### Integration Tests for INS Filters +`core/tests/` also holds the two data files, `test_data.csv` and `perf_baseline.json`. The +Python package has its own tests, run with `uv run pytest -q` (see +[Python Analysis Tooling](./analysis.md)). -The integration tests validate the entire INS filter pipeline using real sensor data. These tests are comprehensive and take longer to run than unit tests. +### The real-data integration tests -#### Test Data +#### Test data -The integration tests use real data collected from the Sensor Logger mobile application: -- IMU measurements (accelerometer and gyroscope) at ~100 Hz -- GNSS position and velocity measurements at ~1 Hz -- Approximately 90 minutes of data with ~5366 samples -- Dataset location: `core/tests/test_data.csv` +`core/tests/test_data.csv` is a Sensor Logger recording from a consumer phone: +**5,366 records at 1 Hz**, spanning about 89 minutes, with IMU, GNSS, barometer, magnetometer +and orientation columns in the ENU convention. IMU and GNSS share the 1 Hz rows. -#### Error Metrics +#### Error metrics -##### Horizontal Position Error -- **Metric**: Haversine distance between estimated and GNSS positions (meters) -- **Formula**: Great-circle distance on Earth's surface -- **Purpose**: Measures planar navigation accuracy +`compute_error_metrics` in the test file matches each navigation result to the GNSS fix at +the same timestamp, skips rows without a valid fix, and reports mean, maximum and RMS of: -##### Altitude Error -- **Metric**: Simple absolute difference (meters) -- **Purpose**: Measures vertical navigation accuracy +- horizontal error: the haversine distance between estimate and fix, in metres; +- altitude error: the absolute difference, in metres; +- velocity error: per component, in m/s. -##### Velocity Error -- **Metric**: Component-wise absolute differences for north, east, and down velocities (m/s) -- **Purpose**: Measures velocity estimation accuracy +Note that the reference here is the GNSS fix, which is also what the filters are aided by; see +the first caveat in [Performance Baselines](./performance.md) for what that does and does not +measure. -#### Test Suite +#### The suite -`core/tests/integration_tests.rs` holds 23 tests, not the handful this page used to walk -through one by one. That walkthrough transcribed each test's thresholds into prose, and the -prose then drifted from the constants -- it was still quoting a 30 m horizontal limit long after -the code moved to 40 m. So this section names the groups and the constants and points at the -source, rather than copying numbers that nothing keeps honest. +`core/tests/integration_tests.rs` holds 23 tests at the time of writing; +`cargo test -p strapdown-core --test integration_tests -- --list` prints the current set. This +section names the groups and points at the source rather than transcribing thresholds, which +is how an earlier version of this page came to quote a limit the code had long since changed. -**Per-filter closed-loop suites.** Each of the four filters gets the same three scenarios: +**Per-filter closed-loop tests:** -| | full-rate GNSS | degraded GNSS (5 s) | beats dead reckoning | +| | full-rate GNSS | reduced-rate GNSS | beats dead reckoning | |---|---|---|---| -| UKF | `test_ukf_closed_loop_on_real_data` | `test_ukf_with_degraded_gnss` | `test_ukf_outperforms_dead_reckoning` | -| EKF | `test_ekf_closed_loop_on_real_data` | `test_ekf_with_degraded_gnss` | `test_ekf_outperforms_dead_reckoning` | -| ESKF | `test_eskf_closed_loop_on_real_data` | `test_eskf_with_degraded_gnss` | `test_eskf_outperforms_dead_reckoning` | -| RBPF | `test_rbpf_closed_loop_on_real_data` | `test_rbpf_with_degraded_gnss` | -- | +| UKF | `test_ukf_closed_loop_on_real_data` | `test_ukf_with_degraded_gnss` (5 s) | `test_ukf_outperforms_dead_reckoning` | +| EKF | `test_ekf_closed_loop_on_real_data` | `test_ekf_with_degraded_gnss` (5 s) | `test_ekf_outperforms_dead_reckoning` | +| ESKF | `test_eskf_closed_loop_on_real_data` | `test_eskf_with_degraded_gnss` (2 s) | `test_eskf_outperforms_dead_reckoning` | +| RBPF | `test_rbpf_closed_loop_on_real_data` | `test_rbpf_with_degraded_gnss` (5 s) | -- | -**Plus**: `test_dead_reckoning_on_real_data` (the unaided baseline), +**Others:** `test_dead_reckoning_on_real_data` (the unaided baseline), +`test_filter_comparison` (UKF, EKF and ESKF on the same data), `test_eskf_output_stays_valid_across_full_run`, `test_eskf_default_initialization_on_real_data`, `test_eskf_auto_covariance_initialization_on_real_data`, `test_eskf_recovers_from_gnss_outage`, `test_filter_output_length_matches_input`, `test_filters_are_deterministic_across_runs`, `test_rmse_benchmark_across_filters`, `test_full_lifecycle_through_ins_engine`, -`magnetometer_yaw_aiding_source_error_matches_derivation`, and -`gating_through_the_closed_loop_no_longer_cascades`. +`magnetometer_yaw_aiding_source_error_matches_derivation` and +`gating_through_the_closed_loop_no_longer_cascades`. The last is marked `#[ignore]`, with the +reason in its attribute and doc comment, so a plain run reports 22 passed and 1 ignored; add +`-- --ignored` to run it. #### Thresholds -The limits live in `core/tests/integration_tests.rs` as named constants, each with its +The limits are named constants at the top of `core/tests/integration_tests.rs`, each with its derivation in a doc comment. Read those rather than any number on this page: | constant | what bounds it | |---|---| | `MAX_HORIZONTAL_RMSE_M` | empirical, with headroom over the observed run | | `MAX_VERTICAL_RMSE_M` | empirical | -| `MAX_LEVEL_ATTITUDE_RMSE_RAD` | **derived** -- gravity observability | -| `MAX_YAW_RMSE_RAD` | **derived** -- a multiple of `MAG_YAW_SOURCE_RMSE_RAD`, the magnetometer's own measured error | -| `DEAD_RECKONING_BASELINE_SAMPLES` | **derived** -- see below | -| `DEAD_RECKONING_BEAT_FACTOR` | **derived** -- how far a filter must beat the baseline for the comparison to be live | +| `MAX_LEVEL_ATTITUDE_RMSE_RAD` | **derived**: gravity observability | +| `MAX_YAW_RMSE_RAD` | **derived**: a multiple of `MAG_YAW_SOURCE_RMSE_RAD`, the magnetometer's own measured error | +| `DEAD_RECKONING_BASELINE_SAMPLES` | **derived**: see below | +| `DEAD_RECKONING_BEAT_FACTOR` | **derived**: how far a filter must beat the baseline for the comparison to mean anything | The distinction matters. An empirical guard answers "is this worse than yesterday?"; a derived -one answers "is this physically possible?". **Do not tighten a derived bound to match a measured -number** -- if a filter beats it, the bound was wrong, and the derivation is what changes. See -`AGENTS.md` for the policy. +one answers "is this physically possible?". **Do not tighten a derived bound to match a +measured number**: if a filter beats it, the derivation is what changes. `AGENTS.md` has the +policy. #### The dead-reckoning baseline is a window, not the whole run -The `*_outperforms_dead_reckoning` tests score the comparison over -`DEAD_RECKONING_BASELINE_SAMPLES` records, not the full 90-minute recording. This page -previously advertised the full-run version -- "dead reckoning RMS error ~7,100 km, improvement ->99.99%" -- and that headline is exactly what the truncation exists to kill. - -Unaided dead reckoning over the whole log ends millions of metres out. A test asserting -`filter_error < dead_reckoning_error` against that is vacuous: it passes for any filter that -does not itself diverge, including a badly broken one. Truncating to a window where the baseline -drift is comparable to the filter's error makes the comparison discriminating again, and -`DEAD_RECKONING_BEAT_FACTOR` then requires a real margin rather than any margin. See #307 and -#299, and the derivation in the module header. +The `*_outperforms_dead_reckoning` tests compare over the first +`DEAD_RECKONING_BASELINE_SAMPLES` records, not the whole recording. Unaided dead reckoning over +the full 89 minutes ends millions of metres out, and a test asserting +`filter_error < dead_reckoning_error` against that passes for any filter that does not itself +diverge, including a badly broken one. Truncating to a window where the baseline drift is +comparable to the filter's error makes the comparison discriminating, and +`DEAD_RECKONING_BEAT_FACTOR` then requires a real margin. See #307 and #299. -#### Running Integration Tests +#### Running them ```bash -cargo test -p strapdown-core --test integration_tests # all of them -cargo test -p strapdown-core --test integration_tests -- --nocapture # with printed output +cargo test -p strapdown-core --test integration_tests # all of them +cargo test -p strapdown-core --test integration_tests -- --nocapture # with printed output cargo test -p strapdown-core --test integration_tests test_ukf_closed_loop_on_real_data -- --nocapture -cargo test -p strapdown-core --test integration_tests -- --test-threads=1 # sequential ``` -#### Expected Runtime - -Deliberately not itemised here. The previous figures covered four tests where there are now 22, -and predated the baseline truncation above, so they were wrong in both directions and nothing -noticed. Nothing in CI measures wall clock today -- that is the open half of #377 -- so treat -any runtime number as an observation you made, not a contract. What *is* bounded is CI itself: -every job sets `timeout-minutes` (#379). - - -#### Implementation Details - -##### Error Metric Calculation +No runtime is given here because none is measured: nothing in CI tracks wall-clock time (the +open half of #377). What is bounded is CI itself: every job sets `timeout-minutes`. -The `compute_error_metrics()` function: -1. Matches navigation results to GNSS measurements by timestamp -2. Skips invalid GNSS data (NaN values) -3. Computes error for each matched sample -4. Filters out non-finite errors -5. Calculates mean, max, and RMS statistics +## Linting and formatting -##### Filter Initialization +The lints are enforced at `deny`, and CI lints two feature configurations. `.cargo/config.toml` +defines an alias for each command CI runs: -Tests use realistic initialization based on first GNSS measurement: -- Position: First GNSS lat/lon/alt -- Velocity: Computed from GNSS speed and bearing -- Attitude: From phone orientation sensors -- Covariances: Conservative initial uncertainties -- Process noise: Tuned for MEMS IMU characteristics +| alias | expands to | CI job | +|---|---|---| +| `cargo fmt-check` | `cargo fmt --all -- --check` | `full` | +| `cargo lint` | `cargo clippy --workspace --all-targets --all-features -- -D warnings` | `full` | +| `cargo lint-min` | `cargo clippy -p strapdown-core --all-targets --no-default-features -- -D warnings` | `minimal` | +| `cargo test-min` | `cargo test -p strapdown-core --no-default-features` | `minimal` | -##### Event Stream Generation +Run all four, plus `cargo test --workspace --all-features`, before pushing. `cargo lint` alone +is not enough: an import used only under `--all-features` is dead under +`--no-default-features`, and only `cargo lint-min` sees it. `cargo lint-fix` applies the fixes +clippy can make itself, and `cargo fmt --all` formats. -Tests use the `build_event_stream()` function to create a sequence of IMU propagation and GNSS update events from the raw data, with configurable scheduling and fault injection. +`clippy::pedantic`, `clippy::nursery`, `missing_docs` and the zero-panic lints +(`unwrap_used`, `expect_used`, `panic`, in library code) are all denied. Relax a lint once, in +`[workspace.lints.clippy]` in the root `Cargo.toml` with a reason, rather than with a call-site +`#[allow]`. `AGENTS.md` lists the existing exceptions and why. -### Future Test Enhancements +The Python half has its own gate: `just check-python` runs `uv run ruff check`, +`uv run ruff format --check` and `uv run pytest -q`. -Potential improvements for the test suite: - -1. ~~**Additional test scenarios**~~ -- **done.** `core/tests/perf_baseline.rs` covers GNSS - outages (`real_outage_60s`, `syn_outage_60s`, both `DutyCycle`), measurement corruption - (`real_degraded`, an AR(1) fault model) and a synthetic cruise profile alongside the real - recording. - -2. ~~**More filters**~~ -- **done.** The EKF is implemented (`core/src/kalman.rs`) and has the - same three-scenario integration suite as the others; the RBPF has - `test_rbpf_closed_loop_on_real_data` and `test_rbpf_with_degraded_gnss`, and its own row in - the performance baseline. - -3. **Performance benchmarks**: *accuracy* regression is done -- `core/tests/perf_baseline.rs` - gates every filter against a checked-in baseline in both directions, and - [Performance Baselines](./performance.md) has the current numbers. What remains is the - *wall-clock* half, which the accuracy gate deliberately does not measure: - - Execution time metrics - - Memory usage tracking - - Scalability tests - -4. **Shorter test datasets**: - - Create focused test datasets for faster CI/CD - - Keep full dataset for comprehensive validation - -## Linting and Formatting +## Documentation -### Running Clippy +### API documentation -Clippy provides lint checks for Rust code: ```bash -cargo clippy --workspace --all-features +cargo docs # alias for cargo doc --workspace --no-deps +cargo doc --workspace --no-deps --open # and open it in a browser ``` -Address warnings and errors before committing. +See [API Documentation](../api/index.md) for what each crate contains and where the published +copy lives. -### Running rustfmt +### Building the book -Format code with rustfmt: -```bash -cargo fmt --all -``` +The book is built with mdBook. CI pins version 0.4.40, so install that one: -Check formatting without making changes: ```bash -cargo fmt --all -- --check +cargo install mdbook --version 0.4.40 --locked +mdbook build book # output in book/book/ +mdbook serve book # live-reloading server on http://localhost:3000 ``` -## Documentation +Run them from the repository root. Pages under `book/src/` include files from elsewhere in the +repository (Rust examples by anchor, `CONTRIBUTING.md`, the generated +`development/baseline-tables.md`), so an edit to those files changes the book too. -### Building API Documentation +### Publishing the book -Generate documentation for all crates: -```bash -cargo doc --workspace --all-features --no-deps -``` +Publishing is automatic. `.github/workflows/deploy-book.yml` builds the book on every push to +`main` (and on pull requests that touch the book or the crates, without deploying), adds the +workspace rustdoc under `api/`, checks the links, and deploys the result to GitHub Pages at +. -Open the documentation in a browser: -```bash -cargo doc --workspace --all-features --no-deps --open -``` +For that to work, the repository's **Settings → Pages → Build and deployment → Source** must be +set to **GitHub Actions**; that is a one-time setting. There is no custom domain: the `cname` +line in `book/book.toml` is commented out, so the site is served from the default +`github.io` address. -### Building the Book +## Continuous integration -This user guide is built using mdBook. Install mdBook: -```bash -cargo install mdbook -``` - -Build the book: -```bash -cd book -mdbook build -``` - -Serve the book locally for development: -```bash -cd book -mdbook serve -``` - -Then open http://localhost:3000 in your browser. +The workflows in `.github/workflows/`: -## Continuous Integration - -The project uses GitHub Actions for continuous integration. See `.github/workflows/` for workflow definitions: - -- `rust.yml`: the blocking gate. Three jobs -- `minimal` (`strapdown-core`, no default - features, on Linux/macOS/Windows), `full` (fmt, clippy `-D warnings`, and the whole suite - with all features, Linux), and `vendored` (the macOS/Windows canary for the source builds of - netCDF and HDF5) -- `deploy-book.yml`: builds and deploys this documentation to GitHub Pages -- `publish.yml`: verifies, then publishes the crates to crates.io. Takes a `dry_run` input -- `draft-pdf.yml`: renders the JOSS paper, on the `joss` branch -- `copilot-setup-steps.yml`: toolchain provisioning only +| workflow | what it does | +|---|---| +| `rust.yml` | the blocking Rust gate. `minimal`: `strapdown-core` with no default features (build, `clippy -D warnings`, test) on Linux, macOS and Windows. `full`: format check, `clippy -D warnings` and the whole test suite with all features, on Linux. `vendored`: builds the vendored netCDF/HDF5 on macOS and Windows. Two advisory jobs: `platform-spread`, which reports how far the accuracy metrics differ across the three platforms, and `semver`, which runs `cargo semver-checks` against the last release | +| `python.yml` | lints and tests the `analysis` package (`ruff check`, `ruff format --check`, `pytest`), deliberately without GMT installed | +| `deploy-book.yml` | builds this book and the rustdoc, checks links, and deploys to GitHub Pages | +| `publish.yml` | verifies, then publishes the crates to crates.io on a release; a manual run takes a `dry_run` input | +| `draft-pdf.yml` | renders the JOSS paper in `papers/joss/` when it changes | +| `copilot-setup-steps.yml` | toolchain provisioning for the Copilot coding agent only | -Every job sets `timeout-minutes` (#379), so a hang fails rather than running to GitHub's -360-minute default. +Every job sets `timeout-minutes`, so a hang fails instead of running to GitHub's 360-minute +default. ## References -- Groves, P. D. (2013). *Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems*, 2nd ed. -- Sensor Logger app: https://www.tszheichoi.com/sensorlogger +- Groves, P. D. (2013). *Principles of GNSS, Inertial, and Multisensor Integrated Navigation + Systems*, 2nd ed. Artech House. +- Sensor Logger app: diff --git a/book/src/development/contributing.md b/book/src/development/contributing.md index 854139a3..d759ebfc 100644 --- a/book/src/development/contributing.md +++ b/book/src/development/contributing.md @@ -1 +1,9 @@ -# Contributing +{{#include ../../../CONTRIBUTING.md}} + +--- + +*This page is the repository's +[`CONTRIBUTING.md`](https://github.com/jbrodovsky/strapdown-rs/blob/main/CONTRIBUTING.md), +included verbatim so the two cannot drift. [Building and Testing](./building.md) covers the +build, the test suites and the full set of lint commands, including the no-default-features +configuration (`cargo lint-min`, `cargo test-min`) that CI also gates on.* diff --git a/book/src/development/performance.md b/book/src/development/performance.md index 3fd218fa..e29afc36 100644 --- a/book/src/development/performance.md +++ b/book/src/development/performance.md @@ -1,12 +1,11 @@ # Performance Baselines -"Performance" on this page means **navigation accuracy**, not wall-clock speed. How far the +"Performance" on this page means **navigation accuracy**, not wall-clock speed: how far the solution was from truth, and whether the filter's covariance knew it. Execution time, memory -and scalability are tracked nowhere yet; see [Building and Testing](./building.md) for what -remains. +and scalability are not measured anywhere yet. -Every number below is recorded in `core/tests/perf_baseline.json` and checked on every CI run -by `core/tests/perf_baseline.rs`. The check fails in **both** directions: +Every number here is recorded in `core/tests/perf_baseline.json` and checked on every CI run by +`core/tests/perf_baseline.rs`. The check fails in **both** directions: - a metric more than **10%** worse than its baseline is a regression, and CI goes red; - a metric more than **25%** better is *also* a failure, and the message asks for the baseline @@ -14,452 +13,163 @@ by `core/tests/perf_baseline.rs`. The check fails in **both** directions: Each scenario also records the number of aligned samples it was scored over and the number of channel-samples dropped as non-finite, both compared for exact equality before any metric is. -The second is zero everywhere and is recorded because dropping a sample does not lower the +The second is zero everywhere. It is recorded because dropping a sample does not lower the first, but does make the metric it was dropped from look better. -Re-bless with `UPDATE_PERF_BASELINE=1 cargo test -p strapdown-core --test perf_baseline`, then -commit the diff; `cargo perf` runs the suite and prints the table without writing anything. -[CONTRIBUTING.md](https://github.com/jbrodovsky/strapdown-rs/blob/main/CONTRIBUTING.md) has the -workflow. +## Running and re-blessing -## Read these seven caveats first +```bash +cargo perf # run the suite and print the table; writes nothing +UPDATE_PERF_BASELINE=1 cargo test -p strapdown-core --test perf_baseline # re-bless +``` -The numbers are meaningless without them. All but the fifth and seventh are properties of the -measurement rather than defects in the harness; the fifth is a defect in the gate, and the -seventh was a defect in the UKF that is now fixed. +In PowerShell the second is `$env:UPDATE_PERF_BASELINE=1; cargo test -p strapdown-core --test +perf_baseline`. Re-blessing rewrites `core/tests/perf_baseline.json` *and* regenerates +`book/src/development/baseline-tables.md`, which this page includes below, and the gate fails if +the two disagree, so there is no second step to forget. It preserves the per-metric `note`, +`gated` and tolerance overrides already in the file. Review the diff before committing it: +every changed number is a claim about the navigation solution, and the pull request should +say which change produced it. [Contributing](./contributing.md) has the workflow. -1. **On the real-data scenarios, "truth" is the GNSS fix -- which is also the filters' aiding - source.** They measure agreement with the aid, not independent accuracy, and cannot fall - below the receiver's own 3.81 m horizontal noise however good the filter is. The `syn_*` - scenarios are scored against an exact synthetic trajectory and have no such floor, which is - why their horizontal figures are five times smaller. -2. **On a full-rate real-data row, the filter is scored against a fix it has already been - given.** This used to read differently: every horizontal figure carried a one-step - propagation offset, because `sim::run_closed_loop` pushed each row *after* applying the - next event, so the row labelled $t_k$ held a state already propagated to $t_{k+1}$. At - 1 Hz and 21.19 m/s that was 21.2 m of along-track error -- very nearly the whole of the - ~23.5 m the `real_clean` rows then showed, and the reason all three filters landed within - 0.2 m of each other. [#367](https://github.com/jbrodovsky/strapdown-rs/issues/367) fixed - it, and every horizontal metric improved at once. - - What that uncovered is caveat 1 with nothing left diluting it. A row at $t_k$ now contains - $t_k$'s GNSS update, so on a `PassThrough` schedule these columns measure how completely a - filter absorbs its own aiding. - - That absorption used to be total: `real_clean__ukf` read 0.014 m and `real_clean__ekf` - 0.0001 m, far below the receiver's own 3.81 m. Not accuracy — a Kalman gain of about 1, - caused by an absolute covariance floor applied to a latitude variance in radians². - [#373](https://github.com/jbrodovsky/strapdown-rs/issues/373) **fixed it**, and the three - filters now read **4.78, 5.22 and 5.12 m** — above the receiver's own noise rather than - reproducing it, and in agreement with the ESKF, which had floored its covariance relatively - since #266. - - **The real-data rows that still measure navigation are the ones where the filter has to - predict between fixes** -- `real_sparse_5s`, `real_outage_60s`, `real_degraded` -- together - with the `syn_*` scenarios and their exact independent truth. -3. **The synthetic scenarios now carry a real magnetic field, and the yaw column says which - filters can use it.** `generate_synthetic` evaluates the World Magnetic Model at each - epoch's position and date and rotates it into the body frame through the truth attitude - ([#369](https://github.com/jbrodovsky/strapdown-rs/issues/369)); before that it wrote - nothing, and heading was observable only through the GNSS velocity fix. With a 1 Hz heading - aid all three filters now hold a fraction of a degree of yaw on `syn_cruise_1hz` -- the EKF - 0.28, the ESKF 0.18, the UKF 0.16, the last down from 108.3 deg without a magnetometer at - all. - - **`syn_outage_60s__ukf` used to be where that stopped, and the stated reason was wrong.** - With the field added that row read 72.68 deg of yaw, 809 m horizontal and a NEES of 44.67, - against the ESKF's 0.255 deg and 24.0 m on the identical stream, and this page cited that - pair as evidence for [#371](https://github.com/jbrodovsky/strapdown-rs/issues/371). It was - not evidence for anything: the three filter constructors shipped initial gyro-bias variances - five orders of magnitude apart -- `1e-3` for the UKF and the EKF against `1e-8` for the - ESKF -- so the two filters were never handed the same prior. `1e-3` is a gyro-bias sigma of - 1.81 deg/s against the 0.028 deg/s a consumer-grade part actually has. - - Sizing all three from the IMU grade takes that row to **0.222 deg of yaw, 27.4 m - horizontal, NEES 4.21 and 3-sigma horizontal containment 0.997** against an ideal of 0.9973 - -- past the ESKF's own numbers on the same stream, so the UKF is not incapable of carrying - an attitude through a coast. `syn_outage_60s__eskf` moved the other way and for the same - reason: its `1e-8` was about 5x *tighter* than the truth it was modelling, and correcting it - costs 24.0 -> 26.3 m of position and 0.255 -> 0.292 deg of yaw while taking its NEES - 4.64 -> 3.87 and its vertical containment 0.866 -> 0.925. Less accurate, more honest. - - #371 was a real defect -- a linear mean over sigma-point Euler triples sends 31 rotations - inside a 0.24 deg cone to a point 34 deg outside it -- but it is not what this row was - measuring. Fixing it, so the UKF perturbs, averages, correlates and injects attitude on - SO(3), moved `syn_outage_60s__ukf` yaw by **0.007 deg** (0.222 to 0.230) and improved its - position (27.39 to 27.17 m). That near-nil result on the rows with exact truth is the - finding. The two `__ekf` rows did not move at all for the prior change either, which is - also a finding rather than a null result: the EKF's bias states are structurally inert - ([#394](https://github.com/jbrodovsky/strapdown-rs/issues/394)), so its prior cannot be - falsified by any measurement. - - **The `real_*` UKF rows are the exception, and the yaw column there cannot adjudicate it.** - `real_clean__ukf` yaw went 22.72 to 25.83 deg and `real_degraded__ukf` 22.18 to 26.01 when - the chart was fixed, which reads as a 14-17% regression. Three measurements say what it - actually is. The UKF's disagreement with the **ESKF** -- the other filter here that does its - attitude arithmetic on a manifold, and which this change does not touch -- fell from 8.40 to - 2.57 deg RMS on the same recording. The recording's own magnetometer, levelled by its own - roll and pitch, disagrees with its yaw column by **16.8 deg RMS after removing every - constant offset**, which is the floor for any filter aided by it, so the 3 deg the UKF moved - is inside what that column can resolve. And on the synthetic rows, where truth is exact, the - change is neutral. Caveat 1 again, in the attitude channel. -4. **The consistency columns mean different things on the two sources.** On the synthetic - scenarios `npes` lands between 3.9 and 12.1 against an ideal of 3.0, which is a real - measurement of whether a filter believes the right thing. On the real-data scenarios it reaches the - hundreds, because caveat 2's offset enters the numerator while the covariance in the - denominator models none of it. Those values are kept as a drift detector, not read as a - consistency verdict. - - The synthetic figures roughly doubled when - [#375](https://github.com/jbrodovsky/strapdown-rs/issues/375) scheduled the barometer, and - the vertical columns say why: what a normalised statistic now sees is a systematic altitude - bias of -0.6 to -2.0 m that nothing models. That is - [#372](https://github.com/jbrodovsky/strapdown-rs/issues/372), measured rather than masked. - -5. **The baseline is blessed on one platform and gated on three.** `rust.yml` runs this suite - on Linux, macOS and Windows, and nothing in the gate knows that -- there is no - cross-platform tolerance floor. For most rows it does not matter: the same commit agrees to - about 1% across platforms. It matters where a metric is a *tail* statistic of something - unstable. `real_rbpf_slice__rbpf`'s `horizontal_cep95_m` measured 34.02 m on Linux against - 24.33 m on Windows -- a 28.5% spread against a 25% improve band -- so a Linux bless failed - the Windows leg of a run that was behaving identically. That metric is ungated with the - numbers recorded in its note; the general problem is - [#386](https://github.com/jbrodovsky/strapdown-rs/issues/386). - -**The spread is now measured, on all three legs of one run** (at the old `ukf_alpha`), by - the advisory `Cross-platform accuracy spread` job. The largest entries: - - | spread | scenario / metric | macOS | Linux | Windows | - |---:|---|---:|---:|---:| - | 10.79% | `real_rbpf_slice__rbpf/horizontal_cep50_m` | 1.82705 | 1.82705 | 1.64916 | - | 8.61% | `real_rbpf_slice__rbpf/horizontal_cep95_m` | 29.2597 | 29.2597 | 26.9394 | - | 4.70% | `syn_outage_60s__ukf/horizontal_cep50_m` | 1.64422 | 1.62557 | 1.57045 | - | 2.87% | `syn_outage_60s__ukf/roll_rmse_deg` | 0.27204 | 0.27475 | 0.26708 | - | 2.39% | `syn_outage_60s__ukf/yaw_rmse_deg` | 0.22314 | 0.22687 | 0.22158 | - - Two readings fall out. The 28.5% this caveat quotes above is now **8.61%**, so #385's fix - did shrink the spread it was blamed for -- that was the missing measurement. And it is - **Windows against the other two**, not three-way scatter: macOS and Linux agree to eleven - significant figures on the RBPF rows, which is what a libm differing by an ulp looks like - rather than anything about the navigation. - - The UKF rows there are caveat 7 seen a second way -- same rows, same order, about half the - magnitude of the one-ulp response recorded there. So caveat 7's fix predicted those rows - would collapse and the RBPF rows would not. - - **That prediction was recorded before the run, and the run confirmed it.** Same job, same - three platforms, on the commit that raised `ukf_alpha`: - - | scenario / metric | before | after | - |---|---:|---:| - | `syn_outage_60s__ukf/horizontal_cep50_m` | 4.70% | **0%** | - | `syn_outage_60s__ukf/roll_rmse_deg` | 2.87% | **0%** | - | `syn_outage_60s__ukf/yaw_rmse_deg` | 2.39% | **0%** | - | `syn_outage_60s__ukf/attitude_geodesic_rmse_deg` | 2.06% | **0%** | - | `syn_outage_60s__ukf/velocity_horizontal_rmse_mps` | 1.76% | **0%** | - | `real_rbpf_slice__rbpf/horizontal_cep50_m` | 10.79% | 10.79% | - | `real_rbpf_slice__rbpf/horizontal_cep95_m` | 8.61% | 8.61% | - - Every `syn_*__ukf` row now agrees across all three platforms to seven or eight significant - figures, and every RBPF row is unchanged to the digit. - - **So what is left of #386 is a single scenario — and it is not floating-point noise.** - `real_rbpf_slice__rbpf` holds every entry above 0%, and nothing else in the suite exceeds - it. The obvious next move is a tolerance, global or per-metric. **Both are wrong, because - the number is not understood.** - - Three one-ulp perturbations, each applied on *every* call — which is what a libm differing - in its last bit actually is: - - | perturbation | worst of 236 metrics | - |---|---:| - | every Gaussian draw in the RBPF | 1.28e-6 % | - | every particle weight, after `exp` | 3.09e-7 % | - | one WGS84 constant (caveat 7's ensemble) | 1.0e-6 % | - - The RBPF moves by about one part in 10⁸ under any of them, and `horizontal_cep50_m` — the - metric carrying the 10.79% spread — is the most responsive at 1.28e-6 %. **The RBPF is not - an amplifier**, the opposite of the UKF in caveat 7, so the residual spread cannot be a - last-bit effect. - - The platform pattern agrees. macOS and Linux match to **eight significant figures** on that - metric, across *different CPU architectures*, while Windows — same architecture as Linux — - sits 10% away. That rules out CPU architecture, SIMD width and runtime feature dispatch, - all of which would separate ARM from x86 rather than Windows from everyone. - - A 10% discrepancy on one platform and one scenario, demonstrably larger than an ulp, is a - **defect to diagnose rather than a band to widen**; fitting a tolerance to it would be - [#288](https://github.com/jbrodovsky/strapdown-rs/issues/288) again. Diagnose it by emitting - the sorted error series behind `horizontal_cep50_m` from each leg — the `PERF_EMIT_MEASURED` - plumbing is most of the way there — and by checking that `test_data.csv` parses identically - on Windows before blaming the filter. - - **Two parts of that are now closed.** The baseline records the operating system it was - blessed on (`blessed_on`), and the improve-side failure no longer tells a contributor their - baseline is stale when the run is simply on a different platform -- it names both platforms - and says to check the blessing one first. Re-blessing on the wrong leg was the specific - damage the old message caused. - - **The tolerance floor is deliberately not set yet, because nobody has measured it.** Every - matrix leg now writes its raw numbers with `PERF_EMIT_MEASURED` and uploads them, and the - advisory `Cross-platform accuracy spread` job prints the per-metric spread across all three. - Setting a floor before reading that output would be gating on a guess, which is the mistake - #386 exists to correct. Read the job's table, then set the floor from it. - - Note also that the 28.5% figure above predates - [#385](https://github.com/jbrodovsky/strapdown-rs/issues/385). That row's tail *was* the - epochs where the particle cloud collapsed, and it no longer collapses, so the spread may - already be much smaller. It has not been re-measured. Until then, **check a Windows run - before calling a re-bless done.** -7. **A one-ulp change to anything upstream used to move these numbers by percents.** This is - [#399](https://github.com/jbrodovsky/strapdown-rs/issues/399), and both halves of its - recorded diagnosis turned out to be wrong, which is worth keeping. - - The issue's experiment was to add a `pub fn` to `linearize.rs` that nothing calls and - re-bless: 59 metrics moved, one by 4.86%. Its proposed mechanism was the innovation gate -- - a continuous statistic turned into a discrete accept/reject, one flipped decision in 15,000 - steps separating two trajectories. - - **The gate was refuted by counting.** Gating is opt-in and this suite never installs one, so - across a full run all **137,876** calls to `GatePolicy::decide` took the "no gate" branch. - There was no accept/reject decision to flip. Nor was there any other discrete branch: all - 98,786 sigma-point factorizations took plain Cholesky with no jitter and no eigenvalue - fallback, no IMU-bias clamp bound in 394,740 opportunities, and the Euler-rate inverse was - admitted on all 106,518. - - **The codegen story did not reproduce either.** The dead `pub fn`, with its symbol confirmed - present in the compiled rlib, leaves all 236 metrics bit-identical. Rust emits IEEE-754 - semantics without fast-math, so LLVM may not reassociate or contract floating-point; - changing what surrounds a function cannot change what it computes. +## Scenarios - **What does reproduce is the amplification, from the data side, and it was the UKF's.** - Perturbing `earth::EQUATORIAL_RADIUS` by one ulp moved 170 of 236 metrics, the worst by - **9.77%** against a 10% band. Over six independent one-ulp perturbations of WGS84 constants: +`real` is `core/tests/test_data.csv`: 5,366 records at 1 Hz over 89 minutes, ENU, from a +consumer phone. `syn` is `sim::generate_synthetic`: 300 s at 50 Hz, NED, a 50 m/s +constant-velocity cruise (40 m/s north, 30 m/s east) from 40° N, 75° W with a consumer-grade +IMU, seed 42, scored against its own exact truth. - | estimator | worst response to one ulp | - |---|---:| - | **UKF** | **9.77%** | - | RBPF | 1.0e-6 % | - | dead reckoning | 1.7e-10 % | - | EKF | 3.8e-11 % | - | ESKF | 3.3e-11 % | +| id | data | GNSS | estimators | +|---|---|---|---| +| `real_clean` | real | every epoch | UKF, EKF, ESKF | +| `real_sparse_5s` | real | every 5 s | UKF, ESKF | +| `real_outage_60s` | real | duty cycle: 60 s off, 120 s on, repeating | ESKF | +| `real_degraded` | real | every epoch, AR(1) position and velocity fault ($\rho$ 0.98, $\sigma$ 3 m and 0.3 m/s, `r_scale` 1) | UKF, ESKF | +| `syn_cruise_1hz` | syn | every 1 s | UKF, EKF, ESKF | +| `syn_outage_60s` | syn | duty cycle: 60 s off, 60 s on, repeating | UKF, ESKF | +| `syn_dead_reckoning` | syn, first 120 s | none | unaided mechanization | +| `real_rbpf_slice` | real, first 1,200 records | every epoch | RBPF, 500 particles | - Eleven orders of magnitude on the same scenarios, which rules out "a 60 s coast compounds a - last bit": `syn_outage_60s__eskf` coasts identically and sits at 3.3e-11 %. The UKF is the - only estimator here that forms a weighted sigma-point mean, and at the `alpha = 1e-3` this - crate shipped, a 16-state filter formed it as `-999,999 x_0 + sum 31,250 x_i` -- six of - `f64`'s sixteen significant digits spent on cancellation, every step. End to end, one ulp of - initial latitude moved a 180 s solution's final position by **1.76 m**. +The two outage scenarios use `start_phase_s = 0`, so each begins with its OFF window (see +[`DutyCycle`](../gnss/scenarios.md#dutycycle)). The barometer and magnetometer are scheduled at +their 1 Hz default in every scenario, so the synthetic vertical and yaw columns are those of a +1 Hz barometer and magnetometer even though the IMU runs at 50 Hz. - **Fixed by raising `ukf_alpha` to `0.1`**, which takes that to 0.18 mm and the worst metric - response to 0.0003%, and improves accuracy slightly on the way. An equilibrated Cholesky - landed with it and is worth a further factor of two. `core/tests/ukf_conditioning.rs` guards - both ends, and the numbers above are in that file and in `ClosedLoopConfig::ukf_alpha`. +Two scenarios are deliberately absent. **Full-run dead reckoning** ends millions of metres out, +at altitudes where floating-point results have already been seen to differ between platforms; +the 120 s synthetic window is the safe version of the same measurement. **A second +particle-filter scenario** is left out to keep the suite's CI time down. - **Use a control build when a bless surprises you.** If a re-bless moves rows the change - cannot reach, compile the new code *without calling it*, bless that, and diff the real - change against the control rather than against the parent commit. On - [#394](https://github.com/jbrodovsky/strapdown-rs/issues/394) that reduced "seven scenarios - moved" to the two the change can actually touch. It is only valid if it neutralises **every** - call site -- a first attempt at #398's control missed the UKF's transport and reported half - the moved metrics. This is now insurance rather than a necessity, but it is the habit that - found every mis-attribution on this project so far. -6. **The `syn_*` rows run at 50 Hz and the `real_*` rows at 1 Hz, and the aiding sensors no - longer follow that.** Until - [#375](https://github.com/jbrodovsky/strapdown-rs/issues/375) the barometer and the - magnetometer were emitted once per record, outside the scheduler, so their update rate was - the log's: 1 Hz on `test_data.csv` and 50 Hz on the synthetic trajectories, where fifty - pressure readings a second each entered the filter as an independent fix. Both channels are - now scheduled at 1 Hz by default. Every `real_*` row came through bit-identical -- they were - already at 1 Hz -- and all five aided `syn_*` rows moved, almost entirely in the vertical - channel: horizontal RMSE on `syn_cruise_1hz` moved by under half a percent. Read the - synthetic vertical columns as a 1 Hz barometer's, not a 50 Hz one's, and the yaw - columns as a 1 Hz magnetometer's. +The suite is not behind a feature or `#[ignore]`, so it runs on all three CI platforms. That is +deliberate: the three legs double as a portability check on the accuracy numbers themselves. ## The metrics | metric | unit | definition | ideal | |---|---|---|---| | `horizontal_rmse_m` | m | RMS great-circle distance from truth | 0 | -| `horizontal_cep50_m` | m | median radial error -- the empirical, non-parametric CEP, *not* the Rayleigh form. Nearest-rank, so it is a value some epoch actually had | 0 | -| `horizontal_cep95_m` | m | 95th percentile radial error, same convention | 0 | +| `horizontal_cep50_m` | m | median radial error: the empirical, nearest-rank CEP, *not* the Rayleigh form | 0 | +| `horizontal_cep95_m` | m | 95th-percentile radial error, same convention | 0 | | `horizontal_max_m` | m | largest single-sample radial error | 0 | | `vertical_rmse_m` | m | RMS altitude error | 0 | -| `vertical_bias_m` | m | **signed** mean altitude error -- catches a slow one-sided drift while it is still small | 0 | +| `vertical_bias_m` | m | **signed** mean altitude error; catches a slow one-sided drift while it is still small | 0 | | `velocity_horizontal_rmse_mps` | m/s | RMS north/east velocity error | 0 | | `velocity_vertical_rmse_mps` | m/s | RMS vertical velocity error | 0 | | `roll_rmse_deg`, `pitch_rmse_deg` | deg | RMS per-axis error, wrapped onto $[-\pi, \pi]$ | 0 | | `yaw_rmse_deg` | deg | RMS heading error, wrapped | 0 | -| `attitude_geodesic_rmse_deg` | deg | RMS rotation angle of $R_{est}^{-1} R_{true}$ -- the only attitude error independent of the Euler sequence | 0 | -| `nees_position` | -- | mean normalized estimation error squared over the full 3x3 position block, $\frac{1}{N}\sum_k e_k^\top P_k^{-1} e_k$ | 3.0 | -| `npes_position` | -- | the same, computed as though $P$ were diagonal: $\overline{\epsilon} = \frac{1}{N}\sum_k \left(\frac{e_{lat}^2}{P_{lat}} + \frac{e_{lon}^2}{P_{lon}} + \frac{e_{alt}^2}{P_{alt}}\right)$ | 3.0 | +| `attitude_geodesic_rmse_deg` | deg | RMS rotation angle of $R_{est}^{-1} R_{true}$: the only attitude error independent of the Euler sequence | 0 | +| `nees_position` | -- | mean normalized estimation error squared over the full 3×3 position block, $\frac{1}{N}\sum_k e_k^\top P_k^{-1} e_k$ | 3.0 | +| `npes_position` | -- | the same, computed as though $P$ were diagonal: $\frac{1}{N}\sum_k \left(\frac{e_{lat}^2}{P_{lat}} + \frac{e_{lon}^2}{P_{lon}} + \frac{e_{alt}^2}{P_{alt}}\right)$ | 3.0 | | `containment_3sigma_horizontal` | fraction | share of latitude and longitude samples inside $\pm 3\sigma$ | 0.9973 | | `containment_3sigma_vertical` | fraction | share of altitude samples inside $\pm 3\sigma$ | 0.9973 | -`nees_position` is the real statistic. It became computable in -[#376](https://github.com/jbrodovsky/strapdown-rs/issues/376), which stopped `NavigationResult` -discarding the position block's off-diagonal terms; before that the crate could only report the -diagonal-only `npes_position`, which equals the NEES when the position states are uncorrelated -and is **optimistic** when they are not. - -Both are kept. `npes_position` stays so the baseline it has accumulated remains comparable -across that change, not as a claim about consistency. - -**How much did the diagonal-only form actually understate?** Less than #376 feared, and -precisely where you would expect: - -| scenario | `npes` | `nees` | ratio | -|---|---:|---:|---:| -| every row with GNSS every epoch | — | — | **1.00–1.01** | -| `real_outage_60s__eskf` | 73.43 | 83.04 | **1.13** | -| `syn_outage_60s__ukf` | 6.54 | 6.82 | **1.04** | +`nees_position` is the real consistency statistic; it became computable when +`NavigationResult` started keeping the position block's off-diagonal terms +([#376](https://github.com/jbrodovsky/strapdown-rs/issues/376)). `npes_position` is kept so its +history stays comparable. It equals the NEES when the position states are uncorrelated and is +optimistic when they are not, which is during a free-inertial coast: compare the two columns on +the outage rows. -Where the filter is aided every epoch the two agree to within 1%, so the historical `npes` -numbers were not misleading. The gap opens on the **outage** rows -- `syn_outage_60s__ukf` read -15.12 against 18.45, a ratio of 1.22, until #375 brought that coast back under control -- and the mechanism is the -obvious one: during a free-inertial coast the position error accumulates along a correlated -direction with no fix to break it up, and a diagonal-only statistic cannot see that. The -contrived worst case is far larger — at a latitude-longitude correlation of 0.9 the two read -2.0 and 20.0 — so the modest ratios here are a measurement about these trajectories, not a -general reassurance. - -Read either beside the containment row, never alone -- together they separate the two failure -modes either one alone hides: +Read either beside the containment columns, never alone. Together they separate the two +failure modes either one hides: | containment | `nees_position` | diagnosis | |---|---|---| | ~1.0 | far below 3 | over-conservative: the filter is right but does not believe it | -| below 0.99 | far above 3 | over-confident -- the dangerous one | +| below 0.99 | far above 3 | over-confident, the dangerous one | | ~0.9973 | ~3 | consistent | -There is no Wasserstein-2 metric, though the original issue floated one. Against a Dirac at -truth it reduces to squared error plus total variance, which is strictly less information than -reporting the error and the consistency separately. Against the empirical error distribution it -is not a distance between samples of anything, because errors along one trajectory are strongly -autocorrelated. The CEP columns carry the "the shape of the error distribution moved" signal -instead, and a human reading a failure message can interpret them. The reasoning is recorded in -full in the `strapdown::metrics` module documentation. - -## Scenarios - -`real` is `core/tests/test_data.csv`: 5,366 samples at 1 Hz over 89 minutes, ENU, from a -consumer MEMS IMU. `syn` is `sim::generate_synthetic`: 300 s at 50 Hz, NED, 50 m/s off-axis -cruise, consumer-grade IMU, seed 42, scored against its own exact truth. - -| id | data | GNSS | estimators | -|---|---|---|---| -| `real_clean` | real | every epoch | UKF, EKF, ESKF | -| `real_sparse_5s` | real | every 5 s | UKF, ESKF | -| `real_outage_60s` | real | 120 s on, 60 s off | ESKF | -| `real_degraded` | real | every epoch, AR(1) position and velocity fault | UKF, ESKF | -| `syn_cruise_1hz` | syn | every 1 s | UKF, EKF, ESKF | -| `syn_outage_60s` | syn | 60 s on, 60 s off | UKF, ESKF | -| `syn_dead_reckoning` | syn, first 120 s | none | unaided mechanization | -| `real_rbpf_slice` | real, first 1,200 samples | every epoch | RBPF, 500 particles | - -Two scenarios are deliberately absent. **Full-run dead reckoning** ends millions of metres out -by way of an altitude at which the `r_e + altitude` denominator in `earth::transport_rate` is -within a few percent of zero, where finiteness is a floating-point accident that has already -differed between Windows and Linux; the 120 s synthetic window is the safe version of the same -measurement. **A second particle-filter scenario** is affordable in principle and not in CI -time: the RBPF is the only estimator here whose cost is measured in seconds. - -The whole suite runs in about 5.5 seconds on Linux and is not gated behind a feature or an -`#[ignore]`, so it runs on all three CI platforms. That is deliberate: the three legs are a -free portability canary for the accuracy numbers themselves, which this project has been bitten -by before. +There is deliberately no Wasserstein metric; the reasoning is in the `strapdown::metrics` +module documentation. + +## Read these caveats first + +The numbers mean little without them. + +1. **On the real-data scenarios, "truth" is the GNSS fix, which is also the filters' aiding + source.** They measure agreement with the aid, not independent accuracy, and they cannot + fall below the receiver's own error. The receiver advertises a 3.81 m horizontal 1σ on + average over this recording (`GNSS_REPORTED_HORIZONTAL_ACCURACY_M` in + `core/tests/integration_tests.rs`, checked against the data on every run). The `syn_*` + scenarios are scored against an exact synthetic trajectory and have no such floor. +2. **On a full-rate real-data row, the filter is scored against a fix it has just been + given.** The row at $t_k$ contains $t_k$'s GNSS update, so on `real_clean` the horizontal + columns measure how a filter weighs its own aiding against its prediction. The real-data + rows that measure navigation are the ones where the filter has to predict between or + without fixes (`real_sparse_5s`, `real_outage_60s`, `real_degraded`), together with the + `syn_*` rows and their exact truth. +3. **The consistency columns mean different things on the two sources.** On the synthetic + rows they are a real measurement of whether a filter believes the right thing. On the + real-data rows the reference is itself noisy and the filters' models of the phone are + approximate, so read them as a drift detector rather than a verdict. +4. **`real_degraded` tells the filter nothing about the fault.** Its fault uses + `r_scale = 1`, so the filter's measurement noise is not inflated while each fix carries an + AR(1) error with a steady-state σ of $3/\sqrt{1-0.98^2} \approx 15$ m. Its NEES and + containment columns are what a filter looks like when its $R$ is wrong, which is the + point of the scenario. Under that fault the GNSS altitude is itself biased, so the + barometric bias state cannot be separated from it, and the vertical columns suffer. +5. **The baseline is blessed on one platform and gated on three.** The JSON records the + platform it was blessed on (`blessed_on`). Most metrics agree across Linux, macOS and Windows + to well inside the tolerance; the advisory `Cross-platform accuracy spread` job in + `rust.yml` measures the spread on every run. `real_rbpf_slice__rbpf` is the exception: its + `horizontal_cep95_m`, `nees_position` and `npes_position` are recorded but **not gated**, + with the reasons in each metric's `note`, until a cross-platform tolerance exists + ([#386](https://github.com/jbrodovsky/strapdown-rs/issues/386)). Check a Windows run before + calling a re-bless done. +6. **A one-ulp change upstream used to move the UKF rows by percents.** At the old + `ukf_alpha = 1e-3` the weighted sigma-point mean lost about six significant digits to + cancellation every step ([#399](https://github.com/jbrodovsky/strapdown-rs/issues/399)). + `ukf_alpha` is now 0.1, and `core/tests/ukf_conditioning.rs` guards the result. If a + re-bless moves rows the change cannot reach, compile the new code without calling it, bless + that as a control, and diff the real change against the control. ## Current values -Measured on `ubuntu-latest`, rustc 1.91. Read-only with `cargo perf`; re-blessed with -`UPDATE_PERF_BASELINE=1 cargo test -p strapdown-core --test perf_baseline`. - -The three tables below are **generated** from `core/tests/perf_baseline.json` by the same test -that gates it, and the gate fails if they drift from it. They used to be maintained here by -hand, which meant every re-bless was a command plus 45 transcribed rows, with nothing checking -the transcription -- and a wrong number on this page is harder to notice than a wrong number in -the gated file, because nothing asserts it (#380). Edit `baseline-tables.md` and the next run -will tell you off; change the numbers by re-blessing. +These tables are **generated** from `core/tests/perf_baseline.json` by the test that gates it, +and the gate fails if they drift from it. Do not edit them; change them by re-blessing. {{#include ./baseline-tables.md}} -## What the table is saying - -- **The `real_clean` horizontal column measures agreement with the aid, not accuracy.** All - three filters now read 4.78, 5.22 and 5.12 m against a reference whose own noise is 3.81 m, - so none of them is below its own reference any more — #373 fixed the absolute covariance - floor that had the UKF and EKF reproducing the fix they were handed (0.014 m and 0.0001 m). - What remains is caveat 1's circularity, which no fix to the filters can remove: the score is - against the aiding source itself. -- **`syn_cruise_1hz` is where the filters actually separate**, and no one of them wins. The - ESKF leads on vertical error, the EKF on horizontal, and all three now hold a fraction of a - degree of yaw — the UKF's, once the worst column in the table at 108.3 deg with no - magnetometer and 2.01 deg with one but a mis-sized bias prior, is now the best of the three. - These rows are scored against exact truth, so nothing here is circular. -- **The outage rows are dominated by attitude, not by position.** 60 s of free inertial turns a - fraction of a degree of tilt error into tens of metres. Both filters now hold about a quarter - of a degree of yaw through the coast and land within a couple of metres of each other, around - 26–27 m. The UKF used to reach 809 m on this row; that was its initial gyro-bias prior, not - its attitude algebra, and the story is in caveat 3. -- **The two consistency columns disagree with each other on real data, and that is the - finding.** Horizontal three-sigma containment sits near 0.50 across the three filters and - vertical near 0.40, where better than half the altitude errors fall outside three sigma. - Neither is the old flat 1.000 — [#373](https://github.com/jbrodovsky/strapdown-rs/issues/373) - removed the absolute covariance floor that made the horizontal figure unfalsifiable, so what - is left in both columns is a real measurement of over-confidence rather than an artefact. The - vertical half is [#372](https://github.com/jbrodovsky/strapdown-rs/issues/372). -- **The ESKF's numbers moved furthest when #367 landed**, and in the right direction: - horizontal containment 0.161 to 0.497, `npes` 1,249 to 68. That is the offset leaving the - numerator. It is still not consistent -- 0.497 against an ideal of 0.9973 -- and what - remains is a real finding rather than an artefact. -- **`real_rbpf_slice__rbpf`'s consistency metrics are not gated.** When this was written `npes` - read 5.7e25 and the real `nees` larger still, because the particle cloud collapsed to a - horizontal sigma of nanometres on 17 of 1,200 epochs, which a mean of $e^2/P$ cannot survive - ([#385](https://github.com/jbrodovsky/strapdown-rs/issues/385)). Roughening removed the - collapse, and the restructure after Canciani & Raquet (2026-09-25) took `npes` to 10.3 and - both channels' 3-sigma containment to 0.95: the filter is now close to consistent. The metrics stay ungated until the gate has a - cross-platform tolerance ([#386](https://github.com/jbrodovsky/strapdown-rs/issues/386)). -- **Several numbers here therefore record known defects rather than good behaviour**, which is - what a two-sided gate is for. Each is annotated in the baseline file. When one is fixed the +## What the tables are saying + +Read the rows rather than any summary here; these are the comparisons worth making. + +- **`real_clean` measures agreement with the aid.** Compare its `horiz RMSE` column with the + receiver's 3.81 m advertised accuracy (caveat 1). The three Kalman filters land within a few + centimetres of each other there, because on this row they are all mostly reporting the fix + they were just handed (caveat 2). +- **`syn_cruise_1hz` is where the filters can actually separate**, because it is scored against + exact truth. Compare its three rows column by column; the attitude table shows what a 1 Hz + magnetometer and GNSS velocity make of yaw on a synthetic trajectory. +- **The outage rows are dominated by the outages.** On `real_outage_60s__eskf` and both + `syn_outage_60s` rows, CEP50 stays at the level of the full-rate rows while RMSE, CEP95 and + the maximum are set by the coasts. That is the shape free-inertial drift gives: most + epochs are aided and accurate, and the error concentrates at the ends of the outages. +- **`syn_dead_reckoning` is the unaided reference.** It is only 120 s long, and its + consistency columns are empty because dead reckoning reports no covariance. +- **The real-data consistency columns say the filters are over-confident.** Their horizontal + and vertical containment sit well below 0.9973 and their NEES well above 3, while the + synthetic rows sit much closer to ideal. That is caveat 3 in numbers; `real_degraded` is the + extreme case for the reason in caveat 4. +- **Several numbers record known defects rather than good behaviour**, which is what a two-sided + gate is for. Each is annotated with a `note` in `perf_baseline.json`. When one is fixed the improvement side trips and asks for the diff that records it. - -## The vertical process-noise retune - -These numbers already include one deliberate change the gate was built to catch, and it is -worth reading as a worked example of the workflow. - -`DEFAULT_PROCESS_NOISE_DENSITY`'s altitude entry was `1e-4` -- a 1 cm standard deviation, -a tenth of the horizontal channel's 0.1 m, an asymmetry nothing ever justified. Issue #308 had -fixed the *units* of the horizontal entries and deliberately left this one alone, recording -that whether `1e-4` was the right *tuning* was a separate question. The consistency columns -answered it: at `1e-4` three-sigma altitude containment was 0.88 on the synthetic trajectory -and 0.44 on the recording, against an ideal of 0.9973. More than half the altitude errors on -real data fell outside the uncertainty the filter reported for them. - -Three entries could have been blamed. Sweeping each alone over four decades, only the altitude -position entry moves containment the right way -- the vertical-velocity entry is flat and costs -a vertical-velocity RMSE growing from 0.34 to 4.37 m/s, and the accelerometer-bias entry makes -containment *worse* while taking the real 60 s-outage horizontal RMSE from 291 m to 443 m. The -entry is now `POSITION_PROCESS_NOISE_M_PER_ROOT_S²`, which makes the position block isotropic in -standard deviation and lands on the knee of the measured curve. - -What it bought, and what it cost: - -| | before | after | -|---|---:|---:| -| `syn_cruise_1hz__eskf` 3-sigma vertical | 0.880 | **0.933** | -| `syn_cruise_1hz__eskf` `npes` (ideal 3.0) | 5.653 | **4.795** | -| `syn_outage_60s__eskf` horizontal RMSE | 137.66 m | **20.22 m** | -| `syn_outage_60s__eskf` pitch RMSE | 1.500 deg | **0.205 deg** | -| `syn_cruise_1hz__eskf` vertical RMSE | **0.665 m** | 0.836 m | -| `syn_outage_60s__ukf` horizontal RMSE | **221.67 m** | 266.68 m | - -The vertical RMSE cost is the honest price: the old value bought that number by under-reporting -its own error, and on the reference recording it is not paid at all -- vertical RMSE there -improved slightly. (These synthetic figures have since moved again, and further, under #375 -- -see caveat 6. The numbers in this table are the ones that change measured, kept as its -record.) The `syn_outage_60s__ukf` row is recorded but not endorsed: across the sweep -its horizontal RMSE runs 222, 174, 267, 344 m while its yaw runs 105, 98, 21, 34 deg, so -position degrades exactly where attitude improves fivefold. That row is -[#371](https://github.com/jbrodovsky/strapdown-rs/issues/371) coasting an outage, and it does -not respond monotonically to this or any other knob. diff --git a/book/src/examples/configurations.md b/book/src/examples/configurations.md index 2a9db591..49be3a90 100644 --- a/book/src/examples/configurations.md +++ b/book/src/examples/configurations.md @@ -2,73 +2,112 @@ Ready-to-run scenario files live in [`examples/configs/`](https://github.com/jbrodovsky/strapdown-rs/tree/main/examples/configs). -For the schema itself — every field, and the naming pitfall that silently disables a scenario -— see [Configuration Files](../user-guide/configuration.md). - -`--config` supplies the whole run, so the subcommand's `-i`/`-o` are ignored alongside it — -set `input:` and `output:` inside the file instead: - -```bash -strapdown-sim --config examples/configs/simple_dropout.yaml -``` - -## Baseline - -| File | What it does | +Each is a complete `SimulationConfig`; for the schema itself see +[Configuration Files](../user-guide/configuration.md), and for every scheduler and fault field +see [Schedulers and Faults Reference](../gnss/scenarios.md). + +Three things apply to all of them: + +- **`--config` supplies the whole run.** The subcommand's `-i`, `-o` and `--seed` are ignored + alongside it, so set `input` and `output` inside the file. Most of these files omit both, and + then read `input.csv` and write `output.csv` in the working directory. +- **They declare the NED frame** by not setting `is_enu`. That is right for `strapdown-sim syn` + output. Sensor Logger exports are ENU, so add `is_enu: true` (or `is_enu = true`) to run one + against a phone recording; a wrong declaration is rejected before the run starts. +- **Duty cycles start with their OFF window** unless `start_phase_s` gives an initial ON + window (see [`DutyCycle`](../gnss/scenarios.md#dutycycle)). Several files below set + `start_phase_s: 0.0`, so their runs begin in an outage. + +Every file in the tables below was run against a 600 s trajectory from +`strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42`, with `input` and `output` +added, and completed. `geonav_example.toml` needs a binary built with `--features geonav` and +map files beside the input. + +## Scenario files + +All of these are `mode: closed-loop` with the default filter (the ESKF), unless noted. + +### Baseline + +| File | Scheduler | Fault | What it models | +|---|---|---|---| +| `baseline.yaml` | `pass_through` | `none` | no degradation; the reference to compare the others against | + +### Availability + +| File | Scheduler | Fault | What it models | +|---|---|---|---| +| `sched_10s.yaml` | `fixed_interval`, 10 s | `none` | a reduced update rate | +| `duty_10on_2off.yaml` | `duty_cycle`, 10 s on / 2 s off, initial 1 s on | `none` | brief periodic outages | +| `simple_dropout.yaml` | `duty_cycle`, 120 s on / 30 s off, no initial on | `none` | bridges and tall buildings; starts with a 30 s outage | +| `extended_gnss_denied.yaml` | `duty_cycle`, 60 s on / 300 s off, no initial on | `none` | tunnels and sustained jamming; starts with a 300 s outage | + +`extended_gnss_denied.yaml` shows dead-reckoning quality most clearly: five minutes is long +enough for consumer-MEMS drift to dominate. Because it sets no initial ON window, the first +five minutes are an outage before the filter has seen any fix; set `start_phase_s` if you want +the filter to converge first. See [Tutorial: GNSS Degradation](./tutorial-gps-degradation.md). + +### Accuracy + +| File | Scheduler | Fault | What it models | +|---|---|---|---| +| `degraded_fullrate.yaml` | `pass_through` | `degraded` (ρ 0.99, σ 3 m; ρ 0.95, σ 0.3 m/s; `r_scale` 5) | AR(1)-correlated error at full rate | +| `degraded_5s.yaml` | `fixed_interval`, 5 s | `degraded`, same values | reduced rate and corruption together | +| `gnss_degradation.yaml` | `fixed_interval`, 10 s | `degraded`, same values | the same idea at 10 s | +| `gnss_degradation.toml` | as above | as above | the same scenario in TOML | +| `gnss_degradation.json` | as above | as above | the same scenario in JSON | +| `combo.yaml` | `fixed_interval`, 5 s | `degraded` (ρ 0.995, σ 4 m; ρ 0.97, σ 0.35 m/s; `r_scale` 5) | reduced rate plus degraded accuracy | +| `particle_filter_comparison.yaml` | `fixed_interval`, 5 s | `degraded` (σ 5 m, 0.5 m/s) | the closed-loop half of a filter comparison; see below | + +None of these sets `tau_pos_s` or `tau_vel_s`, so their `sigma_*` values are per-fix +innovations and their correlation times depend on the fix interval; see +[Fault Simulation](../gnss/fault-simulation.md#correlation-time-and-the-fix-interval). + +`combo.yaml` combines a scheduler with a fault. It is **not** the `combo` fault kind, which +chains several fault models on the same fix; no shipped example uses that kind, and +[Schedulers and Faults Reference](../gnss/scenarios.md#combo) shows its syntax. + +`particle_filter_comparison.yaml` is, despite its name, a closed-loop file. Its comments +describe the comparison: copy it, change the copy's `mode` to `particle-filter` and both +files' `output`, and run each with `--config`. + +### Spoofing + +| File | Scheduler | Fault | What it models | +|---|---|---|---| +| `slowbias.yaml` | `pass_through` | `slow_bias`, 0.02 m/s north, `q_bias` 1e-6 | soft spoofing: each fix looks plausible | +| `slowbias_rot.yaml` | `pass_through` | `slow_bias`, as above with `q_bias` 5e-6 and the direction rotating at 1e-3 rad/s | the same, drifting in a turning direction | +| `hijack.yaml` | `pass_through` | `hijack`, 50 m north from 120 s for 60 s | hard spoofing: an abrupt offset | +| `combo_duty_hijack.yaml` | `duty_cycle`, 10 s on / 3 s off | `hijack`, 10 m north and 10 m east from 150 s for 120 s | duty-cycled availability plus hard spoofing | + +Spoofing scenarios are worth pairing with an innovation gate (`--gate-confidence` on the +command line). A `hijack` produces a large normalized innovation squared the moment it starts, +which a chi-squared gate can reject, while a `slow_bias` is designed to stay under that +threshold. + +### Filter, I/O and geophysical settings + +| File | What it shows | |---|---| -| `baseline.yaml` | No degradation. The reference every other scenario is compared against. | +| `closed-loop-ukf.toml` | selecting the UKF in `[closed_loop]`, with `[logging]` and an undegraded `[aiding]` section | +| `parallel_example.toml` | a directory as `input` and `output`, `parallel = true` to process its files concurrently, and a log file | +| `geonav_example.toml` | a `[geophysical]` section with gravity and magnetic aiding on the UKF; see [Geophysical Navigation](../geonav/overview.md) | -## Availability - -| File | What it does | -|---|---| -| `sched_10s.yaml` | One fix every 10 s, uncorrupted. A reduced update rate. | -| `duty_10on_2off.yaml` | 10 s available, 2 s denied, repeating. Brief periodic outages. | -| `simple_dropout.yaml` | 2 minutes available, 30 s denied. Bridges and tall buildings. | -| `extended_gnss_denied.yaml` | 1 minute available, 5 minutes denied. Tunnels and sustained jamming. | - -`extended_gnss_denied.yaml` is the one that shows dead-reckoning quality most clearly: five -minutes is long enough for consumer-MEMS drift to dominate completely. Expect errors in the -hundreds of metres to kilometres, and see -[Tutorial: GPS Degradation](./tutorial-gps-degradation.md) for why the figure is usually -smaller than double-integrating the sensor's bias would predict. - -## Accuracy - -| File | What it does | -|---|---| -| `degraded_fullrate.yaml` | Full rate, AR(1)-correlated position and velocity error. | -| `degraded_5s.yaml` | Every 5 s *and* degraded. Scheduling and corruption together. | +## Other files -## Spoofing - -| File | What it does | -|---|---| -| `slowbias.yaml` | A slowly drifting offset. Soft spoofing — each fix looks plausible. | -| `slowbias_rot.yaml` | The same, with the drift direction rotating. | -| `hijack.yaml` | An abrupt offset during a fixed window. Hard spoofing. | - -Spoofing scenarios are the ones worth pairing with an innovation gate: a `hijack` produces a -large normalized innovation squared the moment it starts, which a chi-squared gate rejects, -while a `slow_bias` is specifically designed to stay under that threshold. - -## Combined - -| File | What it does | -|---|---| -| `combo.yaml` | Reduced rate plus degraded accuracy. | -| `combo_duty_hijack.yaml` | Duty-cycled availability plus hard spoofing. | - -## Other formats - -`gnss_degradation.json` and `gnss_degradation.toml` are the same schema in the other two -supported formats. The `json/` subdirectory is a different thing: those files are -`{name, args}` CLI-invocation presets, not scenario configs, and they use the command line's -vocabulary rather than the config schema's. +- `README.md` in the directory repeats this catalogue with each scheduler and fault option + written out. +- `json/` holds ten `{"name", "args"}` files (`baseline`, `combo`, `combo_duty_hijack`, + `degraded_5s`, `degraded_fullrate`, `duty_10on_2off`, `hijack`, `sched_10s`, `slowbias`, + `slowbias_rot`). They are **not** configuration files: each is a list of command-line + arguments for the same scenario, in the command line's vocabulary (`--sched fixed`, + `--fault slowbias`) rather than the configuration file's (`fixed_interval`, `slow_bias`). + Nothing in the repository reads them, and `--config` would reject them. ## Running the set -Because `--config` ignores `-i`/`-o`, a batch varies the paths *in the file*: +Because `--config` ignores `-i`/`-o`, a batch varies the paths *in the file*. This writes a +copy of each YAML scenario pointed at one input and a separate output, then runs it: ```bash mkdir -p results .scenarios @@ -81,3 +120,17 @@ for config in examples/configs/*.yaml; do strapdown-sim --config ".scenarios/${name}.yaml" done ``` + +Passing `-o "results/${name}.csv"` instead would be silently ignored, and every scenario would +write to the same `output.csv`. + +## The experiment recipes in `conf/` + +`conf/` holds the fifteen configurations the project's own experiments run through the +`justfile`: `{ukf,ekf,rbpf}_{truth,degraded,grav,mag,both}.toml`, plus the same fifteen under +`conf/real/` for the frozen real-sensor arm. They read a directory of preprocessed recordings +(`data/input`, which is not distributed), declare `is_enu = true`, and carry long comments +explaining every value. `*_truth` is undegraded, `*_degraded` is the single interference +profile described in [Fault Simulation](../gnss/fault-simulation.md), and +`*_{grav,mag,both}` add geophysical aiding to that profile. They are worth reading as worked +examples even without the data. diff --git a/book/src/examples/tutorial-particle-filter.md b/book/src/examples/tutorial-particle-filter.md index 3c26e977..025f89a1 100644 --- a/book/src/examples/tutorial-particle-filter.md +++ b/book/src/examples/tutorial-particle-filter.md @@ -1 +1,239 @@ # Tutorial: Particle Filter + +This tutorial runs the [Rao-Blackwellized particle filter](../filters/rbpf.md) end to end with +`strapdown-sim pf`: generate a synthetic drive and its exact truth, run the filter from flags and +then from a configuration file adapted from the `conf/rbpf_*` recipes, score both against truth, +and see why the simulator does not use the paper's horizontal process noise. Every command and +every block of output below was produced by the `strapdown-sim` 1.0.0 binary and the examples in +`core/examples/`; your timestamps will differ. + +## A trajectory and its truth + +`syn` with the same flags and seed writes the noisy sensor log and, with `--no-noise`, the exact +trajectory it was derived from. This one starts in Philadelphia and drives due north at 15 m/s +for ten minutes with a consumer-grade IMU sampled at 10 Hz (the `syn` defaults). A negative +value has to be attached with `=`, or the parser reads it as a flag. + +```bash +strapdown-sim syn -o drive.csv --duration-s 600 --seed 42 \ + --latitude-deg 39.95 --longitude-deg=-75.16 --altitude-m 50 --velocity-north-mps 15 +strapdown-sim syn -o drive_truth.csv --duration-s 600 --seed 42 \ + --latitude-deg 39.95 --longitude-deg=-75.16 --altitude-m 50 --velocity-north-mps 15 --no-noise +``` + +```text +2026-10-01 20:52:19.901 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 20:52:19.912 [INFO] - Sensor records written to drive.csv +2026-10-01 20:52:19.993 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 20:52:20.000 [INFO] - Truth trajectory written to drive_truth.csv +``` + +`syn` output is NED, which is the default for every subcommand, so no `--enu` is needed. (The +`conf/` recipes set `is_enu = true` because they read ENU Sensor Logger exports.) + +## Run `pf` with its defaults + +```bash +strapdown-sim pf -i drive.csv -o pf_out.csv +``` + +```text +2026-10-01 20:52:20.005 [INFO] - Processing file: drive.csv +2026-10-01 20:52:20.029 [INFO] - Read 6000 records from drive.csv +2026-10-01 20:52:20.030 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.775 m/s^2 against a local gravity of 9.801 m/s^2 +2026-10-01 20:52:21.164 [INFO] - Results written to pf_out.csv +2026-10-01 20:52:21.164 [INFO] - Particle filter simulation complete +``` + +The "Mechanizing input as NED" line is the frame check: the mean vertical specific force has the +sign NED predicts, so the declared frame is accepted. + +With no flags the filter runs 100 particles, uses a horizontal process noise of `[1, 1]` m/√s and +takes every GNSS fix in the file, here one per record (10 Hz). `pf_out.csv` has the same columns +as a `cl` run (see [Output Format](../user-guide/output-format.md)), one row per IMU sample. The +six IMU-bias columns are zero: the RBPF carries no bias states. + +## Comparing against truth + +`core/examples/score_run.rs` loads an output CSV and a truth CSV and scores them with +`metrics::evaluate`, the reduction the accuracy baseline is gated on: + +```rust,ignore +{{#include ../../../core/examples/score_run.rs:score}} +``` + +```bash +cargo run -p strapdown-core --example score_run -- pf_out.csv drive_truth.csv +``` + +```text +aligned samples 5990 +horizontal RMSE 0.988 m +horizontal CEP50 0.808 m +horizontal CEP95 1.751 m +horizontal max 3.375 m +vertical RMSE 1.958 m +horizontal velocity RMSE 0.157 m/s +yaw RMSE 0.409 deg +position NEES (consistent at 3) 13.332 +3-sigma horizontal containment 1.000 +``` + +Rows are matched by timestamp, which works because `syn` always starts at +2025-01-01T00:00:00Z; the first ten aligned samples (one second) are skipped as initialization. +Read the two consistency lines together, as +[metrics](../user-guide/library.md#scoring-a-run-against-truth) explains: the NEES compares the +error with the full position covariance, and containment counts how often the error lies inside +three sigma. + +## A configuration adapted from `conf/rbpf_truth.toml` + +The recipes under `conf/` describe the repository's own experiments: ENU phone recordings in +`data/input`, logs written to `log/`, map biases in `[geophysical]`. For a `syn` trajectory, keep +their `[particle_filter]` section and change the rest: the input and output paths, `is_enu = +false`, no `[geophysical]` section, and an `[aiding]` scheduler that thins the 10 Hz synthetic +fixes to 1 Hz, the rate the recordings' receivers actually deliver. `[aiding]` is the section's +name; the recipes' `[gnss_degradation]` is accepted as an alias. + +```toml +# rbpf_syn.toml: particle filter on a synthetic NED trajectory, adapted from conf/rbpf_truth.toml. +input = "drive.csv" +output = "pf_config_out.csv" +mode = "particle-filter" +is_enu = false # `syn` writes NED; the conf/ recipes read ENU Sensor Logger exports +seed = 42 + +[particle_filter] +num_particles = 1000 +position_init_std_m = [10.0, 10.0, 5.0] +velocity_init_std_mps = 1.0 +attitude_init_std_rad = 0.1 +velocity_process_noise_std_mps = 1e-3 +attitude_process_noise_std_rad = 0.01 +horizontal_process_noise_std_m = [1.0, 1.0] +baro_loop_time_constant_s = 10.0 +baro_error_std_m = 8.3 +baro_error_time_constant_s = 3600.0 +vertical_accel_error_init_std_mps2 = 0.1 +effective_sample_threshold = 1.0 +roughening_factor = 0.2 + +[aiding] +seed = 42 + +[aiding.scheduler] +kind = "fixed_interval" +interval_s = 1.0 +phase_s = 0.0 + +[aiding.fault] +kind = "none" +``` + +A configuration file supplies the whole run, so it is passed on its own: + +```bash +strapdown-sim --config rbpf_syn.toml +cargo run -p strapdown-core --example score_run -- pf_config_out.csv drive_truth.csv +``` + +```text +2026-10-01 20:52:39.076 [INFO] - Loading configuration from rbpf_syn.toml +2026-10-01 20:52:39.076 [INFO] - Configuration loaded successfully +2026-10-01 20:52:39.076 [INFO] - Mode: ParticleFilter +... +2026-10-01 20:52:39.104 [INFO] - Running particle filter simulation +2026-10-01 20:52:43.908 [INFO] - Results written to pf_config_out.csv +``` + +```text +aligned samples 5990 +horizontal RMSE 2.362 m +horizontal CEP50 1.994 m +horizontal CEP95 3.962 m +horizontal max 6.800 m +vertical RMSE 2.676 m +horizontal velocity RMSE 0.570 m/s +yaw RMSE 0.416 deg +position NEES (consistent at 3) 7.880 +3-sigma horizontal containment 0.999 +``` + +The [RBPF configuration table](../filters/rbpf.md#configuration) explains every key. Three are +worth knowing before changing anything: + +- `horizontal_process_noise_std_m`: see the next section. +- `effective_sample_threshold = 1.0` resamples after every update, as Canciani and Raquet do. +- `roughening_factor = 0.2` jitters the cloud after resampling so it cannot collapse onto copies + of one particle; `0.0` reproduces the paper. + +The `[geophysical]` section and the `*_variation_*` keys only matter for map-aided runs, which +need gravity or magnetic anomaly maps; see [Geophysical Navigation](../geonav/overview.md). + +## Why not the paper's zero horizontal noise + +Canciani and Raquet put no process noise on the sampled horizontal position (their eq. 19). The +library's `RbpfConfig` keeps that default; `strapdown-sim` does not. On the same trajectory with +1 Hz fixes: + +```bash +strapdown-sim pf -i drive.csv -o pf_zero.csv --sched fixed --interval-s 1 \ + --horizontal-process-noise-std-m 0 0 +``` + +```text +2026-10-01 20:52:45.263 [INFO] - Processing file: drive.csv +2026-10-01 20:52:45.290 [INFO] - Read 6000 records from drive.csv +2026-10-01 20:52:45.291 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.775 m/s^2 against a local gravity of 9.801 m/s^2 +Error: OutOfRange { what: "speed", value: 500.1655942836976, min: 0.0, max: 500.0 } +``` + +The run diverges until the health monitor's speed limit (`--health-speed-mps-max`, 500 m/s by +default) stops it, and no output file is written. With zero noise each epoch's time update is a +noiseless observation of the velocity and tilt errors, which moves their uncertainty out of the +shared covariance and into the particles' spread; resampling on metre-level fixes then destroys +that spread. [RBPF](../filters/rbpf.md#horizontal-process-noise-why-the-default-is-1-ms) gives the +full account. + +## The same trajectory through the ESKF + +For a reference point, run the default Kalman filter on the same input with the same 1 Hz +schedule and score it the same way: + +```bash +strapdown-sim cl -i drive.csv -o cl_out.csv --sched fixed --interval-s 1 +cargo run -p strapdown-core --example score_run -- cl_out.csv drive_truth.csv +``` + +```text +aligned samples 5990 +horizontal RMSE 2.072 m +horizontal CEP50 1.738 m +horizontal CEP95 3.519 m +horizontal max 5.070 m +vertical RMSE 2.336 m +horizontal velocity RMSE 0.450 m/s +yaw RMSE 0.335 deg +position NEES (consistent at 3) 5.192 +3-sigma horizontal containment 1.000 +``` + +One synthetic straight-line drive is not a ranking of the two filters. The RBPF exists for +map-aided navigation, where the measurement is a nonlinear, often multimodal function of +position; with GNSS fixes alone, as here, it has nothing to exploit that a Kalman filter cannot. +The gated comparison across scenarios, including the RBPF's own row, is on +[Performance Baselines](../development/performance.md), and +[RBPF](../filters/rbpf.md#sparse-gnss-what-this-structure-cannot-recover-from) documents a regime +(one fix a minute) in which it diverges where the EKF does not. + +For the unaided baseline, `strapdown-sim dr -i drive.csv -o dr_out.csv` scored the same way +reports no consistency metrics (a dead-reckoning run has no covariance) and a horizontal error +that grows to tens of kilometres over the ten minutes on this consumer-grade IMU. + +## Where next + +- [RBPF](../filters/rbpf.md): structure, configuration keys and departures from the paper. +- [Particle Filter](../user-guide/particle-filter.md) in the user guide: the `pf` subcommand's + flags. +- [Schedulers and Faults](../gnss/scenarios.md): outages, reduced rates and spoofing for any of + the runs above. diff --git a/book/src/faq.md b/book/src/faq.md index bb6eb971..3cb169d2 100644 --- a/book/src/faq.md +++ b/book/src/faq.md @@ -1,132 +1,178 @@ # Frequently Asked Questions -## General Questions +## General -### What is Strapdown-rs? +### What is strapdown-rs? -Strapdown-rs is a Rust library for implementing strapdown inertial navigation systems (INS). It provides core functionality for processing IMU data to estimate position, velocity, and orientation. +A Rust implementation of strapdown inertial navigation and of the tools to study it under GNSS +degradation. It is three crates: `strapdown-core` (the library, imported as `strapdown`), the +`strapdown-sim` command-line simulator, and the experimental `strapdown-geonav` for gravity +and magnetic map aiding. See the [Introduction](./introduction.md). -### Who should use Strapdown-rs? +### Who is it for? -Strapdown-rs is designed for: -- Researchers working on navigation systems -- Engineers developing autonomous systems -- Students learning about inertial navigation -- Anyone needing a high-performance INS implementation in Rust +Researchers, students and engineers who want reproducible inertial-navigation experiments: +the same input, configuration and seed give the same output, and the GNSS outages, noise and +spoofing are configured rather than hand-edited into the data. ### What coordinate frames are supported? -The library primarily uses the North-East-Down (NED) local-level frame. The 9-state vector includes: -- Position: latitude, longitude, altitude -- Velocity: northward, eastward, downward -- Attitude: roll, pitch, yaw +North-East-Down (NED) is the default local-level frame, as in Groves. East-North-Up (ENU) is an +explicit opt-in: `--enu` on the command line or `is_enu = true` in a config file. The 9-state +navigation vector is: + +- position: latitude, longitude, altitude (height above the ellipsoid, positive up in both + frames); +- velocity: north, east and vertical, where vertical is positive **down** in NED and positive + **up** in ENU (the output column is `velocity_vertical`); +- attitude: roll, pitch, yaw. + +See [Coordinate Frames](./user-guide/coordinate-frames.md). ### Is this production-ready? -Strapdown-rs is primarily intended for research and development. While the code is well-tested and prioritizes correctness, it is still under active development as part of ongoing PhD research. +It is research software. It is tested and prioritises correctness, but it is developed as part +of ongoing PhD research, and `strapdown-geonav` in particular is experimental. -## Installation and Setup +## Installation and setup ### What are the system requirements? -See the [System Requirements](./installation/requirements.md) page for detailed information. In summary: -- Rust 1.91 or later -- A C/C++ compiler and cmake 3.26+, but only for features that use a C library -- No system libraries to install at build time -- libfontconfig at *run* time, if you use `strapdown-sim`'s plotting (a default feature). - It is loaded on demand, so a machine without it builds fine and fails at the first plot -- Supported on Linux, macOS, and Windows +See [System Requirements](./installation/requirements.md). In summary: + +- Rust 1.91 or later (`rust-toolchain.toml` fetches it); +- a C/C++ compiler and cmake 3.26 or newer, but only for features that build a C library + (HDF5, netCDF, `geonav`); `cargo build -p strapdown-core` needs neither; +- no system libraries at build time; +- libfontconfig at *run* time, for `strapdown-sim`'s `--plot` (the `plotting` feature, on by + default). It is loaded on demand, so a machine without it builds fine and fails at the + first plot; +- Linux, macOS or Windows. ### Do I need to install HDF5 and NetCDF? No. They are compiled from vendored sources that ship as ordinary cargo dependencies, along with zlib and freetype, so nothing is searched for on your machine. What you need instead is a -C/C++ compiler and cmake 3.26 or newer to build them with. - -They are used for: -- HDF5: binary data storage for `TestDataRecord` and `NavigationResult` -- NetCDF: geophysical map data for the geonav features - -Neither is on by default in `strapdown-core`, so if you only need the core INS functionality -you need no C toolchain either: `cargo build -p strapdown-core` uses none. +C/C++ compiler and cmake 3.26 or newer to build them with. HDF5 and NetCDF back the optional +`hdf5`/`netcdf` output methods on `NavigationResult` and `TestDataRecord`, and NetCDF also +reads the geophysical maps. ### How do I install on Windows? Install the MSVC toolchain from Visual Studio Build Tools plus -[cmake](https://cmake.org/download/). vcpkg is no longer needed, since nothing is looked up on -the system. WSL2 also works if you prefer a Linux environment. +[cmake](https://cmake.org/download/). vcpkg is not needed, since nothing is looked up on the +system. WSL2 also works. See [Installation](./installation/installation.md). -See [Installation](./installation/installation.md) for details. - -## Usage Questions +## Usage ### What data format does strapdown-sim accept? -The simulation expects CSV files with IMU and GNSS data following the Sensor Logger app format. See [Input Data Format](./user-guide/data-format.md) for details. +CSV files in the Sensor Logger app's format: timestamped IMU, GNSS and optional barometer and +magnetometer columns. `strapdown-sim syn` writes the same format from a synthetic trajectory. +See [Input Data Format](./user-guide/data-format.md). + +### What does it write? + +CSV. Every path through `strapdown-sim` ends in `NavigationResult::to_csv`. An `-o` value ending +in `.csv` is a file; any other value is treated as a directory to write `.csv` +into, so `-o results.parquet` creates a *directory* called `results.parquet`. HDF5, NetCDF and +MCAP writers exist as library methods on `NavigationResult` behind cargo features, not as CLI +options. See [Output Format](./user-guide/output-format.md). + +### My run stops with `InvalidConfiguration { field: "is_enu", ... }` + +The data's frame does not match the one you declared. Sensor Logger exports are ENU, so pass +`--enu` (or set `is_enu = true` in the config file); `syn` output is NED, so leave it off. The +check compares the leading records' vertical specific force with what the declared frame +expects at rest, and the message says which way round it is. -### Which filter should I use: EKF, UKF, or Particle Filter? +### Which filter should I use? -- **EKF**: Fastest, works well for mildly nonlinear systems -- **UKF**: Better accuracy for highly nonlinear systems, 2-3x slower than EKF -- **Particle Filter**: Best for non-Gaussian distributions and multimodal scenarios +- **ESKF** (`cl`, the default): the error-state Kalman filter. Start here. +- **EKF** (`cl --filter ekf`) and **UKF** (`cl --filter ukf`): the full-state alternatives, + and the two Kalman filters that accept geophysical aiding. +- **Particle filter** (`pf`): the Rao-Blackwellized particle filter, the only particle filter + in the crate. It is built for map-aided navigation, where the measurement is a nonlinear, + possibly ambiguous function of position. -See [Filter Comparison](./filters/comparison.md) for detailed analysis. +No speed comparison is published, because none has been measured. For accuracy, see +[Filter Comparison](./filters/comparison.md) and [Performance Baselines](./development/performance.md). ### Can I use my own sensor data? -Yes! You'll need to convert your data to the expected CSV format. The library is designed to work with standard IMU (gyroscope and accelerometer) and GNSS (position) measurements. +Yes: convert it to the input CSV format above. Any IMU (accelerometer and gyroscope) with GNSS +position and velocity will do; barometer and magnetometer columns are optional. ### How do I simulate GNSS outages? -Use the GNSS fault simulation features in the configuration file: +With a scheduler. On the command line, a duty cycle of 100 s available and 50 s denied: -```toml -[gnss] -dropout_probability = 0.1 # 10% chance of dropout -reduced_update_rate = 0.5 # Half the normal rate +```bash +strapdown-sim cl -i input.csv -o output.csv --sched duty --on-s 100 --off-s 50 ``` -See [GNSS Degradation Scenarios](./gnss/fault-simulation.md) for more options. +or one fix a minute with `--sched fixed --interval-s 60`. In a config file the same goes in the +`[aiding]` section: -## Performance Questions +```toml +[aiding.scheduler] +kind = "duty_cycle" +on_s = 100.0 +off_s = 50.0 +start_phase_s = 0.0 +``` -### How fast is Strapdown-rs? +Note that a duty cycle starts with its OFF window unless `start_phase_s` (`--duty-phase-s`) +gives an initial ON window. Noise, bias and spoofing are fault models (`--fault`). See +[Schedulers and Faults Reference](./gnss/scenarios.md) for every option. -Performance varies by filter type and configuration: -- **EKF**: ~10,000-20,000 updates/second -- **UKF**: ~3,000-5,000 updates/second -- **Particle Filter**: Depends on particle count (100 particles: ~500 updates/second) +## Performance -These are approximate figures on modern hardware and will vary based on your system. +### How fast is it? + +There are no published throughput figures, because none have been measured; nothing in CI +tracks wall-clock time yet. What is measured and gated is navigation *accuracy*: see +[Performance Baselines](./development/performance.md). ### Can I run simulations in parallel? -The current implementation processes data sequentially as navigation is inherently a sequential process. However, the particle filter implementation can utilize multiple cores for particle processing. +Across files, yes. Point `input` at a directory and set `parallel = true` in the config file +(or pass `--parallel` with `--config`), and the files are processed concurrently on a thread +pool. Each file's run is itself sequential, as navigation is. + +In this release, `--parallel` takes effect only together with `--config`. With a subcommand +(`strapdown-sim --parallel cl -i dir/ -o out/`) the directory's files are processed one after +another. -### How much memory does it use? +### The particle filter is slow -Memory usage is modest: -- Core library: ~10-50 MB -- Simulations: Depends on data size and filter configuration -- Particle filters: Linear with particle count +Its cost grows with the number of particles, so reduce `--num-particles` (or `num_particles` in +`[particle_filter]`). If you do not need map aiding, a Kalman filter (`cl`) is the usual +choice. -## Development Questions +## Development ### How can I contribute? -We welcome contributions! Please see the [Contributing Guide](./development/contributing.md) and reach out to the project maintainer before starting major work. +See [Contributing](./development/contributing.md), and contact the maintainer before starting +major work. ### Where is the API documentation? -Full API documentation is available at [docs.rs/strapdown-core](https://docs.rs/strapdown-core). This book focuses on high-level concepts and usage patterns. +The rustdoc is published with this book; [API Documentation](./api/index.md) links it and maps +the modules. It will also be on docs.rs once the crates are published with v1.0.0. -### Can I use this in my commercial project? +### Can I use this in a commercial project? -Yes! Strapdown-rs is licensed under the MIT License, which allows commercial use. See the [LICENSE](https://github.com/jbrodovsky/strapdown-rs/blob/main/LICENSE) file for details. +The code is MIT licensed, which allows commercial use. See the +[LICENSE](https://github.com/jbrodovsky/strapdown-rs/blob/main/LICENSE) file. ### How do I cite this work? -If you use Strapdown-rs in your research, please cite the JOSS paper: +The JOSS paper is under review +([openjournals/joss-reviews#11377](https://github.com/openjournals/joss-reviews/issues/11377)). +Until it is published, cite the software with the repository's `CITATION.cff` (GitHub's "Cite +this repository" button). See [Publications and Links](./resources/publications.md). [![JOSS](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4/status.svg)](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4) @@ -137,40 +183,22 @@ If you use Strapdown-rs in your research, please cite the JOSS paper: These are built from source, so failures are cmake or compiler errors rather than linker errors. The two common ones: -1. **`CMake 3.26 or higher is required`** -- your cmake is older than the bundled HDF5 needs. +1. **`CMake 3.26 or higher is required`**: your cmake is older than the bundled HDF5 needs. Ubuntu 22.04 (3.22) and Debian 12 (3.25) both hit this. -2. **A confusing cmake failure inside the netCDF build** -- check `echo "${HDF5_DIR:-unset}"`. +2. **A confusing cmake failure inside the netCDF build**: check `echo "${HDF5_DIR:-unset}"`. If it is set, the HDF5 build switches to looking for a *system* library even though a - vendored build was requested, and netCDF is then given the wrong headers. + vendored build was requested, and netCDF is then given the wrong headers. Unset it. -See [Installation Troubleshooting](./installation/installation.md#troubleshooting) for more help. +See [Installation](./installation/installation.md) for more. -### My simulation produces NaN values +### My simulation produces NaN values or stops on a health check -Common causes: -- Invalid initial conditions -- IMU data with unrealistic values -- Numerical instability in filter - -Check your input data and initial state. Enable debug logging with `--log-level debug` to investigate. - -### The particle filter is very slow - -This is expected with high particle counts. Consider: -- Reducing the number of particles -- Using the RBPF (Rao-Blackwellized) variant -- Using EKF/UKF instead if appropriate +Common causes are a wrong frame declaration (see above), unrealistic IMU values, or a filter +that has diverged. The run's health monitor stops a run whose state leaves configured bounds; +`--log-level debug` shows what happened before it did. ### Where can I get help? -- Check this FAQ and the user guide -- Search existing [GitHub Issues](https://github.com/jbrodovsky/strapdown-rs/issues) -- Open a new issue if you've found a bug -- Contact the maintainer for research collaborations - -## Additional Questions? - -If your question isn't answered here, please: -1. Check the [User Guide](./user-guide/overview.md) -2. Review the [API Reference](./api/core.md) -3. Open an issue on [GitHub](https://github.com/jbrodovsky/strapdown-rs/issues) +- this FAQ and the [User Guide](./user-guide/overview.md); +- existing [GitHub Issues](https://github.com/jbrodovsky/strapdown-rs/issues); +- a new issue, if you have found a bug. diff --git a/book/src/filters/comparison.md b/book/src/filters/comparison.md index e80b64ff..d7bdad6d 100644 --- a/book/src/filters/comparison.md +++ b/book/src/filters/comparison.md @@ -1 +1,67 @@ -# EKF vs UKF Comparison +# Comparison + +The three Kalman filters share the mechanization, the measurement models, the gating and the +initialization helpers (see [Kalman Filters](./kalman.md)). They differ in what they carry as +state and how they handle the nonlinearity. + +| | [ESKF](./eskf.md) | [EKF](./ekf.md) | [UKF](./ukf.md) | +| --- | --- | --- | --- | +| `cl --filter` | `eskf` (default) | `ekf` | `ukf` | +| estimates | the error in a nominal trajectory | the full state | the full state | +| attitude | quaternion nominal, rotation-vector error, multiplicative correction | Euler angles, additive correction | Euler angles, perturbed, averaged and corrected on the rotation group | +| IMU biases | always (15 error states) | with `use_biases` (the default) | always through `initialize_ukf` | +| extra states | barometric bias only | map biases and barometric bias | map biases and barometric bias | +| nonlinearity | linearized about a nominal that is reset after every update | linearized about the current estimate (analytic Jacobians) | sigma points; no Jacobians in the filter | +| covariance update | Joseph form, then reset transport | Joseph form | $P - KSK^\top$, then reset transport | +| tuning beyond $P_0$, $q$ | none | none | $\alpha$, $\beta$, $\kappa$ | +| used by default in | `strapdown-sim cl`, `InsEngine` | | | + +## When to use which + +- **Start with the ESKF.** It is the default for structural reasons, not tuning: biases are + always estimated, attitude is never linearized about Euler angles, and the error the filter + linearizes is small by construction. It is the only filter `InsEngine` builds on its own. It has + no geophysical implementation: `cl --geo` refuses it, and because it is the default, a + geophysical run has to name `--filter ekf` or `--filter ukf`. +- **Use the EKF** for map-aided runs (the geophysical path runs the EKF or the UKF), when you need a + nine-state filter, or when you want the textbook full-state formulation for comparison with + published work. Its Jacobians are analytic and checked against the mechanization by + `core/tests/jacobian_agreement.rs`. +- **Use the UKF** to study whether linearization matters for your scenario: it needs no + Jacobians, so it is the reference against which a linearized filter's approximation shows + up. Keep $\alpha$ at its 0.1 default unless you have measured otherwise + ([why](./ukf.md#why-not-the-textbook-alpha-of-0001)). +- **Use the [RBPF](./rbpf.md)** when the measurement is a highly nonlinear or multimodal + function of position, which is the map-matching case it was built for. It is a particle + filter, run with `strapdown-sim pf`, and its configuration is separate. + +## Accuracy + +Accuracy is not asserted on this page. The numbers below are generated by +`core/tests/perf_baseline.rs`, which scores every filter over a fixed scenario matrix and fails +CI when a metric moves; `real_*` rows are a phone recording scored against its own GNSS track, +`syn_*` rows are synthetic and scored against exact truth. Read the +caveats on [Performance Baselines](../development/performance.md) +before drawing a conclusion from any row: on full-rate real-data rows, for example, every filter +is scored against fixes it has already been given. + +{{#include ../development/baseline-tables.md}} + +No wall-clock comparison of the filters is published; nothing in the repository measures +runtime. + +## Running all three on the same input + +Every filter reads the same input and writes the same CSV columns, so a comparison is three runs +of `cl` that differ only in `--filter` (run here on a `syn` trajectory): + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim cl -i synthetic.csv -o cl_eskf.csv --filter eskf +strapdown-sim cl -i synthetic.csv -o cl_ekf.csv --filter ekf +strapdown-sim cl -i synthetic.csv -o cl_ukf.csv --filter ukf +``` + +`strapdown-sim syn --no-noise` with the same flags writes the exact trajectory, and +[Tutorial: Particle Filter](../examples/tutorial-particle-filter.md#comparing-against-truth) +shows how to score an output CSV against it. diff --git a/book/src/filters/ekf.md b/book/src/filters/ekf.md index cae6020b..3e59153f 100644 --- a/book/src/filters/ekf.md +++ b/book/src/filters/ekf.md @@ -1,174 +1,83 @@ # Extended Kalman Filter (EKF) -The Extended Kalman Filter (EKF) is one of the primary navigation filters available in Strapdown-rs. It provides efficient state estimation for the INS through linearization of nonlinear system and measurement models. - -## Overview - -The EKF linearizes the nonlinear navigation equations using Jacobian matrices (first-order Taylor series expansion). While this approximation is less accurate than the UKF's unscented transform for highly nonlinear systems, it is computationally more efficient and works well for most practical navigation scenarios. - -## Mathematical Foundation - -### State Vector - -The EKF supports two state configurations: - -**9-State Model (Navigation Only)**: -- Latitude, longitude, altitude -- North, east, down velocities -- Roll, pitch, yaw angles - -**15-State Model (Navigation + Biases)**: -- 9 navigation states (as above) -- Accelerometer biases (3) -- Gyroscope biases (3) - -### Prediction Step - -The prediction step propagates the state and covariance forward using IMU measurements using the strapdown mechanization and computed Jacobian matrices. - -### Update Step - -When GNSS or other measurements are available, the Kalman gain is computed and the state is updated with the measurement innovation. - -## Features - -### Advantages - -1. **Computational Efficiency**: 3-5x faster than UKF -2. **Memory Efficient**: Stores only mean and covariance -3. **Well-Understood**: Extensive literature and proven track record -4. **Analytic Jacobians**: Uses pre-computed derivatives for accuracy - -### Limitations - -1. **Linearization Error**: Less accurate for highly nonlinear systems -2. **First-Order Approximation**: May miss higher-order effects -3. **Gaussian Assumption**: Cannot handle multimodal distributions - -## Usage - -### Basic Initialization - -```rust -use strapdown::kalman::{ExtendedKalmanFilter, InitialState, NavigationFilter}; -use nalgebra::{DMatrix, DVector}; - -// Define initial state -let initial_state = InitialState { - latitude: 45.0, - longitude: -122.0, - altitude: 100.0, - northward_velocity: 0.0, - eastward_velocity: 0.0, - vertical_velocity: 0.0, - roll: 0.0, - pitch: 0.0, - yaw: 0.0, - in_degrees: true, - is_enu: false, // NED, the crate default; set true for ENU data -}; - -// Initial covariance diagonal. -// -// Latitude and longitude are held in RADIANS and altitude in metres, so the three position -// entries are not the same unit and cannot come from one literal. `vec![1e-6; 9]` -- what -// this example used to show -- reads as a 6.4 km initial horizontal uncertainty rather than -// as a small number, and `1e-9` as a process-noise density is a 201 m one. Both are issue -// #308; see **Units on the covariance diagonals** below. Write the metres once, convert where -// they are used. -let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; // a 10 m GNSS fix -let mut initial_covariance = vec![ - horizontal_std_rad.powi(2), // latitude, rad^2 - horizontal_std_rad.powi(2), // longitude, rad^2 - 10.0_f64.powi(2), // altitude, m^2 -]; -initial_covariance.extend([0.25; 3]); // velocity, (m/s)^2 -- a 0.5 m/s fix -initial_covariance.extend([1e-4; 3]); // attitude, rad^2 -- ~0.6 deg - -// Initialize 9-state EKF (no biases) -let mut ekf = ExtendedKalmanFilter::new( - initial_state, - vec![], // No biases for 9-state - initial_covariance, - // The crate's own default, which is built the same way: one metric constant, converted - // once. Nine-state filters take its leading nine entries. - DMatrix::from_diagonal(&DVector::from_vec( - strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY[0..9].to_vec(), - )), - false, // use_biases = false for 9-state -); +`kalman::ExtendedKalmanFilter` is a full-state EKF: position, velocity and attitude (as roll, +pitch and yaw) are the state vector itself, and the filter linearizes the mechanization about +its current estimate with analytic Jacobians (Groves Chapter 14.2). Run it from the command line +with `strapdown-sim cl --filter ekf`. For when to prefer it over the default +[ESKF](./eskf.md), see [Comparison](./comparison.md). + +## State + +| configuration | states | +| --- | --- | +| 9-state (`use_biases: false`) | latitude, longitude, altitude, velocity (3), roll, pitch, yaw | +| 15-state (`use_biases: true`, the `EkfConfig` default) | the nine above, then accelerometer bias (3) and gyroscope bias (3) | +| extended | the fifteen, then any `other_states` (geophysical map biases), then the barometric bias if `estimate_baro_bias` is set | + +Units and order are the [shared layout](./kalman.md#state-layout-and-units): latitude and +longitude in radians, attitude in radians on the principal branch. Extra states and the +barometric bias require `use_biases`; `initialize_ekf` refuses them otherwise. + +## Predict + +1. Bias-correct the increments with the current bias estimate, as the ESKF does: + $\Delta v^b = \Delta\tilde v^b - b_a\Delta t$, $\Delta\theta^b = \Delta\tilde\theta^b - b_g\Delta t$. +2. Compute the transition Jacobian $F$ at the **pre-propagation** state with + `linearize::euler_state_transition_jacobian`, from the bias-corrected average rates. +3. For the 15-state filter, widen $F$ with `linearize::widen_with_imu_bias_coupling`: the + blocks through which the accelerometer bias reaches velocity (and position, through the + trapezoidal half-step) and the gyroscope bias reaches attitude. Without them the bias states + exist but never correlate with anything a measurement observes, and the filter estimates nine + states while carrying fifteen; that was the EKF until #394. Extra states get an identity + (random-walk) diagonal. +4. Mechanize the state with `mechanize`. +5. $P \leftarrow F P F^\top + q\ \Delta t$, then regularize. There is no separate noise-input + matrix $G$: $q$ is taken to be the process-noise density already expressed in state space. + +### The Jacobian is in the Euler chart + +The EKF's attitude states are Euler angles, so its Jacobian differentiates with respect to +roll, pitch and yaw. That is a different matrix from the rotation-vector form an error-state +filter uses: the two differ by the Euler-rate matrix $E(\Phi)$, and the difference is as large +as the terms themselves. `linearize` names the choice through `AttitudeParametrization` +(`Euler` for this filter, `RotationVector` for the error-state form) rather than leaving it +implied by which function is called. Using the rotation-vector form in the EKF was #307. + +## Update + +1. Evaluate the measurement Jacobian $H$ from the model's `get_jacobian` first: a geophysical + model whose estimate has left its map reports that as an error there, before any NaN reaches + the innovation. Every model writes its attitude columns as $\partial h/\partial$(roll, pitch, + yaw), which is exactly this filter's chart, so no conversion is needed. A nine-column + Jacobian is padded with zeros to the state width. +2. $S = HPH^\top + R$, innovation $\nu = z - h(x)$, wrapped by the model for angles. +3. Gate, if a gate is installed ([gating](./measurements.md#innovation-gating)). +4. $K = PH^\top S^{-1}$ (through an SPD solve), $x \leftarrow x + K\nu$, then wrap the Euler + angles back onto the principal branch. +5. Joseph-form covariance update, $P \leftarrow (I-KH)P(I-KH)^\top + KRK^\top$, then + regularize. + +## Construction + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:ekf_new}} ``` -### 15-State with Bias Estimation - -```rust -// The nine-state diagonal built above, extended with the bias states. The same unit -// caveat applies to its position block, and for the same reason. -let mut initial_covariance_15 = initial_covariance.clone(); -initial_covariance_15.extend([1e-3; 3]); // accel bias, (m/s^2)^2 -initial_covariance_15.extend([1e-8; 3]); // gyro bias, (rad/s)^2 - -// Initialize 15-state EKF with bias estimation -let mut ekf = ExtendedKalmanFilter::new( - initial_state, - vec![0.0; 6], // Initial bias estimates (3 accel + 3 gyro) - initial_covariance_15, - DMatrix::from_diagonal(&DVector::from_vec( - strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY.to_vec(), - )), // Process noise - true, // use_biases = true for 15-state -); -``` - -### Prediction with IMU Data - -```rust -use strapdown::IMUData; - -let imu_data = IMUData { - timestamp: 1234567890.0, - gyro: [0.01, -0.02, 0.03], // rad/s - accel: [0.5, -0.2, 9.81], // m/s² -}; - -let dt = 0.01; // Time step in seconds -ekf.predict(&imu_data, dt); -``` - -### Update with GNSS - -```rust -use strapdown::measurements::GPSPositionMeasurement; - -let gps_measurement = GPSPositionMeasurement { - latitude: 45.00012, - longitude: -122.00015, - altitude: 101.5, - std_dev: [5.0, 5.0, 10.0], // Measurement uncertainty -}; - -ekf.update(&gps_measurement); -``` - -## Performance - -### Computational Complexity - -- Typical update rate: 10,000-20,000 updates/second on modern hardware -- 3-5x faster than UKF -- Memory usage: ~2 KB for 15-state - -## Comparison with UKF +The arguments are an `InitialState`, a slice of initial bias estimates (used only when +`use_biases` is `true`, with any further entries seeding extra states), the covariance +diagonal, the process-noise density $q$ as a matrix, and `use_biases`. The state's width is the +length of the covariance diagonal; states beyond the nine navigation states and the supplied +seeds start at zero. `kalman_filters.rs` builds the diagonal with +`IMUQuality::auto_covariance`; see [Kalman Filters](./kalman.md#the-constructors-with-p0-from-the-imu-grade). -| Aspect | EKF | UKF | -|--------|-----|-----| -| **Speed** | Faster (3-5x) | Slower | -| **Accuracy** | Good for mildly nonlinear | Better for highly nonlinear | -| **Implementation** | Requires Jacobians | No Jacobians needed | -| **Memory** | Lower | Higher | -| **Best For** | Real-time systems | Research/offline processing | +`sim::initialize_ekf(&record, EkfConfig)` builds the same filter from a `TestDataRecord`, which +is what `strapdown-sim` does; [Kalman Filters](./kalman.md#the-siminitialize_-helpers) lists +the `EkfConfig` fields and the $P_0$ it derives. `EkfConfig::default()` is the 15-state filter; +its `Default` is written by hand precisely so that it does not silently become the 9-state one. -See [EKF vs UKF Comparison](./comparison.md) for detailed analysis. +Driving it is the same as any filter: `predict` with an `ImuSample` or `IMUData`, `update` with +a [measurement model](./measurements.md), each returning a `Result` +([Driving a filter](./kalman.md#driving-a-filter)). ## Units on the covariance diagonals @@ -184,10 +93,10 @@ It is worth knowing what the round numbers mean once the conversion is skipped: |---|---| | `1e-6` rad² | 6367 m | | `1e-9` rad² | 201 m | -| `(5 m × METERS_TO_DEGREES)²` | 286 m (degrees, not radians -- 57.3x too large) | +| `(5 m × METERS_TO_DEGREES)²` | 286 m (degrees, not radians: 57.3x too large) | All three shipped in this crate, in $Q$ and in $P_0$, and are what issue #308 fixed. The -symptom is characteristic: with $Q$ that large the innovation covariance $S = HPH^T + R$ is +symptom is characteristic: with $Q$ that large the innovation covariance $S = HPH^\top + R$ is dominated by the filter's own prediction, so the update discards it and lands on each fix, the solution tracks the fix noise one-for-one instead of averaging it down, and innovation gating cannot function because a genuinely bad fix is still inside what the filter believes @@ -195,20 +104,60 @@ possible. `sim::DEFAULT_PROCESS_NOISE_DENSITY`, `sim::DEFAULT_INITIAL_POSITION_UNCERTAINTY_M` and the `sim::initialize_*` helpers all do the conversion for you; `IMUQuality::auto_covariance` -derives a whole $P_0$ diagonal from an IMU grade and a reported fix accuracy. +derives a whole $P_0$ diagonal from an IMU grade and a reported fix accuracy. Remember too that +the diagonal of $Q$ is a density: the filter multiplies it by $\Delta t$ +([Process noise](./kalman.md#process-noise-is-a-spectral-density)). + +## Analytic Jacobians, and how they are checked + +The EKF's correctness rests on its Jacobians matching the function they linearize, and a wrong +block does not crash: it makes the gain wrong, and the filter degrades in a way that looks like +bad tuning. `core/tests/jacobian_agreement.rs` therefore checks the analytic transition +Jacobians directly against central finite differences of the mechanization, rather than +inferring their correctness from whether a filter converges: + +- `euler_jacobian_matches_the_mechanization_in_both_frames` bounds every entry of the Euler + Jacobian against the finite difference, in NED and ENU, at a derived tolerance of $2\times10^{-5}$. +- `altitude_row_follows_the_frame` pins the one entry whose sign the frame decides: + $\partial\ \text{alt}/\partial v_\text{vertical}$ is $-\Delta t$ in NED, where vertical velocity is + positive down, and $+\Delta t$ in ENU. A wrong sign turns the altitude/vertical-velocity pair + into positive feedback. +- `the_two_parametrisations_differ_in_every_block_that_touches_attitude` checks that the Euler + and rotation-vector forms really are different matrices. + +```bash +cargo test -p strapdown-core --test jacobian_agreement +``` + +The measurement Jacobians live beside them in `core/src/linearize.rs` (`gps_position_jacobian`, +`gps_velocity_jacobian`, `relative_altitude_jacobian`, `magnetometer_yaw_jacobian`, `zupt_jacobian`, +`zaru_jacobian` and others). + +## Troubleshooting + +**The solution copies every fix, and gating never rejects anything.** Check the units of $P_0$ +and $Q$ against the table above. A horizontal variance written as if it were m² is thousands of +metres in radians. + +**The vertical channel runs away, or roll converges to 180°.** The data's frame disagrees with +the declared one. In NED a level IMU reads $-g$ on its down axis; Sensor Logger exports are ENU +and need `is_enu = true` (`--enu`). `sim::check_declared_frame` catches a wrong declaration +before propagation, and `strapdown-sim` calls it on every run. -## Best Practices +**Bias estimates do not move.** A 9-state filter has nowhere to put them; use `use_biases: +true`. On a 15-state filter, position and velocity fixes observe the biases only through the +coupling blocks, and an accelerometer bias and a small tilt produce the same horizontal +signature, so they are not separately observable from position and velocity alone +([Tutorial: GNSS Degradation](../examples/tutorial-gps-degradation.md) shows this). A ZARU +pseudo-measurement observes the gyro bias directly +([Measurement Models](./measurements.md#zero-velocity-and-zero-angular-rate-updates)). -1. **Start with 9-state** unless you need bias estimation -2. **Tune conservatively**: Start with larger uncertainties and reduce -3. **Monitor innovation**: Check measurement residuals for divergence -4. **Use 15-state** for long-duration missions or low-quality IMUs -5. **Validate with dead reckoning**: Compare against the `dr` subcommand's results -6. **Write position uncertainties in metres** and convert once -- see - [Units on the covariance diagonals](#units-on-the-covariance-diagonals) +**An update returns an error.** `StrapdownError::is_recoverable` distinguishes a measurement +that could not be evaluated (skip it) from a filter that can no longer continue. -## Next Steps +## References -- Try the [UKF](./ukf.md) for comparison -- Learn about [Measurement Models](./measurements.md) -- See [Example: Closed-Loop Simulation](../examples/tutorial-basic.md) +- Groves, P. D., *Principles of GNSS, Inertial, and Multisensor Integrated Navigation + Systems*, 2nd ed., Chapter 14.2. +- Bar-Shalom, Y., Li, X.-R. and Kirubarajan, T., *Estimation with Applications to Tracking and + Navigation*, Chapter 5. diff --git a/book/src/filters/eskf.md b/book/src/filters/eskf.md new file mode 100644 index 00000000..a0500c64 --- /dev/null +++ b/book/src/filters/eskf.md @@ -0,0 +1,199 @@ +# Error-State Kalman Filter (ESKF) + +`kalman::ErrorStateKalmanFilter` is the default filter: `strapdown-sim cl` runs it unless told +otherwise (`FilterType::default()` is `Eskf`), and `InsEngine::builder().build()` constructs +one. It is the indirect, error-state formulation of Groves Chapter 14 and Solà (2017): a +nonlinear **nominal** trajectory is mechanized at full fidelity, and a linear Kalman filter +estimates the small **error** in it. + +## Why it is the default + +The ESKF became the default in #258 for reasons that are structural rather than tuning: + +- **It estimates the IMU biases.** The fifteen-element error state always carries + accelerometer and gyroscope bias, coupled to velocity and attitude in the transition matrix, + so a turn-on bias is estimated online instead of integrated as motion. A nine-state EKF has no + bias states to put it in. +- **Attitude is a quaternion, corrected multiplicatively.** The nominal attitude is a unit + quaternion; the error is a three-element rotation vector. Nothing is linearized about Euler + angles, so there is no Euler-chart singularity in the propagation and no Euler addition in the + correction. +- **The linearization point is good by construction.** The error is reset to zero after every + update, so the linear model only ever has to describe a small perturbation. + +The EKF and UKF remain selectable and are unchanged by this default. For measured accuracy of +all three on the same scenarios see [Comparison](./comparison.md). + +## Nominal and error states + +The **nominal** state is held as typed quantities, not a vector: latitude and longitude +(radians), altitude (m), north/east/vertical velocity (m/s), a unit quaternion for the +body-to-navigation attitude, the two bias vectors and, when carried, a barometric bias. + +The **error** state $\delta x$ is + +| index | error in | unit | +| --- | --- | --- | +| 0, 1 | latitude, longitude | rad | +| 2 | altitude | m | +| 3, 4, 5 | north, east, vertical velocity | m/s | +| 6, 7, 8 | attitude, as a body-frame rotation vector $\delta\theta$ | rad | +| 9, 10, 11 | accelerometer bias | m/s² | +| 12, 13, 14 | gyroscope bias | rad/s | +| 15 (optional) | barometric bias | m | + +The position errors are in the filter's own units (radians for latitude and longitude), not +metres. The error-state transition matrix and the GNSS position Jacobian are both written in +those units, so the correction is added to the nominal latitude without conversion; dividing +by the radii of curvature there was #266. + +The filter's width is the length of the covariance diagonal it is constructed with: fifteen, +or sixteen when the last entry is a barometric bias. `get_estimate` returns the *nominal* state +in the [shared layout](./kalman.md#state-layout-and-units), converting the quaternion to Euler +angles; `get_certainty` returns the *error-state* covariance. + +## Predict + +1. **Bias-correct the increments.** Biases are rates and the increments are their integrals: + $$ + \Delta v^b = \Delta \tilde v^b - b_a \Delta t, \qquad + \Delta\theta^b = \Delta\tilde\theta^b - b_g \Delta t . + $$ +2. **Mechanize the nominal** with `mechanize`, the same strapdown equations every filter + uses, and renormalize the quaternion. +3. **Propagate the error covariance** with the error-state transition matrix from + `linearize::error_state_transition_jacobian` (Groves 14.2.4, Solà 2017 Section 6.3), + evaluated at the propagated nominal with the bias-corrected average rates: + $$ + P \leftarrow F_{\delta x}\ P\ F_{\delta x}^\top + q\ \Delta t . + $$ + $F_{\delta x}$ couples the accelerometer bias into velocity and the gyroscope bias into + attitude, which is what makes the biases observable from position and velocity fixes. A + barometric bias is a random walk with no coupling: it reaches the navigation states only + through the barometer's measurement Jacobian. + +## Update, injection and reset + +1. **Predict the measurement** from the nominal state, assembled into the + [shared layout](./kalman.md#state-layout-and-units) including the bias entries (ZARU reads + them). +2. **Form $H$.** Every measurement model writes its attitude columns as $\partial h / + \partial(\text{roll, pitch, yaw})$. The ESKF's attitude error is a body-frame rotation + vector, so those columns are converted by the chain rule, + $$ + \frac{\partial h}{\partial \delta\theta^b} = + \frac{\partial h}{\partial \Phi}\ \frac{\partial \Phi}{\partial \delta\theta^b}, + $$ + with the second factor from `linearize::body_rotation_vector_to_euler_jacobian`. It is the + identity only at zero roll. Near gimbal lock (about 89.45° of pitch) no finite matrix is + right, and the attitude columns are zeroed: the update declines to correct attitude from + that measurement rather than apply an enormous row. +3. **Gate**, if a gate is installed (see [gating](./measurements.md#innovation-gating)). A + rejection inflates the covariance in the observed directions and returns before anything + is injected; there is no undo after injection starts. +4. **Solve** $\delta x = K\nu$ with $K = P H^\top (H P H^\top + R)^{-1}$. +5. **Inject** the error into the nominal: + - position, velocity and biases are added; + - attitude is composed, $q \leftarrow q \otimes \operatorname{Exp}(\delta\theta)$, using the + exact exponential map rather than the first-order quaternion $[1, \tfrac12\delta\theta]$; + - the barometric bias, when carried, is added. +6. **Clamp the IMU biases** (anti-windup): each accelerometer bias to ±2.0 m/s² and each gyro + bias to ±0.05 rad/s. These are orders of magnitude above a consumer MEMS turn-on bias and + exist so that a persistently faulty aiding source cannot drag the biases to physically + impossible values (#286). The barometric bias is not clamped: it does not feed the + mechanization. +7. **Reset** $\delta x \leftarrow 0$. +8. **Update the covariance** in Joseph form, using the same prior $P$ the gain was computed + from: $P \leftarrow (I-KH)P(I-KH)^\top + KRK^\top$. +9. **Transport the covariance** onto the tangent space of the attitude just composed on: + $P \leftarrow G P G^\top$, where $G$ is the identity except for the attitude block, which + is the right Jacobian $J_r(\delta\theta)$ from `linearize::attitude_reset_jacobian`. The + whole covariance is transformed, so attitude's correlations with the other states move with + it (#398). +10. **Regularize**: symmetrize and add the relative jitter described on + [Kalman Filters](./kalman.md#the-update-and-keeping-the-covariance-healthy). + +## Construction + +The direct constructor takes an `InitialState`, six initial bias estimates, the covariance +diagonal and the process-noise density $q$: + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:eskf_new}} +``` + +`initialize_eskf` builds the same filter from a `TestDataRecord` and an `EskfConfig`; its +default $P_0$ is tabulated on [Kalman Filters](./kalman.md#the-siminitialize_-helpers). For the +sixteen-state form pass a sixteen-element diagonal and $q$, and tell the barometer model where +the bias is through `AidingConfig::baro_bias_index`; `EskfConfig::estimate_baro_bias` does the +first half for you, and `EskfConfig::baro_bias_index()` returns the index (always 15) for the +second. + +## Running it end to end + +[`core/examples/eskf.rs`](https://github.com/jbrodovsky/strapdown-rs/blob/main/core/examples/eskf.rs) +is the library route to what `strapdown-sim cl` does: a five-minute synthetic drive due north at +15 m/s from a consumer-grade IMU, GNSS for 120 s, a 60 s outage, then GNSS again, scored against +the exact trajectory. + +```bash +cargo run -p strapdown-core --example eskf +``` + +Build the filter with the barometric-bias state `strapdown-sim` turns on by default: + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:build}} +``` + +Build the event stream. A `DutyCycle` scheduler starts with `start_phase_s` of availability, +then alternates `off_s` of outage and `on_s` of availability; with a zero phase the run would +start in an outage. + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:stream}} +``` + +Run the closed loop and score it with `metrics::evaluate`: + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:run}} +``` + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:score}} +``` + +Read the bias estimates back off the state vector: + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:biases}} +``` + +It printed: + +```text +scored 3000 samples against truth +horizontal error: RMSE 8.49 m, max 54.72 m +end of outage: error 52.98 m, filter 1-sigma 378.53 m north, 379.08 m east +final accelerometer bias estimate: [-0.0541, -0.0320, -0.0694] m/s^2 +final gyroscope bias estimate: [0.00005, -0.00025, 0.00036] rad/s +final barometric bias estimate: 3.280 m +``` + +The "end of outage" line is the last sample before GNSS returns. On this run the filter's own +horizontal uncertainty at that point is several times its actual error: it is conservative, +not over-confident, while coasting. Whether that holds in general is what the consistency +metrics on [Performance Baselines](../development/performance.md) measure: the NEES and the +three-sigma containment columns, read together. + +## References + +- Groves, P. D., *Principles of GNSS, Inertial, and Multisensor Integrated Navigation + Systems*, 2nd ed., Chapter 14 (14.2.4 for the error-state transition matrix). +- Solà, J., "Quaternion kinematics for the error-state Kalman filter", 2017. +- Trawny, N. and Roumeliotis, S., "Indirect Kalman Filter for 3D Attitude Estimation", 2005. + +The implementation, with the reasoning behind each step in its comments, is +`ErrorStateKalmanFilter` in `core/src/kalman.rs` and `error_state_transition_jacobian` in +`core/src/linearize.rs`. diff --git a/book/src/filters/kalman.md b/book/src/filters/kalman.md index 6381b881..57b5e272 100644 --- a/book/src/filters/kalman.md +++ b/book/src/filters/kalman.md @@ -1 +1,272 @@ # Kalman Filters + +`strapdown-core` ships three Kalman-family filters in `core/src/kalman.rs`. All three are +loosely coupled: they take GNSS position and velocity fixes (and the other aiding sources on +[Measurement Models and Integrity](./measurements.md)), not pseudoranges or carrier phase. They +share one interface, one set of measurement models and one propagation core: the strapdown +mechanization of Groves Chapter 5.4-5.5 described in +[The Navigation Model](../user-guide/concepts.md). + +| filter | type | state | attitude carried as | `cl --filter` | +| --- | --- | --- | --- | --- | +| [Error-State Kalman Filter](./eskf.md) | `kalman::ErrorStateKalmanFilter` | 15-element error state, 16 with a barometric bias | unit quaternion, corrected multiplicatively | `eskf` (the default) | +| [Extended Kalman Filter](./ekf.md) | `kalman::ExtendedKalmanFilter` | 9 or 15 full states, plus optional extra states | Euler angles in the state vector | `ekf` | +| [Unscented Kalman Filter](./ukf.md) | `kalman::UnscentedKalmanFilter` | 15 full states through `initialize_ukf` (9 is possible from the constructor), plus optional extra states | Euler angles, perturbed and averaged on the rotation group | `ukf` | + +The [Comparison](./comparison.md) page says when to reach for which. The +[Rao-Blackwellized particle filter](./rbpf.md) implements the same trait and is documented +separately. + +## The `NavigationFilter` trait + +Every filter implements `strapdown::NavigationFilter`, which lives at the crate root +(`core/src/lib.rs`), not in `kalman`. The trait is object-safe, so a +`Box` can hold any of them; that is what lets the +[`InsEngine`](../user-guide/library.md) drive a filter it did not construct. + +| method | returns | what it does | +| --- | --- | --- | +| `predict(&mut self, control_input: &dyn InputModel, dt: f64)` | `Result<(), StrapdownError>` | Propagate by one inertial sample. | +| `update(&mut self, measurement: &dyn MeasurementModel)` | `Result` | Correct with one measurement. | +| `get_estimate(&self)` | `DVector` | The current state, in the filter's native units. | +| `get_certainty(&self)` | `DMatrix` | The current covariance. For the ESKF this is the *error-state* covariance. | +| `set_innovation_gate(&mut self, gate: Option)` | `bool` | Install or clear an innovation gate; `true` if the filter honours it. | +| `set_gate_recovery(&mut self, recovery: GateRecovery)` | `bool` | Tune how a rejection is recovered from; `true` if honoured. | +| `baro_bias_index(&self)` | `Option` | Where the barometric bias lives in `get_estimate`, if the filter carries one. | + +The last three have default bodies (`false`, `false`, `None`). All three Kalman filters and the +RBPF override the two gating methods. + +### Inputs: `InputModel` + +`predict` takes a `&dyn InputModel` and accepts the two inertial forms: + +- `ImuSample`: integrated increments ($\Delta v$, $\Delta\theta$) over its own `dt`. This is + what an IMU emits and what `mechanize` consumes. Its `dt` must agree with the `dt` argument + to a relative tolerance of $10^{-9}$, or `predict` returns + `StrapdownError::InconsistentTimestep` rather than silently preferring one of the two. +- `IMUData`: instantaneous specific force (m/s²) and angular rate (rad/s), which the filter + integrates over `dt` with `ImuSample::from_rates`. + +Anything else that implements `InputModel` (`VelocityData`, for example) is refused with +`StrapdownError::UnsupportedInput`. Accelerometer readings are raw specific force, gravity +included: in NED a level, stationary IMU reads $-g$ on its down axis. See +[Tutorial: Basic INS](../examples/tutorial-basic.md#the-sign-convention-that-catches-everyone). + +### Outputs: `UpdateOutcome` + +`update` returns an `UpdateOutcome` (defined in `gating`, re-exported at the crate root) rather +than `()`, because three cases have to be told apart and only one of them is an error: + +| field | meaning | +| --- | --- | +| `nis` | normalized innovation squared, $\nu^\top S^{-1}\nu$ with $S = HPH^\top + R$ | +| `dof` | the measurement's dimension, the degrees of freedom of the test | +| `accepted` | whether the correction was applied | +| `forced` | applied *despite* failing the gate, because the recovery policy forced it | + +A filter with no gate installed (the default) accepts every measurement and still reports the +NIS. A gated-out measurement is `Ok` with `accepted == false`: the state is unchanged apart from +the covariance inflation described under [gating](./measurements.md#innovation-gating). An +`Err` means the measurement could not be evaluated at all, and +`StrapdownError::is_recoverable` says whether to skip it and carry on (see +[Using the Library](../user-guide/library.md#errors)). + +### Driving a filter + +[`core/examples/kalman_filters.rs`](https://github.com/jbrodovsky/strapdown-rs/blob/main/core/examples/kalman_filters.rs) +builds all three filters two ways, puts the six behind the trait, and runs one loop over a +two-minute synthetic drive with a GNSS position fix once a second: + +```bash +cargo run -p strapdown-core --example kalman_filters +``` + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:trait_objects}} +``` + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:loop}} +``` + +It printed: + +```text +ESKF, initialize_eskf: 15 states, 2.763 m from truth at the end, last update NIS 3.530 (3 dof, accepted: true) +EKF, initialize_ekf: 15 states, 2.761 m from truth at the end, last update NIS 3.530 (3 dof, accepted: true) +UKF, initialize_ukf: 15 states, 2.759 m from truth at the end, last update NIS 3.533 (3 dof, accepted: true) +ESKF, constructor: 15 states, 2.762 m from truth at the end, last update NIS 3.529 (3 dof, accepted: true) +EKF, constructor: 15 states, 2.762 m from truth at the end, last update NIS 3.529 (3 dof, accepted: true) +UKF, constructor: 15 states, 2.760 m from truth at the end, last update NIS 3.532 (3 dof, accepted: true) +``` + +On a short straight drive with a 2.5 m fix every second, all six end up where the fixes put +them. That is a property of this easy scenario, not a ranking; the gated comparison is on +[Comparison](./comparison.md). + +## State layout and units + +All three filters report the same layout from `get_estimate`: + +| index | state | unit | +| --- | --- | --- | +| 0, 1 | latitude, longitude | **radians** | +| 2 | altitude above the WGS84 ellipsoid, positive up in both frames | m | +| 3, 4, 5 | north, east, vertical velocity (vertical is positive down in NED, up in ENU) | m/s | +| 6, 7, 8 | roll, pitch, yaw (XYZ Euler sequence, body to navigation) | rad | +| 9, 10, 11 | accelerometer bias, body frame | m/s² | +| 12, 13, 14 | gyroscope bias, body frame | rad/s | +| 15 onward | extra states: geophysical map biases, then the barometric bias | their own units | + +A nine-state EKF stops at index 8. Latitude and longitude are radians and altitude is metres, +so no covariance diagonal, $P_0$ or $Q$, can be filled with one number for all three position +entries. [Units on the covariance diagonals](./ekf.md#units-on-the-covariance-diagonals) shows +what goes wrong when it is. + +The frame is a property of the data. Every filter is NED unless its `InitialState` or config +sets `is_enu`; see [Coordinate Frames](../user-guide/coordinate-frames.md). + +## Process noise is a spectral density + +Every filter forms the per-step process noise as + +$$ +Q_k = q \ \Delta t +$$ + +from a diagonal $q$ of **variances per second**, and adds it to the propagated covariance. The +noise a run accumulates therefore depends on elapsed time, not on how often the IMU was sampled. +Before #374 the same numbers were added once per sample, which made the effective noise +proportional to the sample rate. The `process_noise` argument of every constructor, and every +`process_noise_diagonal` config field, is $q$, not $Q_k$. + +The default is `sim::DEFAULT_PROCESS_NOISE_DENSITY`, fifteen entries in the state order above: + +| states | density $q$ | +| --- | --- | +| latitude, longitude | $(0.1\ \text{m} \cdot \texttt{METERS\\_TO\\_RADIANS})^2$ rad²/s, i.e. 0.1 m/√s | +| altitude | $10^{-2}$ m²/s, i.e. 0.1 m/√s | +| velocity (3) | $10^{-3}$ (m/s)²/s | +| attitude (3) | $10^{-5}$ rad²/s | +| accelerometer bias (3) | $10^{-6}$ (m/s²)²/s | +| gyroscope bias (3) | $10^{-8}$ (rad/s)²/s | + +The position entries come from one constant, `sim::POSITION_PROCESS_NOISE_M_PER_ROOT_S` +(0.1 m/√s), converted to radians for the horizontal pair. The rest are hand-tuned values, not +derived from a sensor model. A nine-state EKF takes the leading nine entries. A barometric bias +state walks at `sim::BARO_BIAS_PROCESS_NOISE_M2_PER_S`, derived from 8.3 m (about one +hectopascal) of reference drift per hour. + +## The update, and keeping the covariance healthy + +The measurement update is the standard one (Groves Chapter 3): +$K = P H^\top (H P H^\top + R)^{-1}$, with the gain solved through a symmetric +positive-definite solver rather than an explicit inverse. Angular innovations (magnetometer +yaw) are wrapped onto $[-\pi, \pi)$ by the model's `wrap_residual` before they are used. + +- The **EKF and ESKF** update the covariance in **Joseph form**, + $P^+ = (I - KH) P (I - KH)^\top + K R K^\top$, which stays symmetric and positive + semi-definite under rounding where $(I - KH)P$ does not. +- The **UKF** subtracts $K S K^\top$, its sigma-point equivalent. + +After every predict and update each filter symmetrizes $P$ and adds a jitter proportional to +each diagonal entry's own scale: a relative $10^{-9}$, with the process-noise diagonal as a +floor. An absolute jitter cannot work on a state whose units span twelve orders of magnitude. +The absolute $10^{-9}$ the UKF and EKF used to add was about 200 m of horizontal standard +deviation on a radian-valued latitude (#373). + +## Initialization + +A filter needs an initial state, an initial covariance $P_0$ and a process-noise density $q$. +There are two routes. + +### The `sim::initialize_*` helpers + +`initialize_eskf`, `initialize_ekf` and `initialize_ukf` (in `core/src/sim.rs`) build a filter +from one `TestDataRecord`, which is how `strapdown-sim cl` starts every run: + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:helpers}} +``` + +The record supplies the initial state through `TestDataRecord::initial_state(is_enu)`: position +from its GNSS columns, attitude from its quaternion, horizontal velocity from its `speed` and +`bearing`, and zero vertical velocity. Each helper takes a config struct. All three are +`#[non_exhaustive]`, so outside the crate you start from `default()` and assign fields. + +| field | `EskfConfig` | `EkfConfig` | `UkfConfig` | meaning | +| --- | :-: | :-: | :-: | --- | +| `is_enu` | yes | yes | yes | frame of the records; `false` (NED) by default | +| `imu_quality` | yes | yes | yes | IMU grade the bias prior is derived from; `Consumer` by default | +| `imu_biases` | yes | yes | yes | initial bias **estimate**, 6 values; zero by default | +| `imu_biases_covariance` | yes | yes | yes | initial bias variances, 6 values; default `imu_quality.initial_bias_covariance()` | +| `attitude_covariance` | yes | yes | yes | initial attitude variances, 3 values, rad² | +| `process_noise_diagonal` | yes | yes | yes | $q$, one entry per state; default `DEFAULT_PROCESS_NOISE_DENSITY` | +| `estimate_baro_bias` | yes | yes | yes | add a barometric-bias state; `false` by default | +| `use_biases` | | yes | | 15 states (`true`, the default) or 9 | +| `other_states`, `other_states_covariance` | | yes | yes | extra states appended after the fifteen | +| `ukf_alpha`, `ukf_beta`, `ukf_kappa` | | | yes | sigma-point parameters; see [UKF](./ukf.md) | + +The helpers do not derive $P_0$ identically: + +| $P_0$ block | `initialize_eskf` | `initialize_ekf`, `initialize_ukf` | +| --- | --- | --- | +| horizontal position | `DEFAULT_INITIAL_POSITION_UNCERTAINTY_M` (10 m), as rad² | the record's `horizontal_accuracy`, as rad² | +| altitude | (10 m)² | the record's `vertical_accuracy`, squared | +| velocity | $10^{-3}$ (m/s)² | the record's `speed_accuracy`, squared | +| attitude | $10^{-5}$ rad² | $10^{-9}$ rad² | +| IMU biases | `imu_quality.initial_bias_covariance()` | the same | + +`estimate_baro_bias` defaults to `false` on the three library configs but to `true` in +`strapdown-sim` (`ClosedLoopConfig`; `--no-estimate-baro-bias` turns it off). A filter that +carries the bias state must also have it *named* to the barometer model, through +`AidingConfig::baro_bias_index`, or nothing observes it. Read the index from the config's +`baro_bias_index()` method; [ESKF](./eskf.md#running-it-end-to-end) shows the pair. + +The helpers do not check the declared frame against the data, because one record cannot tell a +frame error from a manoeuvre. Call `sim::check_declared_frame` over the whole record slice, as +`strapdown-sim` does. + +### The constructors, with P0 from the IMU grade + +Each filter can also be built directly from a `kalman::InitialState`. `InitialState` holds its +latitude, longitude and Euler angles in whatever unit its `in_degrees` flag names, and the +constructors convert exactly when that flag is set. + +`IMUQuality::auto_covariance` derives a fifteen-element $P_0$ from two things known at start-up: +the IMU grade, and how well the first fix placed the vehicle (an `InitialUncertainty` holding +one-sigma horizontal and vertical position in metres and velocity in m/s). Each block comes back +in the filter's own units: + +| states | derived from | +| --- | --- | +| position | the uncertainty, converted to rad² through the WGS84 radii at the given latitude (Groves Eq. 2.105-2.106) | +| velocity | the uncertainty, plus the grade's velocity random walk over a one-minute alignment | +| attitude | the grade's angle random walk over the same minute, plus the levelling error a bias-instability-sized accelerometer bias causes (Groves 5.6.3) | +| accelerometer bias | the grade's bias instability, squared | +| gyroscope bias | the grade's bias instability, converted from rad/h to rad/s, squared | + +It treats heading like roll and pitch, so widen entry 8 when the initial yaw came from a +magnetometer or a gyrocompass, as +[Using the Library](../user-guide/library.md#coarse-alignment) does. + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:initial_covariance}} +``` + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:constructors}} +``` + +The constructors are infallible and take the filter's width from the length of the covariance +diagonal; they do not check it. The `initialize_*` helpers do, and return +`StrapdownError::InvalidConfiguration` on a mismatch. + +## Where next + +- [ESKF](./eskf.md): the default filter, its error state and its reset. +- [EKF](./ekf.md): analytic Jacobians, and the units traps on $P_0$ and $Q$. +- [UKF](./ukf.md): sigma points, and why $\alpha = 0.1$. +- [Measurement Models and Integrity](./measurements.md): what `update` accepts, and gating. +- [Closed Loop](../user-guide/closed-loop.md): running these filters from the command line. diff --git a/book/src/filters/measurements.md b/book/src/filters/measurements.md index 66462436..80022994 100644 --- a/book/src/filters/measurements.md +++ b/book/src/filters/measurements.md @@ -1 +1,229 @@ -# Measurement Models +# Measurement Models and Integrity + +Every filter's `update` takes a `&dyn MeasurementModel`, so any model below can be applied to +the ESKF, EKF, UKF or RBPF, and a new aiding source is a new type implementing the trait, with no +change to the filters. The models live in `core/src/measurements.rs`; the stationary detector +in `core/src/stationary.rs`; innovation gating in `core/src/gating.rs`. + +The integration is **loosely coupled**: GNSS enters as a position (and velocity) solution +produced by the receiver. There are no pseudorange or carrier-phase models in this crate. + +## The `MeasurementModel` trait + +| method | returns | meaning | +| --- | --- | --- | +| `get_dimension()` | `usize` | length of the measurement vector | +| `get_measurement(&state)` | `Result, StrapdownError>` | the observed $z$; `state` is passed so a model can use it (the magnetometer levels itself with the estimated roll and pitch) | +| `get_expected_measurement(&state)` | `DVector` | the prediction $h(x)$ | +| `get_noise()` | `DMatrix` | the measurement covariance $R$, a **variance** | +| `get_jacobian(&state)` | `Result, StrapdownError>` | $H = \partial h/\partial x$ | +| `wrap_residual(&mut residual)` | | wraps angular innovations; the default does nothing | + +Three conventions hold for every model: + +- `state` is the [shared layout](./kalman.md#state-layout-and-units): latitude and longitude in + radians, attitude as roll, pitch and yaw. +- The attitude columns of $H$ are derivatives with respect to **roll, pitch and yaw**. The EKF + uses them directly; the ESKF converts them to its body-frame rotation-vector error + ([ESKF](./eskf.md#update-injection-and-reset)) and the RBPF to its nav-frame tilt. +- Every `*_noise_std` field is a one-sigma standard deviation; the model squares it to form + $R$. + +`get_measurement` and `get_jacobian` return `Result` because some models can legitimately fail +to produce a value; a geophysical model queried off its map is the main case, and its error is +*recoverable* (skip the measurement, carry on). + +## The models + +| model | dim | $z$ | noise fields | observes | +| --- | :-: | --- | --- | --- | +| `GPSPositionMeasurement` | 3 | latitude, longitude (given in degrees, converted to radians), altitude | `horizontal_noise_std`, `vertical_noise_std` (m) | position | +| `GPSVelocityMeasurement` | 3 | north, east, vertical velocity (vertical in the state's frame convention) | `horizontal_noise_std`, `vertical_noise_std` (m/s) | velocity | +| `GPSPositionAndVelocityMeasurement` | 5 | latitude, longitude, altitude, north and east velocity; **no vertical velocity** | `horizontal_noise_std`, `vertical_noise_std` (m), `velocity_noise_std` (m/s) | position, horizontal velocity | +| `RelativeAltitudeMeasurement` | 1 | `relative_altitude + reference_altitude` | `noise_std` (m) | altitude, and a barometric bias if `bias_index` names one | +| `MagnetometerYawMeasurement` | 1 | tilt-compensated heading from a body-frame field, optionally corrected by WMM declination | `noise_std` (rad) | yaw | +| `ZuptMeasurement` | 3 | zero velocity | `velocity_noise_std` (m/s) | velocity | +| `ZaruMeasurement` | 3 | the raw body-frame gyro reading | `angular_rate_noise_std` (rad/s) | gyroscope bias | + +The GNSS position noise is a ground distance in metres. It is converted to latitude and +longitude variances through the WGS84 radii of curvature at the fix, separately for each axis, +because a metre of easting is a larger angle of longitude than a metre of northing is of +latitude. + +### What `strapdown-sim` feeds the filter + +`messages::build_event_stream` turns each input record into events, and the simulator's filters +see three measurement types: + +| source | model | rate | noise | +| --- | --- | --- | --- | +| GNSS | `GPSPositionAndVelocityMeasurement` | every record with a fix, then thinned by the `[aiding]` scheduler and corrupted by its fault model | the record's `horizontalAccuracy`, `verticalAccuracy`, `speedAccuracy` | +| barometer | `RelativeAltitudeMeasurement` | `baro_scheduler`, 1 Hz by default | `baro_noise_std_m`, default $\sqrt{5}$ m | +| magnetometer | `MagnetometerYawMeasurement`, declination on | `magnetometer_scheduler`, 1 Hz by default | `MAG_YAW_NOISE`, 0.2 rad, fixed | + +[Schedulers and Faults](../gnss/scenarios.md) covers the scheduler and fault options. ZUPT and +ZARU are not applied by the simulator; they are library building blocks. + +### Barometric altitude + +`RelativeAltitudeMeasurement` predicts `altitude + bias`, where the bias is `state[bias_index]` +when `bias_index` is `Some` and zero otherwise. An index that falls inside the fifteen +navigation and IMU-bias states, or past the end of the state, is refused with an error rather +than read as a bias. The default noise, `BAROMETRIC_ALTITUDE_NOISE_M`, is $\sqrt5 \approx 2.24$ +m: it keeps the variance the model used before the field existed, which was 5 m². + +### Magnetometer yaw + +`MagnetometerYawMeasurement` levels the body-frame field with the state's roll and pitch (never +its yaw, which would make the measurement depend on the quantity it measures), takes the +heading with the `atan2` appropriate to the frame, and, with `apply_declination`, adds the World +Magnetic Model declination at the state's position for `year` and `day_of_year`. Declination +enters with opposite signs in NED and ENU, and the `is_enu` field **must** match the state's +frame: a mismatch reflects the heading rather than degrading it. `get_declination(lat_deg, +lon_deg, alt_m)` returns the declination in radians, positive east. + +```rust,ignore +{{#include ../../../core/examples/aiding.rs:magnetometer}} +``` + +### Zero-velocity and zero-angular-rate updates + +A stationary vehicle is free information. `ZuptMeasurement` asserts the velocity is zero +(Groves 15.2.1); because velocity error is correlated with accelerometer bias error, the +correction reaches the bias too. `ZaruMeasurement` asserts that a stationary gyro reads only its +own bias plus the Earth's rotation, which makes it a direct observation of the gyro bias (Groves +15.2.2). It predicts $b_g + C_n^b\ \omega_{ie}^n$ (`set_earth_rate_compensation(false)` drops +the Earth-rate term), and it **requires** a filter with gyro-bias states: on a nine-state filter +it returns `StrapdownError::DimensionMismatch` instead of silently correcting nothing. + +Neither model decides whether the vehicle is still. That is `stationary::StationaryDetector`'s +job: it keeps a sliding window of IMU samples and declares the platform stationary when all four +hold across the window: + +1. specific-force variance (trace) below `accel_variance_threshold`, (m/s²)²; +2. mean specific-force magnitude within `gravity_tolerance_mps2` of 9.807 m/s²; +3. angular-rate variance (trace) below `gyro_variance_threshold`, (rad/s)²; +4. mean angular-rate magnitude below `gyro_mean_threshold`, rad/s. This separates a stop from a + steady turn, which has small gyro *variance* and a large mean. + +It latches only after `min_stationary_samples` consecutive stationary windows and releases on +the first sample that fails. `StationaryConfig::default()` (a 100-sample window, latching after +50 more) is tuned for a consumer MEMS IMU at about 100 Hz. Constant-velocity cruise on a smooth +road can pass all four tests; prefer conservative thresholds, since a false stop injects a hard +zero-velocity constraint into a moving solution. + +[`core/examples/aiding.rs`](https://github.com/jbrodovsky/strapdown-rs/blob/main/core/examples/aiding.rs) +parks an ESKF-backed `InsEngine` for 30 s with a gyro bias the filter has not been told about: + +```bash +cargo run -p strapdown-core --example aiding +``` + +```rust,ignore +{{#include ../../../core/examples/aiding.rs:stationary}} +``` + +### Geophysical models + +The experimental `strapdown-geonav` crate adds gravity-anomaly, magnetic-anomaly and combined +models (`GravityMeasurement`, `MagneticAnomalyMeasurement`, `CombinedGeophysicalMeasurement`) +that implement the same trait against a loaded map. See +[Maps and Measurement Models](../geonav/maps.md). + +## Innovation gating + +A Kalman update trusts its measurement unconditionally. Under the filter's own model the +innovation is $\nu \sim \mathcal N(0, S)$ with $S = HPH^\top + R$, so the normalized innovation +squared + +$$ +d^2 = \nu^\top S^{-1} \nu +$$ + +is $\chi^2$-distributed with as many degrees of freedom as the measurement has components +(Bar-Shalom, Li and Kirubarajan, Section 5.4). Rejecting an update whose NIS exceeds a $\chi^2$ +quantile discards what the model says should almost never happen. Every filter reports the NIS +in its [`UpdateOutcome`](./kalman.md#outputs-updateoutcome) whether or not a gate is installed. + +`gating::InnovationGate` has two variants: + +| variant | threshold | constructor | +| --- | --- | --- | +| `ChiSquared { confidence }` | the `confidence` quantile of $\chi^2$ at the measurement's own dimension | `InnovationGate::chi_squared(confidence)`, refusing values outside $(0, 1)$ | +| `Fixed { threshold }` | the same number for every measurement | `InnovationGate::fixed(threshold)` | + +Prefer the $\chi^2$ form: a filter mixing 1-D barometer, 3-D position and 5-D position and +velocity updates needs a different threshold for each. `gating::chi_squared_quantile(p, dof)` +and `chi_squared_cdf(x, dof)` are public. Gating is **off** by default everywhere: in the +library, in `ClosedLoopConfig` and on the command line. + +### A gate needs a way back + +Rejecting a fix leaves the state where it was, but the filter keeps propagating and keeps +accumulating error, while the covariance the next fix is judged against does not grow. The next +innovation is larger, fails by more, and one rejection can become permanent dead reckoning +(#340). `gating::GateRecovery` is the way back, and it is on by default once a gate is +installed: + +| field | default | effect | +| --- | --- | --- | +| `rejection_inflation` | 2.0 | each rejection multiplies the filter's uncertainty in the directions that measurement observes, so $HPH^\top$ grows by this factor; 1.0 disables it | +| `forced_update_after` | `Some(5)` | when this many consecutive measurements in a row fail the gate, the last of them is applied anyway and reported with `forced: true`; `None` disables it, and values below 2 are refused by `GateRecovery::new` | + +The streak counts every sensor and resets on any accepted update. `GateRecovery::none()` turns +both mechanisms off. + +The third scene of `aiding.rs` installs a 0.999 gate on a vehicle driving north at 10 m/s, then +from t = 61 s feeds fixes 200 m east of the truth: + +```rust,ignore +{{#include ../../../core/examples/aiding.rs:gate}} +``` + +```rust,ignore +{{#include ../../../core/examples/aiding.rs:outcome}} +``` + +### Configuring the gate in `strapdown-sim` + +| `cl` flag | config key in `[closed_loop]` | meaning | +| --- | --- | --- | +| `--gate-confidence

` | `innovation_gate = { chi_squared = { confidence = P } }` | install a $\chi^2$ gate | +| `--gate-inflation ` | `gate_recovery = { rejection_inflation = F }` | inflation per rejection, default 2 | +| `--gate-force-after ` | `gate_recovery = { forced_update_after = N }` | forced-update streak, default 5 | + +`innovation_gate = { fixed = { threshold = T } }` selects the fixed variant in a file. Gating +rejects individual measurements; a filter that has genuinely diverged produces a large NIS on +every fix, and the run's health monitor (`--nis-pos-max`, `--nis-pos-consec-fail`) is what stops +it rather than letting it degrade silently to dead reckoning. + +## Output of `aiding.rs` + +```text +parked for 30 s, 286 ZUPT/ZARU pairs + gyro bias estimate z: 0.00200 rad/s (injected 0.00200) + speed: 0.0000 m/s, altitude 12.00 m +magnetometer: declination -11.85 deg, true heading 30.00 deg, measured 30.00 deg +chi-squared 0.999 threshold, 1 dof: 10.83 +chi-squared 0.999 threshold, 3 dof: 16.27 +chi-squared 0.999 threshold, 5 dof: 20.52 +t = 59 s NIS 0.0 accepted true forced false +t = 60 s NIS 0.0 accepted true forced false +t = 61 s NIS 2805.9 accepted false forced false +t = 62 s NIS 1564.2 accepted false forced false +t = 63 s NIS 674.9 accepted false forced false +t = 64 s NIS 256.0 accepted false forced false +t = 65 s NIS 93.6 accepted true forced true +t = 66 s NIS 43.5 accepted false forced false +t = 67 s NIS 107.0 accepted false forced false +t = 68 s NIS 114.9 accepted false forced false +t = 69 s NIS 88.9 accepted false forced false +t = 70 s NIS 58.8 accepted true forced true +``` + +The ZARU updates recover the injected gyro bias; the magnetometer model returns the heading the +field was built for; and the gate rejects the spoofed fixes while each rejection inflates the +covariance, so the NIS falls, until the fifth consecutive failure is forced through. Against a +constant 200 m offset the forced updates pull the solution toward the spoofed track a step at a +time: a gate bounds how long the filter disagrees with its sensor, it does not detect a +spoofer. diff --git a/book/src/filters/particle-filter.md b/book/src/filters/particle-filter.md index c2a27a10..5fd6bd0b 100644 --- a/book/src/filters/particle-filter.md +++ b/book/src/filters/particle-filter.md @@ -1,531 +1,65 @@ -# Particle Filters - -## Overview - -This document outlines the design for a particle filter-based Inertial Navigation System (INS) following the architecture established in the UKF implementation (`kalman.rs`). The design addresses vertical channel instability through two alternative approaches: third-order damping and 2.5D navigation. - -## Executive Summary: Key Architectural Decisions - -### 1. Custom Forward Propagation Function Required - -**Critical insight**: For 2.5D navigation, we **cannot reuse** the standard `forward()` function from `lib.rs`. A new `forward_2_5d()` function must be implemented in `particle.rs`. - -**Why?** -- Standard strapdown mechanization (Groves Eq 5.54) fully integrates vertical acceleration into vertical velocity -- This creates deterministic coupling: `v_v(t+dt) = v_v(t) + ∫[a_z + g - Coriolis] dt` -- In 2.5D navigation, we want vertical velocity to be a **random walk** constrained by measurements, not deterministically coupled to accelerometer readings - -**Solution**: `forward_2_5d()` implements: -- **Full strapdown** for: attitude, horizontal velocities (v_n, v_e), horizontal position (lat, lon) -- **Simplified** for: vertical velocity (random walk), altitude (integration of v_v only) - -### 2. Vertical Channel Treatment - -Two approaches are provided: - -| Approach | States | Vertical Velocity | Complexity | Recommended | -|----------|--------|-------------------|------------|-------------| -| **2.5D Simplified** | 15 | Random walk with large process noise | Low | **Yes** - Start here | -| Third-Order Damping | 17 | Damped feedback control | High | Optional - if 2.5D insufficient | - -### 3. Implementation Priority - -1. First: Implement `forward_2_5d()` - this is the foundation -2. Second: Implement particle filter with 2.5D mode -3. Later: Optionally add third-order damping if needed - -## State Representation - -### Standard 15-State Model - -Following the UKF implementation, the baseline particle filter uses a 15-state model: - -**Navigation states (9):** -- Position: `[latitude, longitude, altitude]` (rad, rad, m) -- Velocity: `[v_north, v_east, v_vertical]` (m/s) -- Attitude: `[roll, pitch, yaw]` (rad) - represented internally as DCM - -**Bias states (6):** -- Accelerometer biases: `[b_ax, b_ay, b_az]` (m/s²) -- Gyroscope biases: `[b_gx, b_gy, b_gz]` (rad/s) - -### Extended State for Vertical Channel Damping - -For third-order vertical channel damping, add **optional states**: -- Altitude error estimate: `δh` (m) -- Altitude error rate: `δh_dot` (m/s) - -Total state dimension: **15 + 2 = 17 states** (when using damping) - -**Implementation note**: Use `Particle.other_states: Option>` for the damping states. - -## Vertical Channel Approaches - -### Approach A: Third-Order Vertical Channel Damping - -**Motivation**: The vertical channel in INS is inherently unstable due to coupling between altitude and vertical velocity errors. Third-order damping provides feedback to stabilize this channel. - -**State augmentation:** -``` -x = [lat, lon, h, v_n, v_e, v_v, φ, θ, ψ, b_a, b_g, δh, δh_dot] - └─────────────────────────────────────┘ └─────┘ └────────┘ - 15 standard states biases damping -``` - -**Dynamics model:** -- Altitude error dynamics: `δh_dot = δv_v` -- Altitude error rate dynamics: `δh_ddot = -k₁·δh - k₂·δh_dot + w_h` - - `k₁`, `k₂`: Damping coefficients (tunable) - - `w_h`: Process noise on altitude error - -**Feedback mechanism:** -During propagation (`predict` step): -1. Propagate particles through standard strapdown equations -2. Update altitude using damping feedback: - ``` - h_corrected = h_propagated - k_fb · δh - v_v_corrected = v_v_propagated - k_fb · δh_dot - ``` -3. During measurement update with GPS altitude: - - Innovation: `z - h_expected` updates both `h` and `δh` - - Damping states receive weighted update based on particle importance - -**Pros:** -- Theoretically rigorous approach based on observability analysis -- Provides smooth vertical channel behavior -- Well-suited for long GNSS outages - -**Cons:** -- Additional states increase computational cost -- Requires careful tuning of damping coefficients -- Increased complexity in implementation - -### Approach B: 2.5D Navigation (Simplified Vertical Treatment) - -**Motivation**: Treat vertical acceleration as primarily noise-driven rather than deterministic, simplifying the vertical channel dynamics. This is the recommended approach based on past success. - -**State representation:** -``` -x = [lat, lon, h, v_n, v_e, v_v, φ, θ, ψ, b_a, b_g] - └────────────────────────────────────────────┘ - Standard 15 states only -``` - -**Modified dynamics:** -- **Horizontal navigation**: Full 6-DOF strapdown mechanization for position, velocity, and attitude -- **Vertical channel**: Simplified treatment: - - Altitude propagation: `h(t+dt) = h(t) + v_v(t)·dt` - - Vertical velocity: Integrate vertical acceleration with **high process noise** - - Vertical acceleration noise: Model as white noise with large variance - ``` - Q_vertical = [Q_h, Q_v_v] where Q_v_v >> Q_v_n, Q_v_e - ``` - -**Process noise tuning:** -- Position (lat/lon): `1e-6` (tight) -- Altitude: `1e-3` to `1e-2` (loose, allows drift) -- Horizontal velocity: `1e-3` -- Vertical velocity: `1e-2` to `1e-1` (very loose, absorbs vertical uncertainty) -- Biases: `1e-6` to `1e-8` (standard) - -**Measurement handling:** -- GPS altitude measurements directly constrain `h` with measurement noise -- Barometric altitude (if available) provides additional vertical constraint -- Vertical velocity measurements (if available) constrain `v_v` -- During GNSS outages: Altitude drifts according to high process noise - -**Pros:** -- Simpler implementation (no additional states) -- Computationally efficient -- Leverages particle filter's ability to handle non-Gaussian distributions -- Works well with intermittent GPS -- Avoids complex tuning of damping coefficients - -**Cons:** -- Less theoretically rigorous -- Vertical channel accumulates uncertainty faster during outages -- Requires careful process noise tuning to balance stability and responsiveness - -## Particle Filter Architecture - -### Core Components - -#### Particle struct (already defined in `particle.rs`) -```rust -pub struct Particle { - pub nav_state: StrapdownState, // 9 nav states - pub accel_bias: DVector, // 3 accel biases - pub gyro_bias: DVector, // 3 gyro biases - pub other_states: Option>, // Optional: damping states - pub state_size: usize, // Total dimension - pub weight: f64, // Importance weight -} -``` - -#### ParticleFilter struct -```rust -pub struct ParticleFilter { - particles: Vec, - num_particles: usize, - process_noise: ProcessNoise, - resampling_strategy: ResamplingStrategy, - averaging_strategy: AveragingStrategy, - effective_particle_threshold: f64, // e.g., 0.5 * num_particles - rng: StdRng, // Seeded RNG for reproducibility - is_enu: bool, - - // Vertical channel specific - vertical_channel_mode: VerticalChannelMode, // ThirdOrder or Simplified - damping_coefficients: Option<(f64, f64)>, // (k1, k2) if using damping -} -``` - -#### ProcessNoise struct -```rust -pub struct ProcessNoise { - pub position_std: Vector3, // [σ_lat, σ_lon, σ_h] - pub velocity_std: Vector3, // [σ_vn, σ_ve, σ_vv] - pub attitude_std: Vector3, // [σ_φ, σ_θ, σ_ψ] - pub accel_bias_std: Vector3, // [σ_ba_x, σ_ba_y, σ_ba_z] - pub gyro_bias_std: Vector3, // [σ_bg_x, σ_bg_y, σ_bg_z] - pub damping_states_std: Option>, // [σ_δh, σ_δh_dot] -} -``` - -#### Enums -```rust -pub enum VerticalChannelMode { - ThirdOrderDamping { k1: f64, k2: f64 }, - Simplified, // 2.5D navigation -} - -pub enum ResamplingStrategy { - Systematic, - Stratified, - Residual, - Multinomial, -} - -pub enum AveragingStrategy { - WeightedMean, // Standard weighted average - MaximumWeight, // Use highest weight particle - MeanWithTrimming, // Remove outliers then average -} -``` - -### Core Algorithm - -#### Initialization -```rust -impl ParticleFilter { - pub fn new( - initial_state: InitialState, - imu_biases: Vec, - covariance_diagonal: Vec, - process_noise: ProcessNoise, - num_particles: usize, - vertical_mode: VerticalChannelMode, - seed: Option, - ) -> Self; -} -``` - -**Initialization steps:** -1. Create `num_particles` particles from initial state -2. Sample each particle's state from `N(μ₀, P₀)` where: - - `μ₀` = initial mean state - - `P₀` = diag(covariance_diagonal) -3. Initialize all weights to `1.0 / num_particles` -4. Seed RNG for reproducibility - -#### Predict Step - -**Key architectural decision**: For 2.5D navigation, we **cannot** use the standard `forward()` function from `lib.rs`. The standard strapdown mechanization fully integrates vertical acceleration to update vertical velocity (Equation 5.54 from Groves), which creates deterministic coupling we want to avoid in 2.5D. - -**Why a new forward function is needed:** - -The standard `forward()` computes vertical velocity as: -```rust -// velocity_update() in lib.rs, line 532 -velocity + (specific_force + gravity - r * (transport_rate + 2.0 * rotation_rate) * velocity) * dt -``` - -This tightly couples vertical acceleration to vertical velocity. In 2.5D navigation, we want: -- **Horizontal dynamics**: Full strapdown (deterministic acceleration → velocity → position) -- **Vertical dynamics**: Decoupled or weakly coupled (vertical velocity as random walk, altitude from velocity integration only) - -**Solution**: Implement `forward_2_5d()` that modifies vertical channel propagation. - -#### Update Step -```rust -impl NavigationFilter for ParticleFilter { - fn update(&mut self, measurement: &M) { - // 1. Compute weights based on measurement likelihood - for particle in &mut self.particles { - let predicted_meas = measurement.get_expected_measurement( - &particle.to_state_vector() - ); - let innovation = measurement.get_vector() - predicted_meas; - let meas_cov = measurement.get_noise(); - - // Likelihood: p(z|x) ∝ exp(-0.5 * innovation^T * R^-1 * innovation) - let likelihood = self.compute_likelihood(&innovation, &meas_cov); - particle.weight *= likelihood; - } - - // 2. Normalize weights - self.normalize_weights(); - - // 3. Check effective particle count - let n_eff = self.effective_particle_count(); - - // 4. Resample if needed - if n_eff < self.effective_particle_threshold { - self.resample(); - } - } -} -``` - -#### Resampling - -Systematic resampling is the recommended strategy: - -```rust -fn systematic_resample(&mut self) { - let n = self.num_particles; - let mut new_particles = Vec::with_capacity(n); - - // Cumulative sum of weights - let cumsum: Vec = self.particles.iter() - .scan(0.0, |sum, p| { *sum += p.weight; Some(*sum) }) - .collect(); - - // Systematic sampling - let step = 1.0 / n as f64; - let start = self.rng.gen_range(0.0..step); - - for i in 0..n { - let u = start + i as f64 * step; - let idx = cumsum.iter().position(|&cs| cs >= u).unwrap_or(n - 1); - let mut new_particle = self.particles[idx].clone(); - new_particle.weight = 1.0 / n as f64; - new_particles.push(new_particle); - } - - self.particles = new_particles; -} -``` - -#### State Estimation - -```rust -fn weighted_mean_estimate(&self) -> DVector { - let mut mean_state = DVector::zeros(self.particles[0].state_size); - - for particle in &self.particles { - let state_vec = particle.to_state_vector(); - mean_state += particle.weight * state_vec; - } - - // Special handling for circular quantities (angles) - self.wrap_angles(&mut mean_state); - - mean_state -} -``` - -## The forward_2_5d() Function - -This function implements modified strapdown mechanization for 2.5D navigation. It should be added to `particle.rs` as a module-level function. - -### Mathematical Formulation - -**Full strapdown components (unchanged from Groves Ch 5.4-5.5):** -1. **Attitude update** (Equation 5.46): - ``` - C(t+dt) = C(t) * [I + Ω_ib * dt] - [Ω_ie + Ω_el] * C(t) * dt - ``` - - Full implementation using gyros, transport rate, Earth rate - -2. **Horizontal velocity update** (Equation 5.54, horizontal components only): - ``` - v_n(t+dt) = v_n(t) + [f_n + g_n - Coriolis_n - transport_n] * dt - v_e(t+dt) = v_e(t) + [f_e + g_e - Coriolis_e - transport_e] * dt - ``` - - Full mechanization for northward and eastward velocities - -3. **Horizontal position update** (Equation 5.56, lat/lon only): - ``` - lat(t+dt) = lat(t) + [v_n / (R_N + h)] * dt - lon(t+dt) = lon(t) + [v_e / ((R_E + h) * cos(lat))] * dt - ``` - -**Simplified vertical components (2.5D modification):** - -4. **Vertical velocity**: Random walk model (NOT integrated from acceleration) - ``` - v_v(t+dt) = v_v(t) + w_v * sqrt(dt) - ``` - - `w_v ~ N(0, σ_vv²)` - large process noise - - **No deterministic integration of vertical specific force** - - Vertical velocity drifts according to process noise only - -5. **Altitude update**: Simple integration of vertical velocity - ``` - h(t+dt) = h(t) + v_v(t) * dt - ``` - - Uses trapezoidal rule: `h(t+dt) = h(t) + 0.5 * [v_v(t) + v_v(t+dt)] * dt` - -### Implementation Details - -```rust -/// Modified forward propagation for 2.5D navigation. -/// -/// Implements full strapdown mechanization for horizontal navigation (position, -/// velocity, attitude) while treating the vertical channel with simplified dynamics. -/// Vertical acceleration is NOT integrated into vertical velocity; instead, vertical -/// velocity follows a random walk model constrained by measurements and process noise. -/// -/// # Arguments -/// * `state` - Mutable reference to the navigation state -/// * `imu_data` - Bias-corrected IMU measurements -/// * `dt` - Time step in seconds -/// -/// # Mathematical Basis -/// - Horizontal: Full Groves Equations 5.46, 5.54, 5.56 -/// - Vertical: Simplified dynamics, vertical velocity as random walk -pub fn forward_2_5d(state: &mut StrapdownState, imu_data: IMUData, dt: f64) { - // See full implementation in source code -} -``` - -### Key Differences from Standard forward() - -| Component | Standard `forward()` | `forward_2_5d()` | -|-----------|---------------------|------------------| -| Attitude | Full mechanization (Eq 5.46) | **Same** - Full mechanization | -| Specific force transform | Full (Eq 5.47) | **Same** - Full transform | -| Horizontal velocity | Full mechanization (Eq 5.54) | **Same** - Full mechanization | -| **Vertical velocity** | **Integrated from vertical accel** | **NOT integrated - random walk** | -| Horizontal position | Full mechanization (Eq 5.56) | **Same** - Full mechanization | -| Altitude | Integrated from vertical velocity | **Same** - Simple integration | - -**Critical insight**: The ONLY difference is that `v_v(t+dt) ≈ v_v(t)` in the propagation step. Changes to vertical velocity come entirely from: -1. Process noise injection (large `σ_vv`) -2. Measurement updates (GPS altitude/velocity) - -This makes vertical velocity behave like a "free parameter" that is constrained by measurements rather than deterministic dynamics. - -## Integration with Existing Codebase - -### Mirroring UKF Structure - -The particle filter implements the same `NavigationFilter` trait as UKF: -```rust -pub trait NavigationFilter { - fn predict(&mut self, imu_data: IMUData, dt: f64); - fn update(&mut self, measurement: &M); - fn get_estimate(&self) -> DVector; - fn get_certainty(&self) -> DMatrix; // Empirical covariance from particles -} -``` - -### Simulation Integration - -In `sim.rs`, add particle filter option to `closed_loop` function: -```rust -pub enum FilterType { - UKF { alpha: f64, beta: f64, kappa: f64 }, - ParticleFilter { - num_particles: usize, - vertical_mode: VerticalChannelMode, - resampling: ResamplingStrategy, - }, -} -``` - -## Testing Strategy - -### Unit Tests -- Test particle initialization from `InitialState` -- Test process noise injection -- Test resampling algorithms (verify particle diversity) -- Test weight computation and normalization -- Test vertical channel damping (if used) -- Test state estimation with different averaging strategies - -### Integration Tests -- Compare PF vs UKF on same trajectory -- Test with GNSS dropout scenarios -- Test vertical channel stability (hover, climb, descent scenarios) -- Verify deterministic results with seeded RNG - -### Vertical Channel Tests -Create tests similar to UKF tests in `kalman.rs`: -- `pf_free_fall_motion()` - vertical dynamics with no support -- `pf_hover_motion()` - stationary altitude -- `pf_climb_descent()` - vertical motion profile -- `pf_altitude_drift_during_outage()` - verify controlled drift with 2.5D - -## Recommended Implementation Order - -1. ✅ Define `Particle` struct (already done) -2. **Implement `forward_2_5d()` function in `particle.rs`** - - Copy `attitude_update()` from `lib.rs` or make it public - - Implement `velocity_update_horizontal()` helper - - Reuse `position_update()` from `lib.rs` (make public if needed) -3. Implement `ProcessNoise` struct -4. Implement `ParticleFilter` struct with basic initialization -5. Implement `predict()` using `forward_2_5d()` -6. Implement `update()` with systematic resampling -7. Implement `get_estimate()` with weighted mean -8. Write unit tests for each component -9. Integration with `sim.rs` -10. Test on real datasets -11. (Optional) Implement third-order damping if 2.5D is insufficient - -## Tuning Guidance - -### Number of Particles -- Start with **500-1000** particles for smartphone IMU -- Increase to **2000-5000** for high-precision applications -- Monitor computational cost vs. accuracy tradeoff - -### Process Noise (2.5D) -**Conservative (high uncertainty, smooth vertical):** -- `σ_h = 0.1 m`, `σ_vv = 0.2 m/s` - -**Aggressive (low uncertainty, responsive vertical):** -- `σ_h = 0.01 m`, `σ_vv = 0.05 m/s` - -**Recommended starting point:** -- `σ_h = 0.05 m`, `σ_vv = 0.1 m/s` - -### Resampling Threshold -- Effective particle threshold: `0.5 * num_particles` -- Resample more frequently if seeing particle degeneracy -- Monitor `n_eff` over time to diagnose issues - -## Expected Performance - -### Advantages over UKF -- Better handling of non-Gaussian distributions -- More robust to outliers in measurements -- Can represent multimodal posteriors (e.g., during ambiguous scenarios) -- 2.5D approach naturally handles vertical channel uncertainty - -### Computational Cost -- PF with N=1000 particles ≈ 10-50× slower than UKF -- Use Rayon for parallel particle propagation if needed -- Profile and optimize hot paths (propagation, resampling) - -### Memory Requirements -- Each particle: ~200 bytes (15 states × 8 bytes + overhead) -- 1000 particles ≈ 200 KB (negligible for modern systems) - -## References - -- Groves, P. D. (2013). *Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd Edition*. Chapters 14-15 (INS error models, vertical channel instability). -- Gustafsson, F., et al. (2002). "Particle filters for positioning, navigation, and tracking." *IEEE Transactions on Signal Processing*. -- Existing `kalman.rs` implementation for architectural patterns. +# Particle Building Blocks + +`core/src/particle.rs` is not a particle filter. There is no canonical particle-filter INS, so +the module provides pieces for assembling one: a trait for a particle, strategy enums, and four +resampling algorithms. The one concrete particle filter in the crate is the +[Rao-Blackwellized particle filter](./rbpf.md) in `core/src/rbpf.rs`, and it uses only the +resampling half of this module. + +## What the module provides + +| item | kind | purpose | +| --- | --- | --- | +| `Particle` | trait | a weighted state hypothesis: `new(&DVector, weight)`, `state()`, `set_state(..)`, `weight()`, `set_weight(..)`, plus `as_any` downcasting helpers | +| `ParticleFilter` | trait | one method, `resample(&mut self)`, for a filter built from these parts | +| `ParticleResamplingStrategy` | enum | `Multinomial`, `Systematic` (the default), `Stratified`, `Residual` | +| `ParticleAveragingStrategy` | enum | `Mean` (ignores weights), `WeightedMean` (the default), `HighestWeight` (the maximum-a-posteriori particle) | +| `multinomial_resample`, `systematic_resample`, `stratified_resample`, `residual_resample` | functions | draw ancestor indices from a weight vector | + +Each resampling function takes `(weights: &[f64], num_samples: usize, rng: &mut R)` for any +`R: rand::Rng` and returns a `Vec` of indices into the original particles; a particle +whose index appears $k$ times is copied $k$ times. The weights must already be normalized to sum +to one, and that is the caller's job. + +| algorithm | how it draws | variance | +| --- | --- | --- | +| multinomial | $N$ independent draws from the weight distribution | highest | +| systematic | one random offset, then $N$ evenly spaced points through the cumulative weights | lower | +| stratified | one random point in each of $N$ equal strata of $[0, 1)$ | lower than multinomial | +| residual | $\lfloor N w_i \rfloor$ copies of each particle deterministically, the remainder systematically from the fractional parts | lower than multinomial | + +The algorithms are deterministic given the RNG state, so a seeded `StdRng` makes resampling +reproducible. + +## How the RBPF uses them + +The RBPF does not implement `Particle` or `ParticleFilter`, and it does not use +`ParticleAveragingStrategy`. Its particles (`rbpf::RbpfParticle`) carry a two-element horizontal +position error, the conditional mean of the linear states and a weight, which is a different +shape from a `Particle`'s single state vector; its estimate is a weighted mean with the linear +states' covariance added by the law of total covariance (see [RBPF](./rbpf.md)). + +What it does use is the resampling: + +- `RbpfConfig::resampling_strategy` is a `ParticleResamplingStrategy`, defaulting to + `Systematic`. +- After each measurement update the filter computes the effective sample size, + $N_\text{eff} = 1 / \sum_i w_i^2$ (`RaoBlackwellizedParticleFilter::effective_sample_size`), + and resamples when it falls below `effective_sample_threshold` times the particle count. + The default threshold, 1.0, resamples after every update, as Canciani and Raquet do. +- The selected strategy's function draws the indices; the survivors are copied with uniform + weights and then roughened by `roughening_factor` (see [RBPF](./rbpf.md#configuration)). +- The draws come from the filter's own `StdRng`, seeded by `RbpfConfig::seed`, so a run is + reproducible. + +`resampling_strategy` is a field of the library's `RbpfConfig` only. The simulator's +`[particle_filter]` section and the `pf` subcommand do not expose it, so `strapdown-sim pf` +always resamples systematically. + +## Building your own + +A filter assembled from these parts implements `Particle` for its state type, keeps its own +weights normalized, calls one of the resampling functions inside its `ParticleFilter::resample`, +and implements [`NavigationFilter`](./kalman.md#the-navigationfilter-trait) so it can be driven +by `sim::run_closed_loop` or [`InsEngine`](../user-guide/library.md). The RBPF is the worked +example of the last two steps; `core/src/rbpf.rs` documents the rest. diff --git a/book/src/filters/rbpf.md b/book/src/filters/rbpf.md index 132c163a..3a1ab6f9 100644 --- a/book/src/filters/rbpf.md +++ b/book/src/filters/rbpf.md @@ -124,6 +124,19 @@ The paper's setting never tests this: a navigation-grade INS, map fixes every ep 6. **Process noise across an epoch.** The accumulated process noise carries each state's own decay across the epoch, so a Gauss-Markov state stays stationary over long gaps between sparse fixes. -7. **Configurable horizontal process noise.** The default is the paper's zero. +7. **Configurable horizontal process noise.** The library's `RbpfConfig` defaults to the paper's + zero; `strapdown-sim` and its `[particle_filter]` section default to `[1, 1]` m/√s, for the + reason [above](#horizontal-process-noise-why-the-default-is-1-ms). The implementation, with equation references throughout, is `core/src/rbpf.rs`. + +## See also + +- [Tutorial: Particle Filter](../examples/tutorial-particle-filter.md) runs `strapdown-sim pf` + end to end on a synthetic trajectory and scores it against truth. +- [Particle Building Blocks](./particle-filter.md): the resampling algorithms this filter uses. + It resamples systematically; `RbpfConfig::resampling_strategy` selects another algorithm from + the library, and the simulator does not expose it. +- [Measurement Models and Integrity](./measurements.md): the models it is updated with, and the + innovation gate it honours like the Kalman filters. +- [Particle Filter](../user-guide/particle-filter.md) in the user guide: the `pf` subcommand. diff --git a/book/src/filters/ukf.md b/book/src/filters/ukf.md index 0092ade7..e5952c90 100644 --- a/book/src/filters/ukf.md +++ b/book/src/filters/ukf.md @@ -1 +1,150 @@ # Unscented Kalman Filter (UKF) + +`kalman::UnscentedKalmanFilter` propagates a deterministic set of **sigma points** through the +nonlinear mechanization and the measurement models instead of linearizing them (Julier and +Uhlmann). It needs no transition Jacobian, and its update uses only each model's +`get_expected_measurement`. Run it with `strapdown-sim cl --filter ukf`. + +## State + +The full state vector of the [shared layout](./kalman.md#state-layout-and-units): nine +navigation states, the six IMU biases, then any extra states (`UkfConfig::other_states`, such as +geophysical map biases) and, last, the barometric bias when `estimate_baro_bias` is set. +`initialize_ukf` always builds the fifteen-state core; the constructor accepts an empty bias +slice for a nine-state filter, in which case no bias correction is applied. + +## Sigma points and weights + +For an $n$-element state the scaled unscented transform uses $2n+1$ points and + +$$ +\lambda = \alpha^2 (n + \kappa) - n , +$$ + +$$ +w^{(m)}_0 = \frac{\lambda}{n+\lambda}, \qquad +w^{(c)}_0 = \frac{\lambda}{n+\lambda} + (1 - \alpha^2 + \beta), \qquad +w^{(m)}_i = w^{(c)}_i = \frac{1}{2(n+\lambda)} . +$$ + +Point 0 is the mean; points $i$ and $i+n$ are the mean plus and minus column $i$ of a matrix +square root of $(n+\lambda)P$ (`linalg::matrix_square_root`: an equilibrated Cholesky, with +plain, jittered and eigenvalue fallbacks for a matrix that is not positive definite). + +The attitude rows are not treated as a vector. Adding a rotation vector to an Euler triple is +not the same rotation as composing it, so for attitude each point is +$R_\text{mean}\operatorname{Exp}(\pm\text{column})$, re-expressed on the mean's branch of the +Euler angles. The same idea runs through the rest of the filter (#371): + +- **Predict.** Each sigma point is bias-corrected with *its own* bias hypothesis + ($\Delta v - b_a\Delta t$, $\Delta\theta - b_g\Delta t$) and mechanized. The mean of every + non-attitude row is the weighted sum. The mean attitude is formed in the tangent space at + propagated sigma point 0: the residuals $\operatorname{Log}(R_0^\top R_i)$ are summed with the + mean weights and mapped back. The covariance uses tangent-space attitude residuals, and + $q\ \Delta t$ is added to it. +- **Update.** Every sigma point is passed through `get_expected_measurement`, giving + $\hat z$, $S$ and the state-measurement cross covariance (again with tangent-space attitude + residuals). After the gate, $K = P_{xz} S^{-1}$; the attitude part of $K\nu$ is a rotation + vector and is composed onto the mean attitude, the rest is added. The covariance becomes + $P - K S K^\top$ and is then transported onto the new attitude's tangent space with the same + right Jacobian the [ESKF](./eskf.md#update-injection-and-reset) uses (#398). + +## Defaults: $\alpha = 0.1$, $\beta = 2$, $\kappa = 0$ + +| parameter | default | role | +| --- | --- | --- | +| `alpha` | `0.1` (`sim::DEFAULT_UKF_ALPHA`) | spread of the points about the mean | +| `beta` | `2.0` | prior knowledge of the distribution; 2 is optimal for a Gaussian | +| `kappa` | `0.0` | secondary spread | + +They are set by `UkfConfig::{ukf_alpha, ukf_beta, ukf_kappa}` (each an `Option`, `None` taking +the default), by `[closed_loop]`'s `ukf_alpha`, `ukf_beta` and `ukf_kappa` in a configuration +file, or by `--ukf-alpha`, `--ukf-beta` and `--ukf-kappa` on `strapdown-sim cl`. All three routes +default to the same values. + +### Why not the textbook alpha of 0.001 + +With $\kappa = 0$ the weights are $w_0 = 1 - \alpha^{-2}$ and $w_i = 1/(2n\alpha^2)$. They sum to +exactly 1 for any $\alpha$, but they cancel only in exact arithmetic. At $\alpha = 10^{-3}$ and +$n = 16$ the mean is formed as $-999{,}999\ x_0 + \sum 31{,}250\ x_i$: every term is about +$10^6$ times the answer, so six of `f64`'s sixteen significant digits go to cancellation on +every propagation step. At $\alpha = 0.1$, $w_0 = -99$ and the cancellation costs two digits. + +That was the cause of #399, where the accuracy baseline moved under source changes that should +have been invisible. The investigation recorded in `ClosedLoopConfig::ukf_alpha`'s +documentation perturbed a single WGS84 constant by one unit in the last place and took the +worst response over the gated metrics: + +| configuration | worst response to one ulp | +| --- | ---: | +| `alpha = 1e-3`, plain Cholesky (before #399) | 9.77% | +| `alpha = 1e-3`, equilibrated Cholesky | 3.57% | +| `alpha = 0.1`, plain Cholesky | 0.000297% | +| `alpha = 0.1`, equilibrated Cholesky (shipped) | 0.000140% | + +`alpha` accounts for almost all of it; the equilibrated Cholesky that landed in the same change +for a further factor of two. Over the same perturbations the EKF, ESKF and RBPF, none of which +forms a weighted sigma-point mean, moved by less than $10^{-6}$%. The value is 0.1 rather than +1.0 because $\alpha$ still has a job: it keeps the points near the mean (0.4 sigma at 0.1), so +the transform samples the local nonlinearity rather than a shell several sigma out. + +### The conditioning tests + +`core/tests/ukf_conditioning.rs` holds both ends of that: + +- `one_ulp_of_input_cannot_move_the_solution_by_a_centimetre` runs the UKF twice through a + 180 s, 50 Hz synthetic scenario with GNSS duty-cycled 60 s on and 60 s off, the second time with the initial + latitude moved by one ulp, and requires the final positions to agree within 1 cm. The test's + own documentation records 1.76 m at the old $\alpha$ and 0.18 mm at 0.1. +- `the_shipped_sigma_point_weights_do_not_cancel_away_the_mantissa` checks that the default + weights spend no more than 2.5 of `f64`'s digits on cancellation, so lowering $\alpha$ again + fails a test that names its own cause. + +```bash +cargo test -p strapdown-core --test ukf_conditioning +``` + +## Construction + +```rust,ignore +{{#include ../../../core/examples/kalman_filters.rs:ukf_new}} +``` + +The arguments are an `InitialState`, the six initial bias estimates, optional extra states, the +covariance diagonal (one entry per state, in the units of the +[shared layout](./kalman.md#state-layout-and-units)), the process-noise density $q$ as a +matrix, and $\alpha$, $\beta$, $\kappa$. `sim::initialize_ukf(&record, UkfConfig)` builds the +same filter from a `TestDataRecord`; [Kalman Filters](./kalman.md#the-siminitialize_-helpers) +lists the config fields and the $P_0$ it derives. + +The same run as a configuration file (verified with `strapdown-sim --config` on a `syn` +trajectory): + +```toml +input = "synthetic.csv" +output = "cl_ukf_config.csv" +mode = "closed-loop" + +[closed_loop] +filter = "ukf" +ukf_alpha = 0.1 +ukf_beta = 2.0 +ukf_kappa = 0.0 +``` + +## Things to know + +- The update calls `get_expected_measurement` on all $2n+1$ points and never `get_jacobian`, + so a model's Jacobian does not affect this filter. `MeasurementModel` still requires one, + because the other filters use it. +- Each predict and each update factorizes the covariance once and evaluates the mechanization + or the measurement model at all $2n+1$ points. No timing comparison is published for this + crate. +- The reported Euler angles are on the principal branch after both predict and update. + +## References + +- Julier, S. J. and Uhlmann, J. K., "Unscented Filtering and Nonlinear Estimation", + *Proceedings of the IEEE* 92(3), 2004. +- `UnscentedKalmanFilter` in `core/src/kalman.rs`, and `ClosedLoopConfig::ukf_alpha` in + `core/src/sim.rs` for the full #399 record. diff --git a/book/src/geonav/maps.md b/book/src/geonav/maps.md new file mode 100644 index 00000000..a1cd3f5f --- /dev/null +++ b/book/src/geonav/maps.md @@ -0,0 +1,219 @@ +# Maps and Measurement Models + +> **Experimental.** This page documents `strapdown-geonav` 0.1 (imported as `geonav`). See +> [Geophysical Navigation Overview](./overview.md) for how to run it. + +A geophysical measurement compares two numbers: an **anomaly computed from a sensor reading**, +and the **anomaly read off a map** at the filter's estimated position. Everything below is +about how each of those is formed, and how the map's own error is absorbed by a bias state. + +## Maps: `GeoMap` + +`GeoMap` holds one regular latitude/longitude grid: a vector of latitudes, a vector of +longitudes (both in degrees, ascending), a matrix of values with one row per latitude and one +column per longitude, and a `GeophysicalMeasurementType` saying what the values are. + +`GeoMap::load_geomap(path, map_type)` reads a NetCDF file with three variables: + +| variable | shape | contents | +|---|---|---| +| `lat` | 1-D | latitudes, degrees | +| `lon` | 1-D | longitudes, degrees | +| `z` | `lat` × `lon` | anomaly: milligal for gravity, nanotesla for magnetic | + +This is the layout GMT writes for its gridded datasets. The file does not say what kind of map +it is, so the caller supplies the type. A file that cannot be opened, lacks one of the three +variables, or whose `z` does not have `len(lat) × len(lon)` values is rejected with +`StrapdownError::MapLoad`. + +### Map types and resolutions + +`GeophysicalMeasurementType` is `Relief`, `Gravity` or `Magnetic`, each carrying a resolution +enum. The resolutions are the grid spacings GMT publishes its remote datasets at, and each +formats as GMT's code for it (`01d`, `30m`, ... `01m`): + +| type | resolutions | +|---|---| +| `GravityResolution` | one degree; 30, 20, 15, 10, 6, 5, 4, 3, 2 and 1 arc-minutes | +| `MagneticResolution` | one degree; 30, 20, 15, 10, 6, 5, 4, 3 and 2 arc-minutes | +| `ReliefResolution` | also includes arc-second spacings; no measurement model uses relief | + +The resolution is metadata. Interpolation uses the grid actually stored in the file, so a label +that disagrees with the file changes nothing numerically. + +### Reading a value: bilinear interpolation + +`GeoMap::get_point(lat, lon)` interpolates bilinearly between the four surrounding grid nodes +$(\varphi_1, \lambda_1)$, $(\varphi_2, \lambda_2)$: + +$$ +m(\varphi, \lambda) = \frac{ + q_{11}(\lambda_2-\lambda)(\varphi_2-\varphi) + q_{12}(\lambda_2-\lambda)(\varphi-\varphi_1) + + q_{21}(\lambda-\lambda_1)(\varphi_2-\varphi) + q_{22}(\lambda-\lambda_1)(\varphi-\varphi_1) +}{(\lambda_2-\lambda_1)(\varphi_2-\varphi_1)} +$$ + +with $q_{ij}$ the value at $(\varphi_i, \lambda_j)$, and linear interpolation along an edge. A +non-finite coordinate is `StrapdownError::NonFinite`; a point outside the grid is +`StrapdownError::OutOfMapBounds`. + +What happens when the estimate leaves the grid depends on the filter: + +- **EKF and particle filter:** `OutOfMapBounds` is classed as recoverable, so the measurement is + skipped with a warning naming the map bounds, and the run continues unaided by the map. +- **UKF:** the predicted measurement is evaluated at each sigma point through a method that + cannot return an error, and an off-map point yields `NaN` there. In the current release that + `NaN` reaches the state and the run stops with `NonFinite { what: "filter state" }`, even when + only a sigma point, not the estimate itself, is off the map. + +Either way, size the map with room to spare around the track. + +### The map gradient + +The EKF and the particle filter's Kalman part need $\partial m / \partial \varphi$ and +$\partial m / \partial \lambda$. `GeoMap::get_gradient` takes central differences with a step of +$\epsilon = 10^{-6}$ degrees, falling back to a one-sided difference at the edge of the grid: + +$$ +\frac{\partial m}{\partial \varphi} \approx \frac{m(\varphi+\epsilon, \lambda) - m(\varphi-\epsilon, \lambda)}{2\epsilon}, +\qquad +\frac{\partial m}{\partial \lambda} \approx \frac{m(\varphi, \lambda+\epsilon) - m(\varphi, \lambda-\epsilon)}{2\epsilon}. +$$ + +The result is per degree and is converted to per radian, the unit of the state. Because the +interpolant is piecewise bilinear, this is the slope of the cell the estimate is in. + +## Measurement models + +Both models implement core's `MeasurementModel` trait and are one-dimensional. Each predicts the +map value plus a bias, $h(\mathbf{x}) = m(\varphi, \lambda) + b$, and has +$R = \sigma^2$ from its `noise_std`. The Jacobian row is + +$$ +H = \begin{bmatrix} \dfrac{\partial m}{\partial \varphi} & \dfrac{\partial m}{\partial \lambda} & 0 & \cdots & 0 & 1 & 0 & \cdots \end{bmatrix} +$$ + +with the 1 in the column of that channel's bias state, if the filter carries one. + +What makes them unusual is the *observation*. The sensor does not measure an anomaly; it +measures a total field. Turning that into an anomaly needs the vehicle's position, altitude, +velocity and the date, and those are only known once a filter is running. So the observation +is computed from the state the filter hands the model, at update time, not when the event +stream is built. + +### Gravity: `GravityMeasurement` + +The observed specific-force magnitude $g_\text{obs}$ is the norm of the record's +`grav_x`, `grav_y`, `grav_z` columns (m/s²). The anomaly, in milligal, is + +$$ +\Delta g = \left[\ g_\text{obs} - \gamma(\varphi, h) + E \ \right] \times 10^{5} +$$ + +where: + +- $\gamma(\varphi, h) = \gamma_e \dfrac{1 + k \sin^2\varphi}{\sqrt{1 - e^2 \sin^2\varphi}} - 3.08\times10^{-6}\ h$ + is Somigliana normal gravity on the WGS84 ellipsoid with the free-air gradient + (`earth::gravity`), evaluated at the observation height; +- $E = 2\Omega v_E \cos\varphi + \dfrac{v_N^2}{R_N + h} + \dfrac{v_E^2}{R_E + h}$ is the Eötvös + correction (`earth::eotvos`): the down component of the Coriolis and transport-rate term of + Groves eq. 5.54, with the transport rate of eq. 5.44. A gravimeter on a moving platform reads + low by $E$, so it is added back; +- $10^5$ converts m/s² to milligal (`earth::MGAL_PER_M_PER_S2`). + +Because $h$ is height above the ellipsoid, this is strictly the gravity disturbance rather than +the classical free-air anomaly, which is referred to the geoid. The two differ by about +$0.3086\ N$ mGal for a geoid undulation of $N$ metres: near-constant over a trajectory, and +absorbed by the bias state. This is `earth::gravity_anomaly`. + +### Magnetic: `MagneticAnomalyMeasurement` + +The observed total field $|\mathbf{B}_\text{obs}|$ is the norm of `mag_x`, `mag_y`, `mag_z`, +which are microtesla, scaled by 1000 into nanotesla. The anomaly is + +$$ +\Delta F = |\mathbf{B}\_\text{obs}| - F\_\text{WMM}(\varphi, \lambda, h, t) +$$ + +where $F_\text{WMM}$ is the World Magnetic Model's total intensity at the estimated position and +the record's date, from the `world_magnetic_model` crate, in nanotesla. Altitude is clamped to +the model's valid band of −1 km to 850 km. A position or date the model cannot serve is +`StrapdownError::ExternalModel`, which is recoverable: the measurement is skipped. + +The total-field magnitude is used because it does not depend on the sensor's orientation, so +attitude error does not enter the observation. + +### Both: `CombinedGeophysicalMeasurement` + +When a run loads both maps and a record carries both readings, the two are emitted as a single +two-dimensional measurement, $[\Delta g, \Delta F]$, with a diagonal $R$: the two channels' +noises are taken as independent. For the filters that use the Jacobian, either half falling +off its map fails the whole measurement rather than applying the other half alone. + +## Bias states + +The map is not the truth. Grids are smoothed, referenced to a different datum or epoch, and a +magnetometer in a vehicle also sees the vehicle. Each channel therefore has a bias state $b$ +in the map's unit, and $h(\mathbf{x}) = m + b$. + +**UKF and EKF.** The biases are appended after the 15 navigation and IMU-bias states, gravity +first, then magnetic, then the barometric bias if it is estimated (the default; see +`--no-estimate-baro-bias`). Each is a random walk: initial value `gravity_bias` / +`magnetic_bias`, initial variance `*_bias_init_std`², and process-noise density +`*_bias_process_noise_std`² (a variance per second, so $Q = q\ \Delta t$). The +initial standard deviation defaults to the measurement noise; the random-walk rate defaults to +that prior spread over an hour, $\sigma_0/\sqrt{3600\ \text{s}}$. + +`GeoBiasLayout` records where these states sit and how wide the filter's state is. Each +measurement carries a `BiasState` with both numbers and checks them against the state vector +it is handed, so a measurement built for one filter layout and given to another fails with +`StrapdownError::DimensionMismatch` instead of reading an IMU bias as a map bias. + +**Particle filter.** Following Canciani & Raquet, each channel's bias is split into a +first-order Gauss-Markov temporal variation $V$ and a constant $c$, both linear states of the +filter's shared Kalman part, and the measurement selects $V + c$. See +[Rao-Blackwellized Particle Filter](../filters/rbpf.md). + +## When measurements are emitted + +`geonav::build_event_stream` builds the ordinary event stream with +`strapdown::messages::build_event_stream` (IMU, GNSS, barometer and magnetometer heading, with +their schedules and GNSS faults) and merges the geophysical events into it. Its geophysical +settings come in a `GeophysicalAiding` struct: the maps, the two noise values, the interval +between measurements, and the bias layout. + +- With `geo_interval_s` unset, every record that carries the reading produces a measurement; + with it set, one is produced at most every `geo_interval_s` seconds. +- A record with any `NaN` among `grav_*` (or `mag_*`) produces no measurement on that channel. +- At equal timestamps the geophysical measurement comes after the others. +- Geophysical measurements continue through a GNSS outage; that is the point of them. + +## Where the maps come from + +The Python tooling downloads them. `analyze preprocess --getmaps` (see +[Python Analysis Tooling](../development/analysis.md)) calls PyGMT for each trajectory, over its +bounding box padded by a fraction (`--buffer`, default 0.1) and a fixed margin +(`--margin-km`, default 5 km): + +| file | PyGMT call | grid | +|---|---|---| +| `_gravity.nc` | `load_earth_free_air_anomaly(resolution="01m")` | 1 arc-minute free-air anomaly, mGal | +| `_magnetic.nc` | `load_earth_magnetic_anomaly(resolution="03m", data_source="wdmam")` | 3 arc-minute World Digital Magnetic Anomaly Map, nT | +| `_relief.nc` | `load_earth_relief(resolution="15s")` | 15 arc-second relief, for plotting only | + +These names are what `strapdown-sim` looks for beside each input CSV. Note that the shipped +recipes label the magnetic map `two_minutes` while the file holds a 3 arc-minute grid; as above, +the label has no numerical effect. When preprocessing splits a recording into segments without +`--getmaps`, each segment is given a copy of its parent recording's maps, which necessarily +cover it. Downloading needs the GMT C library and network access; nothing else in the toolchain +does. + +## The Python mirror of these models + +`analysis/src/analysis/geostats.py` (`analyze geostats`) characterises a data set's geophysical +readings against its maps: per-channel bias, measurement noise, signal-to-noise ratio and the +de-correlation length along track. For those statistics to describe the quantity the filter +actually sees, its anomaly functions (`normal_gravity`, `eotvos`, `gravity_anomaly_mgal`, +`observed_field_nt`, `wmm_total_field_nt`) mirror `core/src/earth.rs` and `geonav/src/lib.rs` +term for term, and its `self_check()` asserts that the two implementations agree on known cases +before any statistic is computed. diff --git a/book/src/geonav/overview.md b/book/src/geonav/overview.md index d2e33d5e..ba4037d2 100644 --- a/book/src/geonav/overview.md +++ b/book/src/geonav/overview.md @@ -1 +1,197 @@ # Geophysical Navigation Overview + +> **Experimental.** The `strapdown-geonav` crate is held at version 0.1 so that its API can +> still change, and its results are research results, not a validated product. Everything on +> this page describes what the code does today. + +Geophysical navigation aids an inertial solution with a quantity the Earth itself provides: the +local **gravity anomaly** or the local **magnetic anomaly**. Both vary with position and are +published as global grids. A sensor on the vehicle measures the anomaly, the filter reads the +same quantity off a map at its estimated position, and the difference corrects the position +estimate. Unlike GNSS, nothing has to be received, so there is nothing to jam or spoof. The +price is that a map measurement is a highly nonlinear, often ambiguous function of position, +and that the signal (tens of milligal, tens to hundreds of nanotesla) is small against the +sensor errors of anything cheap. + +The work this module supports is described in two Institute of Navigation papers, one using the +UKF and one the particle filter (see [Publications and Links](../resources/publications.md)): + +- J. Brodovsky and P. Dames, "Navigation in GNSS-denied environments using MEMS-grade sensors + and geophysical anomalies: A UKF approach," *ION ITM 2026*, pp. 155–164, + [doi:10.33012/2026.20508](https://doi.org/10.33012/2026.20508). +- J. Brodovsky and P. Dames, "Navigation in GNSS-Denied Environments Using MEMS-Grade Sensors + and Geophysical Anomalies: A Particle Filter Approach," *ION Pacific PNT 2026*, pp. 399–410, + [doi:10.33012/2026.20618](https://doi.org/10.33012/2026.20618). + +[Maps and Measurement Models](./maps.md) has the measurement equations, the bias states and +where the maps come from. + +## Building + +Geophysical aiding is a feature of `strapdown-sim`, off by default because it compiles libnetcdf +(and with it HDF5) from source, which needs a C compiler and cmake 3.26 or newer: + +```bash +cargo build --release -p strapdown-sim --features geonav +# or, to install the binary +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim --features geonav +``` + +A binary built without the feature has no `--geo` flag at all. (There used to be a separate +`geonav-sim` binary; it was folded into `strapdown-sim`.) + +## Which filters + +| mode | filter | geophysical aiding | +|---|---|---| +| `cl --filter ukf` | UKF | yes | +| `cl --filter ekf` | EKF | yes | +| `cl` (default `--filter eskf`) | ESKF | **no**: the run stops with "ESKF is not yet implemented for geophysical navigation" | +| `pf` | Rao-Blackwellized particle filter | yes | +| `dr` | dead reckoning | no | + +Because the ESKF is the closed-loop default, `cl --geo` must name `--filter ukf` or +`--filter ekf`, and a config file must set `filter` in `[closed_loop]`. + +## Command-line flags + +All of these require `--geo`, and at least one of `--gravity-resolution` or +`--magnetic-resolution` must be given; a run with `--geo` and neither stops with "geophysical +navigation needs at least one map". The same flags are available on `cl` and `pf`. + +| flag | unit | default | meaning | +|---|---|---|---| +| `--geo` | -- | off | enable geophysical aiding | +| `--gravity-resolution` | -- | unset | enable gravity aiding; one of `one-degree` ... `one-second` (see below) | +| `--gravity-map-file` | path | `_gravity.nc` | gravity anomaly map | +| `--gravity-noise-std` | mGal | 100 | gravity measurement noise σ | +| `--gravity-bias` | mGal | 0 | initial value of the gravity map-bias state | +| `--gravity-bias-init-std` | mGal | `--gravity-noise-std` | initial σ of the gravity map-bias state | +| `--gravity-bias-process-noise-std` | mGal/√s | init σ / √3600 | random-walk rate of the gravity map bias | +| `--magnetic-resolution` | -- | unset | enable magnetic aiding; same choices | +| `--magnetic-map-file` | path | `_magnetic.nc` | magnetic anomaly map | +| `--magnetic-noise-std` | nT | 150 | magnetic measurement noise σ | +| `--magnetic-bias` | nT | 0 | initial value of the magnetic map-bias state | +| `--magnetic-bias-init-std` | nT | `--magnetic-noise-std` | initial σ of the magnetic map-bias state | +| `--magnetic-bias-process-noise-std` | nT/√s | init σ / √3600 | random-walk rate of the magnetic map bias | +| `--geo-interval-s` | s | every record | seconds **between** geophysical measurements (alias `--geo-frequency-s`) | + +The particle filter adds four more, for the temporal-variation part of its map-bias model (see +[Rao-Blackwellized Particle Filter](../filters/rbpf.md)): `--gravity-variation-std`, +`--gravity-variation-time-constant-s` (default 300 s), `--magnetic-variation-std` and +`--magnetic-variation-time-constant-s` (default 300 s). The two standard deviations default to +values derived from the corresponding `--*-bias-process-noise-std`. + +The 100 mGal and 150 nT noise defaults are placeholders; nothing has measured them against +these maps. Set the noise and the bias prior for your sensor. `--magnetic-bias-init-std` in +particular wants to be large for a magnetometer inside a vehicle, whose own field the bias +state exists to absorb. + +**The resolution flags are labels, not selectors.** The grid used is whatever the map file +contains; the resolution is recorded with the map but changes nothing numerically. Gravity +labels stop at `one-minute` and magnetic labels at `two-minutes`, and finer choices are +recorded as those. + +## Configuration file + +The same settings, in snake case, go in a `[geophysical]` section. This file ran as written +against a synthetic trajectory with map files beside it: + +```toml +input = "synthetic.csv" +output = "results/geo_cfg.csv" +mode = "closed-loop" + +[closed_loop] +filter = "ekf" + +[geophysical] +gravity_resolution = "one_minute" +gravity_noise_std = 50.0 +gravity_bias_init_std = 100.0 +magnetic_resolution = "two_minutes" +magnetic_noise_std = 20.0 +magnetic_bias_init_std = 500.0 +geo_interval_s = 5.0 + +[aiding.scheduler] +kind = "duty_cycle" +on_s = 100.0 +off_s = 200.0 +start_phase_s = 0.0 +``` + +| key | CLI counterpart | +|---|---| +| `gravity_resolution`, `magnetic_resolution` | `--gravity-resolution`, `--magnetic-resolution` (values in snake case: `one_minute`, `two_minutes`, ...) | +| `gravity_map_file`, `magnetic_map_file` | `--gravity-map-file`, `--magnetic-map-file` | +| `gravity_noise_std`, `magnetic_noise_std` | `--gravity-noise-std`, `--magnetic-noise-std` | +| `gravity_bias`, `magnetic_bias` | `--gravity-bias`, `--magnetic-bias` | +| `gravity_bias_init_std`, `magnetic_bias_init_std` | `--gravity-bias-init-std`, `--magnetic-bias-init-std` | +| `gravity_bias_process_noise_std`, `magnetic_bias_process_noise_std` | `--gravity-bias-process-noise-std`, `--magnetic-bias-process-noise-std` | +| `geo_interval_s` (alias `geo_frequency_s`) | `--geo-interval-s` | + +The particle filter's variation settings go in `[particle_filter]` as +`gravity_variation_std`, `gravity_variation_time_constant_s`, `magnetic_variation_std` and +`magnetic_variation_time_constant_s`. The recipes `conf/{ukf,ekf,rbpf}_{grav,mag,both}.toml` +are complete worked examples, with the source of every number in their comments. + +## How maps are found + +Unless a map file is given explicitly, `strapdown-sim` looks for it **beside the input CSV, +named after it**: for `data/input/drive.csv` it opens `data/input/drive_gravity.nc` and +`data/input/drive_magnetic.nc`. A directory input is processed file by file, so every +trajectory needs its own pair. A missing map stops the run with +`Gravity map file not found: ` (or the magnetic equivalent). + +A map is a NetCDF grid with one-dimensional `lat` and `lon` variables in degrees and a +two-dimensional `z` variable of anomaly values, gravity in milligal and magnetic in nanotesla, +which is the layout GMT writes. `analyze preprocess --getmaps` in the Python tooling downloads +both maps for each trajectory and writes them under exactly these names; see +[Maps and Measurement Models](./maps.md#where-the-maps-come-from). + +## Output + +The output CSV always carries `gravity_bias`, `gravity_bias_cov`, `magnetic_bias` and +`magnetic_bias_cov` columns (see [Output Format](../user-guide/output-format.md)). For each +channel a geophysically aided run uses, they hold the filter's estimate of the map bias, in +the map's unit, and its variance; for a channel the run does not use, they are `NaN`. + +## Example + +The command below ran against `strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42`, +with `synthetic_gravity.nc` and `synthetic_magnetic.nc` beside it. Those two maps were written +by a short Python script as a smooth artificial field covering the track: `syn` does not +produce maps, and its `grav_*` and `mag_*` columns are not sampled from any map, so this shows +the mechanics of a run, not geophysical navigation performance. + +```bash +strapdown-sim cl -i synthetic.csv -o geo_both.csv --filter ekf \ + --geo --gravity-resolution one-minute --magnetic-resolution two-minutes \ + --gravity-noise-std 50 --magnetic-noise-std 20 --magnetic-bias-init-std 500 + +strapdown-sim pf -i synthetic.csv -o geo_pf.csv \ + --geo --gravity-resolution one-minute --geo-interval-s 5 --num-particles 200 +``` + +## Status and limitations + +- **Experimental API.** `strapdown-geonav` is 0.1 and may change in any release. +- **No ESKF.** Only the UKF, the EKF and the particle filter accept geophysical measurements. +- **Leaving the map.** With the EKF and the particle filter, a measurement whose estimate is + off the map tile is skipped with a warning, so a map that is too small quietly stops aiding. + With the UKF, an off-map sigma point currently stops the run with + `NonFinite { what: "filter state" }`. Pad the map well beyond the track either way; see + [Maps and Measurement Models](./maps.md#reading-a-value-bilinear-interpolation). +- **The sensor matters more than the filter.** The gravity observation is the magnitude of the + record's `grav_x/y/z` columns and the magnetic observation the magnitude of `mag_x/y/z`. + Phone recordings from Sensor Logger are poor sources for both: the magnitude of the `grav_*` + channel is pinned to a device constant, so it carries no gravity information, and the + magnetometer is dominated by the vehicle's own field. The project's own experiments therefore + replace those two channels with simulated readings of dedicated low-cost sensors + (`analyze preprocess --synthetic`, documented in `analysis/src/analysis/synthetic.py`). +- **The noise defaults are not measurements.** `analyze geostats` in the + [Python tooling](../development/analysis.md) measures bias, noise and signal-to-noise for a + data set against its maps. +- **Relief maps are not used.** `GeoMap` can represent a terrain-relief grid, and the + preprocessing downloads one for plotting, but no measurement model consumes it. diff --git a/book/src/gnss/fault-simulation.md b/book/src/gnss/fault-simulation.md index 66d0fd8a..e4eb4c5d 100644 --- a/book/src/gnss/fault-simulation.md +++ b/book/src/gnss/fault-simulation.md @@ -1,110 +1,152 @@ # Fault Simulation -This chapter is the experiment-design rationale behind `conf/`: which GNSS conditions are -worth simulating, what the published measurements say those conditions look like, and which -knobs in `GnssFaultModel` correspond to which. +This chapter is the experiment-design rationale behind the recipes in `conf/`: which GNSS +conditions are worth simulating, what the published measurements say those conditions look +like, and which knobs in `GnssFaultModel` correspond to which. Every variant, field, default +and flag is listed in [Schedulers and Faults Reference](./scenarios.md); this page explains why +the recipes use the values they do. + +Two pieces of the library are involved, and they are deliberately separate: + +- a **scheduler** (`MeasurementScheduler`) decides *when* a GNSS fix reaches the filter; +- a **fault model** (`GnssFaultModel`) decides *what* is wrong with each fix that does. + +Both live in the `[aiding]` section of a configuration file. That section was called +`[gnss_degradation]` before the v1.0 freeze, and the old name is still accepted as an alias, +which is why the recipes under `conf/` still spell it that way. ## What jamming actually does The dominant, repeatedly measured effect of jamming is **loss of fix**, not graceful -degradation. At Jammertest — the open field trial run annually at Andøya by the Norwegian -Public Roads Administration, Communications Authority, Defence Research Establishment, -Metrology Service and Space Agency — the jamming signal was in most runs strong enough to -cause complete GNSS signal loss, and a smartphone roughly ten metres from a jammer lost lock -on benign signals entirely. Receivers reacquired when terrain or buildings shadowed them from -the jammer and lost lock again on returning to line of sight. +degradation. At Jammertest, the open field trial run annually at Andøya by Norwegian public +agencies, the jamming signal was in most runs strong enough to cause complete GNSS signal +loss, and a smartphone roughly ten metres from a jammer lost lock on benign signals entirely. +Receivers reacquired when terrain or buildings shadowed them from the jammer and lost lock +again on returning to line of sight. That matters for how a scenario is built. A receiver inside the jammed area does not report a noisier position; it reports nothing. A model that keeps delivering fixes with more error on -them describes the *fringe* of the jammed area, not the inside of it — and it is the inside -that an inertial system, or a geophysical aid, exists to carry you through. +them describes the *fringe* of the jammed area, not the inside of it. The inside is what an +inertial system, or a geophysical aid, exists to carry you through. The scale is not marginal. IATA recorded a 220% rise in GPS signal-loss events between 2021 and 2024; European flights encountering GNSS interference rose from roughly 200 per day in Q1 2024 to around 900 per day in Q2, and EUROCONTROL logged more than 2,500 jamming events in -2024 alone. +2024 alone (see the sources at the end of this page). -## The interference tier +## The interference tiers -| Tier | What it models | Scheduler | Fault | Configs | +| Tier | What it models | Scheduler | Fault | Recipes | |---|---|---|---|---| +| **Denial** | inside a jammed area: no fix at all | `duty_cycle` | `none` | none shipped; use `--sched duty` | | **Degraded** | the fringe of a jammed area | `fixed_interval` | `degraded` | `conf/*_degraded.toml` | -| **Spoof** | a deceptive signal | either | `slow_bias` / `hijack` | — | +| **Spoof** | a deceptive signal | either | `slow_bias` / `hijack` | none shipped; use `--fault slowbias` or `hijack` | -An earlier version of this experiment matrix also carried a **Denial** tier -(`duty_cycle`/`none`, modeling total loss of fix inside a jammed area) as a config separate -from Degraded. It was consolidated into the single Degraded tier below: keeping one -interference profile, used identically everywhere a geo-aided run is scored, was worth more -than covering both regimes at the cost of doubling the config matrix and the pairing logic -that keeps it honest (see "Choosing a baseline" below). +The experiment matrix in `conf/` carries one interference profile, Degraded. Keeping one +profile, used identically everywhere a geophysically aided run is scored, was worth more than +covering both regimes at the cost of doubling the config matrix and the pairing logic that +keeps it honest (see [Choosing a baseline](#choosing-a-baseline-to-compare-against) below). + +The Denial tier is still one command away. This gives 30 s of fixes in every 150 s, with +availability as the only variable: + +```bash +strapdown-sim cl -i input.csv -o denial.csv \ + --sched duty --on-s 30 --off-s 120 --duty-phase-s 30 --fault none +``` + +`--duty-phase-s` is the length of an initial ON window. Without it the cycle starts with its +OFF window, so the filter would begin with a 120 s outage before it had seen a single fix; see +[the duty-cycle timeline](./scenarios.md#dutycycle). ### Degraded -`conf/*_degraded.toml`: fixes every 5 s, with AR(1)-correlated error on position and velocity -and the advertised accuracies inflated. +`conf/*_degraded.toml` keeps **one fix every 60 s** (`fixed_interval`, `interval_s = 60.0`) +from a roughly 1 Hz receiver, and puts AR(1)-correlated error on each one, with the advertised +accuracies inflated: -Anchoring the magnitude to measurement rather than to a round number: smartphone GNSS is 3–5 m -under good multipath and over 10 m in harsh multipath, with semi-urban CEP95 around 10–12 m. -Under jamming the tracked satellite count falls and GDOP rises, so true error and advertised -accuracy inflate together. `sigma_pos_m = 35.0` is about 14× this dataset's 2.58 m median -advertised accuracy — a receiver still producing fixes, but bad ones. +```toml +[gnss_degradation.scheduler] # the same as [aiding.scheduler] +kind = "fixed_interval" +interval_s = 60.0 +phase_s = 0.0 + +[gnss_degradation.fault] # the same as [aiding.fault] +kind = "degraded" +rho_pos = 0.99 # ignored while tau_pos_s is set +sigma_pos_m = 35.0 # steady-state wander, metres +tau_pos_s = 500.0 +rho_vel = 0.95 # ignored while tau_vel_s is set +sigma_vel_mps = 1.5 # steady-state wander, m/s +tau_vel_s = 100.0 +r_scale = 5.0 # advertised sigmas x5, so R x25 +``` + +The magnitude is anchored to measurement rather than to a round number. Smartphone GNSS is +3–5 m under good multipath and over 10 m in harsh multipath, with semi-urban CEP95 around +10–12 m. Under jamming the tracked satellite count falls and GDOP rises, so true error and +advertised accuracy inflate together. 35 m of steady-state wander is roughly an order of +magnitude above the few metres a phone advertises in open conditions: a receiver still +producing fixes, but bad ones. `r_scale` inflates the *advertised 1σ*, which the measurement model then squares, so **R moves by `r_scale²`**: the standard 5.0 is a 25× R, not a 5× one. ### Spoofing -`slow_bias` and `hijack` are implemented and unused in the current experiment matrix. They -differ in detectability by construction: a `hijack` steps the position with no matching -velocity change, so it produces a large NIS the moment it starts and a chi-squared gate -rejects it. A `slow_bias` moves position and velocity consistently, which is what keeps it -under the gate — that is the whole design of a soft spoof. +`slow_bias` and `hijack` are implemented and not used in the current experiment matrix. They +differ in detectability by construction. A `hijack` steps the position with no matching +velocity change, so it produces a large normalized innovation squared (NIS) the moment it +starts, which an innovation gate (`--gate-confidence`, off by default) can reject. A +`slow_bias` moves position and velocity consistently, which is what is meant to keep it under +the gate; that is the whole design of a soft spoof. ## Correlation time and the fix interval -`rho_pos` and `rho_vel` are applied **once per emitted fix**, with no reference to elapsed -time. The error's correlation time is therefore `-interval_s / ln(rho)` — a property of the -*scheduler*, not of the error model. At `rho_pos = 0.99`, a 5 s fix interval is a 498 s -correlation time and a 30 s interval is 2985 s. +Without a time constant, `rho_pos` and `rho_vel` are applied **once per emitted fix**, with no +reference to elapsed time. The error's correlation time is then -So an experiment that sweeps the fix interval is also sweeping the error timescale, and the -two cannot be separated in the result. +$$\tau = -\frac{\Delta t_\text{fix}}{\ln \rho}$$ -`tau_pos_s` and `tau_vel_s` fix this. When set, the coefficient becomes `exp(-dt / tau)` for -the actual interval between fixes, and `sigma_pos_m` is reinterpreted as the **steady-state** -standard deviation in metres rather than the per-step innovation: +which is a property of the *scheduler*, not of the error model. At `rho_pos = 0.99`, a 5 s fix +interval is a 498 s correlation time and a 30 s interval is 2985 s. An experiment that sweeps +the fix interval is therefore also sweeping the error timescale, and the two cannot be +separated in the result. -```toml -[gnss_degradation.fault] -kind = "degraded" -sigma_pos_m = 35.0 # steady-state wander, metres, whatever the schedule -tau_pos_s = 500.0 # correlation time in seconds -``` +`tau_pos_s` and `tau_vel_s` (added in commit `96a47f4`) fix this. When set, the coefficient +becomes $\rho = e^{-\Delta t/\tau}$ for the actual interval $\Delta t$ between fixes, and +`sigma_pos_m` is reinterpreted as the **steady-state** standard deviation rather than the +per-step innovation. This is the first-order Gauss-Markov process, whose autocorrelation is +$R(\Delta t) = \sigma^2 e^{-|\Delta t|/\tau}$. They are optional and additive. Omit them and the per-fix form applies exactly as before, so -every configuration written earlier — and every result recorded from one — still means what it +every configuration written earlier, and every result recorded from one, still means what it meant. Prefer them for anything that varies the schedule. Note also that without a time constant, `sigma_pos_m` is the per-step innovation, not the -error you get: an AR(1) settles at `sigma / sqrt(1 - rho²)`, so `sigma_pos_m = 3.0` with +error you get. An AR(1) settles at $\sigma / \sqrt{1 - \rho^2}$, so `sigma_pos_m = 3.0` with `rho_pos = 0.99` is 21 m of wander, not 3 m. +The same commit fixed the clock the fault advances on. A fault is applied once per *emitted* +fix, and its `dt` is now the time since the previous emitted fix rather than the record +spacing. Before that, a `slow_bias` under a 5 s schedule integrated one second of drift per +five seconds of run time. + ## Choosing a baseline to compare against -`geoperf-all` scores each geophysically-aided run against that filter's unaided run, so the -two must carry an **identical** GNSS profile. If they differ, the "improvement" reported is -the difference between two degradations and has nothing to do with the aid. The -`[gnss_degradation]` and `[health_limits]` sections of `conf/{ukf,ekf,rbpf}_{grav,mag,both}.toml` -are duplicated from `conf/*_degraded.toml` for exactly this reason, and -`core/tests/example_configs.rs` covers `conf/` so a drift in them fails a test. - -`conf/*_degraded.toml` carries this recalibrated 35 m profile, and it is the only -interference tier in the config matrix: `conf/{ukf,ekf,rbpf}_{grav,mag,both}.toml` are all -scored against their own filter's `conf/{ukf,ekf,rbpf}_degraded.toml`, and RBPF's geo-aided -runs are additionally scored against `conf/ekf_degraded.toml` as the canonical INS baseline -(`just geoperf-all`/`geoperf-rbpf`). Keeping a single profile, applied identically everywhere, -is what makes the pairing in `core/tests/example_configs.rs` a simple filename convention -rather than a per-tier lookup. +`just geoperf-all` scores each geophysically aided run against that filter's unaided run, so +the two must carry an **identical** GNSS profile. If they differ, the reported "improvement" is +the difference between two degradations and has nothing to do with the aid. The aiding +(`[gnss_degradation]`) and `[health_limits]` sections of +`conf/{ukf,ekf,rbpf}_{grav,mag,both}.toml` are duplicated from the matching +`conf/*_degraded.toml` for exactly this reason, and `core/tests/example_configs.rs` loads the +`conf/` recipes so that a drift between them fails a test. + +All nine geophysical recipes are scored against their own filter's +`conf/{ukf,ekf,rbpf}_degraded.toml`. The RBPF's geo-aided runs are additionally scored against +`conf/ekf_degraded.toml` as the canonical INS baseline (`just geoperf-all` and +`just geoperf-rbpf`). Keeping a single profile, applied identically everywhere, is what lets +that pairing be a filename convention rather than a per-tier lookup. ## Sources @@ -120,4 +162,4 @@ rather than a per-tier lookup. - [EUROCONTROL GNSS interference testing guide v2.0](https://www.eurocontrol.int/sites/default/files/2023-03/eurocontrol-gnss-interference-testing-guide-v2-0.pdf) - [RTCA DO-235C, *Assessment of radio frequency interference relevant to the GNSS L1 frequency band*](https://standards.globalspec.com/std/14557649/do-235c) - [Overbounding the effect of uncertain Gauss-Markov noise in Kalman filtering, *NAVIGATION* 68(2):259](https://navi.ion.org/content/68/2/259) -- [GNSS error simulator for farm machinery navigation](https://www.sciencedirect.com/science/article/abs/pii/S0168169915003348) — the first-order Gauss-Markov form `R(Δt) = σ² e^(-|Δt|/τ)` these time constants implement +- [GNSS error simulator for farm machinery navigation](https://www.sciencedirect.com/science/article/abs/pii/S0168169915003348), the source of the first-order Gauss-Markov form the time constants implement diff --git a/book/src/gnss/scenarios.md b/book/src/gnss/scenarios.md new file mode 100644 index 00000000..f25f927d --- /dev/null +++ b/book/src/gnss/scenarios.md @@ -0,0 +1,274 @@ +# Schedulers and Faults Reference + +This page lists every scheduler and fault model in `strapdown::messages`, with each field, its +unit and default, and how to set it from a configuration file and from the command line. For +why the shipped recipes use the values they do, see [Fault Simulation](./fault-simulation.md). + +The two are orthogonal. A **scheduler** (`MeasurementScheduler`) decides *when* a measurement +reaches the filter. A **fault model** (`GnssFaultModel`) decides *what* is wrong with each GNSS +fix that does. Both are applied once, when `build_event_stream` turns the input records into +the time-ordered stream of IMU and measurement events, so every filter sees exactly the same +degraded stream. + +## Where they are configured + +Everything on this page sits in one struct, `AidingConfig`, which is the `[aiding]` section of +a configuration file. The section was called `[gnss_degradation]` before the v1.0 freeze and +that spelling is still accepted as an alias; the recipes under `conf/` use it. + +| key | type | default | meaning | +|---|---|---|---| +| `scheduler` | scheduler | `pass_through` | when GNSS fixes are delivered | +| `fault` | fault model | `none` | how GNSS fixes are corrupted | +| `baro_scheduler` | scheduler | `fixed_interval`, 1 s | when barometric altitude is delivered | +| `magnetometer_scheduler` | scheduler | `fixed_interval`, 1 s | when magnetometer heading is delivered | +| `baro_noise_std_m` | m (1σ) | 2.236 (√5) | barometric altitude noise standard deviation | +| `baro_bias_index` | integer or unset | unset | which filter state holds the barometric bias (library use; see below) | +| `seed` | integer | 42 | seed for the fault model's random draws | +| `max_imu_gap_s` | s | 5.0 | longest inertial gap tolerated before the run is refused; `<= 0` disables | + +A complete example that runs as written (`input`/`output` pointed at a synthetic trajectory): + +```toml +mode = "closed-loop" +input = "synthetic.csv" +output = "results/baro.csv" + +[aiding] +seed = 7 +baro_noise_std_m = 3.0 +max_imu_gap_s = 5.0 + +[aiding.scheduler] # GNSS: 100 s available, 50 s denied +kind = "duty_cycle" +on_s = 100.0 +off_s = 50.0 +start_phase_s = 0.0 + +[aiding.baro_scheduler] # an independent barometer outage +kind = "duty_cycle" +on_s = 60.0 +off_s = 60.0 +start_phase_s = 0.0 + +[aiding.magnetometer_scheduler] +kind = "pass_through" +``` + +Three behaviours of the parser are worth knowing: + +- **A misspelled `kind` is an error.** `kind = "fixed"` fails with + ``unknown variant `fixed`, expected one of `pass_through`, `fixed_interval`, `duty_cycle` ``. +- **A missing field is an error.** Scheduler and fault fields have no defaults, except the two + optional time constants on `degraded`; leave one out and the run stops with + ``missing field `...` ``. +- **A misspelled section name is not.** Unknown top-level keys are ignored, so + `[gnss_degredation]` parses, is discarded, and the run proceeds with no degradation at all. + `core/tests/example_configs.rs` guards the shipped files against this; your own files are + not guarded, so check section names against this page. + +**Seeds.** In a configuration file the fault model is seeded by `[aiding] seed`, not by the +top-level `seed`: two files differing only in the top-level `seed` produce identical +closed-loop output. On the command line, `--seed` is what seeds the fault model. + +**`baro_bias_index`** tells the barometer measurement which state of the filter holds a +barometric bias. `strapdown-sim` overwrites it from the filter it builds (and clears it for the +particle filter, which carries no barometric bias state), so setting it in a `strapdown-sim` +config file has no effect. It matters when you call `build_event_stream` from Rust with your +own filter. + +## Schedulers + +The configuration file names the variant with `kind` in snake case. The command line uses +shorter names: `--sched passthrough|fixed|duty`. The GNSS scheduler is the only one with flags; +the barometer and magnetometer schedulers are set from a configuration file or from Rust, and +otherwise take their 1 Hz default. + +### `PassThrough` + +`kind = "pass_through"`, `--sched passthrough` (the CLI default). Every record that carries a +complete fix delivers it. No fields. + +### `FixedInterval` + +`kind = "fixed_interval"`, `--sched fixed`. + +| field | unit | CLI flag | CLI default | meaning | +|---|---|---|---|---| +| `interval_s` | s | `--interval-s` | 1.0 | interval between delivered fixes | +| `phase_s` | s | `--phase-s` | 0.0 | time of the first tick | + +Ticks fall on exact multiples of `interval_s` after `phase_s`, so they do not creep over a long +run. It is a **rate limit, not a resampler**: it delivers the first record at or after each +tick, so a log sampled more slowly than `interval_s` delivers every record. After a gap in the +log the clock jumps past the gap rather than bursting to catch up. A non-positive or non-finite +`interval_s` behaves as `PassThrough` rather than silently withholding every fix. + +### `DutyCycle` + +`kind = "duty_cycle"`, `--sched duty`. + +| field | unit | CLI flag | CLI default | meaning | +|---|---|---|---|---| +| `on_s` | s | `--on-s` | 10.0 | length of each ON (available) window | +| `off_s` | s | `--off-s` | 10.0 | length of each OFF (denied) window | +| `start_phase_s` | s | `--duty-phase-s` | 0.0 | length of an initial ON window | + +The timeline is `start_phase_s` of ON, then `off_s` OFF and `on_s` ON, repeating. **The cycle +starts with its OFF window**, so with `start_phase_s = 0` the run begins in an outage: + +```text +start_phase_s = 0: OFF [0, off_s) ON [off_s, off_s+on_s) OFF ... +start_phase_s = p: ON [0, p) OFF [p, p+off_s) ON [p+off_s, p+off_s+on_s) OFF ... +``` + +Set `start_phase_s` (on the command line, `--duty-phase-s`; note it is not `--phase-s`, which +belongs to `fixed`) to let the filter converge on some fixes before the first outage. Every +record inside an ON window delivers its fix. A cycle whose `on_s + off_s` is not positive and +finite behaves as `PassThrough`. + +## Fault models + +The configuration file names the variant with `kind` in snake case. The command line spells it +`--fault none|degraded|slowbias|hijack`. Every field is required in a configuration file except +`tau_pos_s` and `tau_vel_s`; the CLI defaults below apply only on the command line. + +Faults apply to GNSS only. Barometer and magnetometer measurements are scheduled but never +corrupted. Each fault advances once per *delivered* fix, with `dt` the time since the previous +delivered fix. + +### `None` + +`kind = "none"`, `--fault none` (the CLI default). Fixes reach the filter unchanged. + +### `Degraded` + +`kind = "degraded"`, `--fault degraded`. AR(1)-correlated error added to position (north, east +and up) and to north/east velocity, and the advertised accuracies inflated. + +| field | unit | CLI flag | CLI default | meaning | +|---|---|---|---|---| +| `rho_pos` | -- | `--rho-pos` | 0.99 | AR(1) coefficient for position error, per fix | +| `sigma_pos_m` | m | `--sigma-pos-m` | 3.0 | position innovation σ per fix; steady-state σ if `tau_pos_s` is set | +| `rho_vel` | -- | `--rho-vel` | 0.95 | AR(1) coefficient for velocity error, per fix | +| `sigma_vel_mps` | m/s | `--sigma-vel-mps` | 0.3 | velocity innovation σ per fix; steady-state σ if `tau_vel_s` is set | +| `r_scale` | -- | `--r-scale` | 5.0 | multiplies the advertised horizontal and velocity 1σ | +| `tau_pos_s` | s | `--tau-pos-s` | unset | position correlation time (optional) | +| `tau_vel_s` | s | `--tau-vel-s` | unset | velocity correlation time (optional) | + +Each error state $x$ follows + +$$x_k = \rho\ x_{k-1} + w_k, \qquad w_k \sim \mathcal{N}(0, s^2).$$ + +Without a time constant, $\rho$ is `rho_pos` and $s$ is `sigma_pos_m`, applied per delivered +fix, so the correlation time depends on the schedule and the error settles at +$s/\sqrt{1-\rho^2}$, not at $s$. With `tau_pos_s` set, $\rho = e^{-\Delta t/\tau}$ for the +actual interval $\Delta t$ between fixes and $s = \sigma\sqrt{1-\rho^2}$, which holds the +steady-state standard deviation at `sigma_pos_m` whatever the schedule. The time constants were +added in commit `96a47f4` for exactly this reason; [Fault +Simulation](./fault-simulation.md#correlation-time-and-the-fix-interval) has the argument. A +non-positive or non-finite `tau` falls back to the per-fix form. + +`r_scale` scales the advertised **standard deviations**, which the GNSS measurement model +squares to build $R$, so $R$ grows by `r_scale²`. The advertised vertical accuracy is not +scaled, although the altitude does receive the position error. + +### `SlowBias` + +`kind = "slow_bias"`, `--fault slowbias`. A soft spoof: a north/east position offset that +drifts at a set rate, with a matching velocity offset so that position and velocity stay +mutually consistent. + +| field | unit | CLI flag | CLI default | meaning | +|---|---|---|---|---| +| `drift_n_mps` | m/s | `--drift-n-mps` | 0.02 | northward drift rate of the offset | +| `drift_e_mps` | m/s | `--drift-e-mps` | 0.0 | eastward drift rate of the offset | +| `q_bias` | m²/s | `--q-bias` | 1e-6 | random-walk intensity on the offset; 0 disables | +| `rotate_omega_rps` | rad/s | `--rotate-omega-rps` | 0.0 | rotation rate of the drift direction; 0 keeps it fixed | + +At each fix the offset grows by the drift rate times $\Delta t$, plus a zero-mean draw with +variance `q_bias`$\ \Delta t$ per axis. With `rotate_omega_rps` non-zero, the drift vector is +rotated by $\omega t$ before it is applied. The reported velocity is offset by the current drift +rate. The advertised accuracies are unchanged. + +### `Hijack` + +`kind = "hijack"`, `--fault hijack`. A hard spoof: a constant north/east offset over a fixed +window, with velocity left alone. + +| field | unit | CLI flag | CLI default | meaning | +|---|---|---|---|---| +| `offset_n_m` | m | `--hijack-offset-n-m` | 50.0 | northward offset | +| `offset_e_m` | m | `--hijack-offset-e-m` | 0.0 | eastward offset | +| `start_s` | s | `--hijack-start-s` | 120.0 | start of the window, from the first record | +| `duration_s` | s | `--hijack-duration-s` | 60.0 | length of the window | + +The offset applies to fixes with elapsed time in $[t_\text{start}, t_\text{start} + +t_\text{duration}]$, endpoints included, and to none outside it. + +### `Combo` + +`kind = "combo"`. Several fault models applied in sequence to the same fix, the output of each +feeding the next. It is available from a configuration file and from Rust; `--fault` has no +`combo` choice. The members go in a `faults` array, each a complete fault model of its own: + +```toml +[aiding.fault] +kind = "combo" + +[[aiding.fault.faults]] +kind = "slow_bias" +drift_n_mps = 0.02 +drift_e_mps = 0.0 +q_bias = 1e-6 +rotate_omega_rps = 0.0 + +[[aiding.fault.faults]] +kind = "hijack" +offset_n_m = 50.0 +offset_e_m = 0.0 +start_s = 120.0 +duration_s = 60.0 +``` + +The members share one fault state. Combine *different* kinds: two `degraded` members step the +same AR(1) states twice per fix rather than adding two independent errors. + +## Example commands + +Each of these was run against a 600 s trajectory from `strapdown-sim syn -o synthetic.csv +--duration-s 600 --seed 42`: + +```bash +# One fix a minute, otherwise clean +strapdown-sim cl -i synthetic.csv -o fixed.csv --sched fixed --interval-s 60 --fault none + +# 100 s on / 50 s off, with time-constant AR(1) degradation on the fixes that remain +strapdown-sim cl -i synthetic.csv -o duty.csv --seed 42 \ + --sched duty --on-s 100 --off-s 50 \ + --fault degraded --sigma-pos-m 35 --tau-pos-s 500 --sigma-vel-mps 1.5 --tau-vel-s 100 + +# Soft spoof, drift direction rotating at 1 mrad/s +strapdown-sim cl -i synthetic.csv -o slowbias.csv \ + --fault slowbias --drift-n-mps 0.05 --rotate-omega-rps 0.001 + +# Hard spoof: 50 m north between t = 120 s and t = 180 s +strapdown-sim cl -i synthetic.csv -o hijack.csv \ + --fault hijack --hijack-offset-n-m 50 --hijack-start-s 120 --hijack-duration-s 60 + +# The particle filter takes the same flags +strapdown-sim pf -i synthetic.csv -o pf.csv --sched fixed --interval-s 10 --num-particles 200 +``` + +## Shipped recipes + +- `examples/configs/*.yaml` covers each scheduler and fault on its own and in pairs; see + [Example Configurations](../examples/configurations.md). +- `conf/{ukf,ekf,rbpf}_degraded.toml` is the experiment matrix's single interference profile: + `fixed_interval` at 60 s with `degraded` at 35 m and 1.5 m/s steady state, `tau_pos_s = 500`, + `tau_vel_s = 100`, `r_scale = 5`. The `conf/*_truth.toml` recipes are the unscheduled, + unfaulted counterparts, and `conf/*_{grav,mag,both}.toml` repeat the degraded profile with + geophysical aiding added. `conf/real/` holds the same fifteen recipes for the frozen + real-sensor arm of the experiment. +- No recipe ships for total denial. `--sched duty --on-s 30 --off-s 120 --duty-phase-s 30 + --fault none` gives 30 s of fixes in every 150 s with availability as the only variable. diff --git a/book/src/installation/installation.md b/book/src/installation/installation.md index 52731fa8..13d61bde 100644 --- a/book/src/installation/installation.md +++ b/book/src/installation/installation.md @@ -1,146 +1,166 @@ # Installation -This page provides detailed instructions for installing Strapdown-rs on your system. +There are two things to install, and most readers want only one of them: -## Prerequisites +- the **`strapdown-sim` binary**, to run simulations from the command line; +- the **`strapdown-core` library**, to call the mechanization and filters from your own Rust + code. It is imported as `strapdown`. -Before installing Strapdown-rs, ensure you have the following: +## Version 1.0 is installed from git -- **Rust**: Version 1.91 or higher (install from [rustup.rs](https://rustup.rs)) -- **A C/C++ compiler and cmake >= 3.26**, for the features that use a C library +At the time of writing, crates.io carries only pre-1.0 releases of these crates, whose API +differs from the one this book documents. Version 1.0.0 is published to crates.io together with +the JOSS paper. **Until then, install from the GitHub repository**, as below. Once 1.0.0 is on +crates.io, drop the `--git` argument from every command on this page. -## Build Dependencies +## Before you start -There are **no system libraries to install**. libhdf5, libnetcdf, zlib and freetype are -compiled from vendored sources that ship as ordinary cargo dependencies, so nothing is looked -for on your machine. You only need a toolchain to compile them with. +You need a Rust toolchain, version **1.91 or newer**, from [rustup.rs](https://rustup.rs). +Whether you need anything else depends on which features you build: -### Ubuntu/Debian +| What you build | Needs | +| --- | --- | +| `strapdown-core` with default features | Rust only | +| `strapdown-core` with `mcap`; `strapdown-sim` with its default `plotting` feature | Rust and a C compiler | +| `strapdown-core` with `hdf5` or `netcdf`; `strapdown-sim` with `geonav`; `strapdown-geonav` | Rust, a C/C++ compiler, and **cmake 3.26 or newer** | -```bash -sudo apt update -sudo apt install -y build-essential cmake -``` +No system libraries are searched for. libhdf5, libnetcdf, zlib and FreeType are compiled from +vendored sources that ship as ordinary cargo dependencies, so the compiler and cmake are all +the build needs. Two details catch people out: -Check `cmake --version`: the bundled HDF5 needs **3.26 or newer**, which is more than Ubuntu -22.04 (3.22) or Debian 12 (3.25) ship. On those, install cmake from -[Kitware's APT repository](https://apt.kitware.com/), or with `pip install cmake`, or with -`snap install cmake --classic`. +- **cmake 3.26 is newer than some distributions ship.** Ubuntu 22.04 has 3.22 and Debian 12 has + 3.25, and both fail. Install a newer cmake from + [Kitware's APT repository](https://apt.kitware.com/), with `pip install cmake`, or with + `snap install cmake --classic`. Ubuntu 24.04 and Fedora 40+ are new enough as shipped. +- **`HDF5_DIR` must be unset.** When it is set, the HDF5 build script looks for a system + library instead of building the vendored one, even though a vendored build was requested, + and the netCDF build then fails against those headers with an error that does not name the + cause. A leftover conda or pixi shell is the usual source. -### Fedora/RHEL +[System Requirements](./requirements.md) lists the per-platform commands for the compiler and +cmake. + +## Install the simulator ```bash -sudo dnf install -y gcc gcc-c++ cmake +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim ``` -### macOS +With experimental geophysical (gravity and magnetic anomaly) aiding, which adds the `--geo` +family of flags to `cl` and `pf`: ```bash -xcode-select --install -brew install cmake +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim --features geonav ``` -### Windows - -Install the MSVC toolchain from Visual Studio Build Tools, plus -[cmake](https://cmake.org/download/). vcpkg is not needed. +Check the result: -> **Note:** `strapdown-core` on its own needs none of this -- `cargo add strapdown-core` and a -> Rust toolchain are enough unless you turn on its `hdf5` or `netcdf` features. - -## Installation Methods +```console +$ strapdown-sim --version +strapdown-sim 1.0.0 +``` -### Method 1: Install from Crates.io (Recommended) +`cargo install` puts the binary in `~/.cargo/bin`, which rustup adds to your `PATH`. -The easiest way to use Strapdown-rs is to add it as a dependency in your project: +## Add the library to a project ```bash -cargo add strapdown-core +cargo add strapdown-core --git https://github.com/jbrodovsky/strapdown-rs ``` -Or add manually to your `Cargo.toml`: +That writes this entry to your `Cargo.toml`: ```toml [dependencies] -strapdown-core = "0.1" +strapdown-core = { git = "https://github.com/jbrodovsky/strapdown-rs", version = "1.0.0" } ``` -To install the simulation binary: +Add features with `--features`, for example +`cargo add strapdown-core --git https://github.com/jbrodovsky/strapdown-rs --features mcap`. +In your code the crate is `strapdown`, not `strapdown_core`. [Using the +Library](../user-guide/library.md) continues from here. -```bash -cargo install strapdown-sim -``` - -### Method 2: Build from Source +## Cargo features -Clone the repository and build locally: +### `strapdown-core` -```bash -# Clone the repository -git clone https://github.com/jbrodovsky/strapdown-rs.git -cd strapdown-rs +| Feature | Default | What it adds | Build needs | +| --- | --- | --- | --- | +| `clap` | no | `clap` `ValueEnum` and `Args` derives on the simulation configuration types in `strapdown::sim`, so a CLI can take them as arguments. `strapdown-sim` turns this on | Rust only | +| `hdf5` | no | `NavigationResult::to_hdf5` / `from_hdf5` and the matching `TestDataRecord` readers and writers | C/C++ compiler, cmake ≥ 3.26 | +| `netcdf` | no | `NavigationResult::to_netcdf` / `from_netcdf` and the `TestDataRecord` equivalents | C/C++ compiler, cmake ≥ 3.26 | +| `mcap` | no | `NavigationResult::to_mcap` / `from_mcap` and the `TestDataRecord` equivalents | C compiler (bundled LZ4 and Zstandard) | +| `full` | no | All four of the above | as for `hdf5` | -# Build the entire workspace -cargo build --workspace --all-features --release +The default build has none of them and compiles with Rust alone. These writers are library +methods only. `strapdown-sim` writes CSV whatever features the library was built with: an +`-o` path that does not end in `.csv` is treated as a *directory*, and the results are written +into it as `.csv` files (`-o out.h5` produces `out.h5/.csv`). -# Install the simulation binary -cargo install --path sim +### `strapdown-sim` -# Optionally, with geophysical (gravity/magnetic) navigation. This is the variant that -# compiles libnetcdf from source, so it needs cmake. -cargo install --path sim --features geonav -``` +| Feature | Default | What it adds | Build needs | +| --- | --- | --- | --- | +| `plotting` | **yes** | Rendering for the global `--plot` flag, a performance plot against the GNSS track. Without the feature the flag is still accepted, but it logs an error and draws nothing | C compiler (bundled FreeType); libfontconfig at **run time** | +| `geonav` | no | Gravity and magnetic anomaly map aiding: `--geo`, `--gravity-*`, `--magnetic-*` and `--geo-interval-s` on `cl` and `pf`. Without the feature these flags do not exist | C/C++ compiler, cmake ≥ 3.26 | -> The repository used to ship a `pixi.toml` for environment management. It was removed once -> the C libraries began building from source, since there was nothing left for it to provide; -> `cargo build` is now the whole story. +`plotting` loads libfontconfig when it first draws text rather than linking it at build time. +A machine without it builds and runs normally, and fails only when a plot is rendered. It is +present on virtually every desktop Linux install; on a minimal container, `apt install +libfontconfig1`. -## Verifying Installation +## Build from a clone -After installation, verify everything works: +Contributors, and anyone who wants the examples and test fixtures, should clone the +repository: ```bash -# Check strapdown-sim version -strapdown-sim --version - -# Run a simple test -cargo test -p strapdown-core +git clone https://github.com/jbrodovsky/strapdown-rs.git +cd strapdown-rs +cargo build --workspace --release ``` -## Troubleshooting - -### HDF5/NetCDF Build Issues +The binary is then at `target/release/strapdown-sim`. Inside the clone, `rust-toolchain.toml` +pins the toolchain the CI uses (1.91) and rustup fetches it on the first cargo command, so you +do not choose a version yourself. `.cargo/config.toml` additionally forces the vendored +FreeType, so a repository build never links a system copy. (`cargo install` from git does not +read that file and may link a system FreeType if pkg-config reports one; both work.) -These libraries are compiled from source, so failures look like cmake or compiler errors -rather than linker errors. +To put a binary built from your clone on your `PATH`: -1. **`CMake 3.26 or higher is required`** -- your cmake is too old. See the Build Dependencies - section above. +```bash +cargo install --path sim +cargo install --path sim --features geonav # with geophysical aiding +``` -2. **Confusing cmake failures inside the netCDF build** -- check whether `HDF5_DIR` is set: - ```bash - echo "${HDF5_DIR:-unset}" - ``` - If it is, the HDF5 build script quietly switches to looking for a *system* library instead - of building the vendored one, and the netCDF build is then handed the wrong headers. Unset - it. A leftover conda or pixi shell is the usual source. +There is no environment manager to set up; `cargo build` is the whole setup. Running the test +suite, the lint gate and the performance baselines is covered in +[Building and Testing](../development/building.md). -3. **`cargo install` is slow the first time** -- that is the one-off source build of libhdf5 - and libnetcdf, roughly a minute on a modern machine. It is cached afterwards. +## Troubleshooting -### Rust Version Issues +**`CMake 3.26 or higher is required`.** Your cmake is too old for the bundled HDF5. Install a +newer one as described under [Before you start](#before-you-start). -Ensure you're using a recent Rust version: +**Confusing cmake failures inside the netCDF build.** Check for a stale `HDF5_DIR`: ```bash -rustc --version +echo "${HDF5_DIR:-unset}" ``` -Inside a clone of the repository, `rust-toolchain.toml` pins the version and rustup fetches it -for you, so this should already agree with what CI uses. +If it prints a path, unset it and rebuild. + +**The first build of `hdf5`, `netcdf` or `geonav` is slow.** That is the one-off source build +of libhdf5 and libnetcdf. Cargo caches it, and later builds reuse it. + +**A plot fails to render while everything else works.** libfontconfig is missing at run time; +see the `plotting` row above. + +**`cargo add strapdown-core` without `--git` resolved to a 0.x version.** That is the last +pre-1.0 release on crates.io. Its API is not the one in this book. Use the `--git` form until +1.0.0 is published. -## Next Steps +## Next steps -- Continue to the [Quick Start](../quick-start.md) guide -- Learn about [System Requirements](./requirements.md) -- Explore [Building from Source](./building.md) in detail +- [Quick Start](../quick-start.md): generate a trajectory and run every simulation mode on it. +- [System Requirements](./requirements.md): platform-specific setup. diff --git a/book/src/installation/requirements.md b/book/src/installation/requirements.md index 04853f2d..c96ef7b8 100644 --- a/book/src/installation/requirements.md +++ b/book/src/installation/requirements.md @@ -1,41 +1,24 @@ # System Requirements -This page outlines the system requirements for running Strapdown-rs. +This page lists what each part of the project needs to build and run, and how to get it on +each platform. [Installation](./installation.md) has the install commands themselves. -## Hardware Requirements +## Platforms -### Minimum +The code is platform-independent Rust plus vendored C libraries. Continuous integration builds +and tests `strapdown-core` on Linux, macOS and Windows (`ubuntu-latest`, `macos-latest`, +`windows-latest` GitHub runners), builds `strapdown-geonav` with its vendored netCDF and HDF5 +on macOS and Windows, and builds and tests the whole workspace with every feature on Linux. -- **CPU**: Any modern 64-bit processor (x86_64 or ARM64) -- **RAM**: 2 GB -- **Storage**: 100 MB for binaries, plus space for data files +There are no special hardware requirements. Memory and run time scale with the length of the +trajectory and, for the particle filter, with `--num-particles`. -### Recommended +## Rust toolchain -- **CPU**: Multi-core processor (4+ cores) for parallel processing -- **RAM**: 8 GB or more for large simulations -- **Storage**: SSD with sufficient space for trajectory data and results - -## Software Requirements - -### Operating Systems - -Strapdown-rs supports all major operating systems: - -- **Linux**: Ubuntu 24.04+, Debian 13+, Fedora 40+, or equivalent. Older releases work too, - but only once a cmake newer than the distribution's own is installed -- see the C toolchain - note below. -- **macOS**: 11.0 (Big Sur) or later -- **Windows**: Windows 10/11 with MSVC or MinGW - -### Rust Toolchain - -- **Minimum Rust version**: 1.91 -- **Required components**: `rustc`, `cargo`, `clippy`, `rustfmt` - -Contributors working in a clone of the repository do not need to choose a version: a -`rust-toolchain.toml` pins 1.91 and rustup fetches it automatically on the first cargo -command. +- **Rust 1.91 or newer** (`rustc` and `cargo`). This is the workspace's declared + `rust-version`. +- `clippy` and `rustfmt` only if you plan to contribute; see + [Building and Testing](../development/building.md). Install via [rustup](https://rustup.rs/): @@ -43,99 +26,89 @@ Install via [rustup](https://rustup.rs/): curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh ``` -### C Toolchain - -There are **no system libraries to install**. libhdf5, libnetcdf, zlib and freetype are all -compiled from vendored sources shipped as ordinary crates.io dependencies. What you do need is -a toolchain to compile them with: +Inside a clone of the repository you do not choose a version: `rust-toolchain.toml` pins 1.91 +and rustup fetches it on the first cargo command. -- **A C/C++ compiler**: GCC, Clang, or MSVC. -- **cmake, version 3.26 or later**: required by the bundled HDF5. +## C toolchain -Both are needed only when you build something that touches a C library -- `strapdown-geonav` -always, `strapdown-core`'s `hdf5`/`netcdf` features, and `strapdown-sim`'s `plotting` feature -(for freetype). `cargo build -p strapdown-core` on its own needs neither. +There are **no system libraries to install**. libhdf5, libnetcdf, zlib and FreeType are +compiled from vendored sources that ship as ordinary cargo dependencies. What you may need is a +toolchain to compile them, depending on the features you build: -One caveat on "nothing is searched for": inside a clone of this repository that is exactly -true, because `.cargo/config.toml` sets `FREETYPE2_NO_PKG_CONFIG` and the HDF5/netCDF/zlib -builds are pinned to their vendored sources by cargo features. `freetype-sys` on its own would -prefer a system FreeType when pkg-config reports one, so `cargo install strapdown-sim` -- which -does not see this repository's cargo config -- may link the copy on your machine instead. Both -work; only the repository build is fully hermetic. +| Needs | When | +| --- | --- | +| Nothing beyond Rust | `strapdown-core` with default features | +| A C compiler (GCC, Clang or MSVC) | `strapdown-sim` with its default `plotting` feature (FreeType); `strapdown-core` with `mcap` (LZ4, Zstandard) | +| A C/C++ compiler **and cmake 3.26 or newer** | `strapdown-core` with `hdf5` or `netcdf`; `strapdown-sim` with `geonav`; `strapdown-geonav` always | > **cmake 3.26 is newer than some distributions ship.** Ubuntu 22.04 LTS has 3.22 and > Debian 12 has 3.25; both fail. Install a newer cmake from > [Kitware's APT repository](https://apt.kitware.com/), with `pip install cmake`, or with > `snap install cmake --classic`. Ubuntu 24.04 and Fedora 40+ are new enough as shipped. -#### Runtime +### A variable to keep unset + +**`HDF5_DIR`**: if this is set, the HDF5 build script looks for a system library instead of +building the vendored one, and the netCDF build then fails against those headers with an +error that does not name the cause. Unset it. A leftover conda or pixi shell is the usual +source. -- **libfontconfig**: needed at *run time* by `strapdown-sim`'s `plotting` feature, which loads - it on demand rather than linking it. A machine without it builds fine and fails only when a - plot is first rendered. Present by default on virtually every desktop Linux install; on a - minimal container, `apt install libfontconfig1`. +### "Nothing is searched for" has one exception -#### A variable to keep unset +Inside a clone of the repository the build is fully hermetic: `.cargo/config.toml` sets +`FREETYPE2_NO_PKG_CONFIG`, and the HDF5, netCDF and zlib builds are pinned to their vendored +sources by cargo features. `freetype-sys` on its own prefers a system FreeType when pkg-config +reports one, so `cargo install` from git or crates.io, which does not read the repository's +cargo configuration, may link the copy on your machine instead. Both work. -- **`HDF5_DIR`**: if this is set, the HDF5 build script looks for a system library instead of - building the vendored one, and the netCDF build then fails against those headers with an - error that does not name the cause. Unset it (a leftover conda or pixi shell is the usual - source). +### Runtime -## Platform-Specific Notes +**libfontconfig** is needed at *run time* by `strapdown-sim`'s `plotting` feature, which loads +it on demand rather than linking it. A machine without it builds and runs normally, and fails +only when a plot is first rendered. It is present on virtually every desktop Linux install; on +a minimal container, `apt install libfontconfig1`. + +## Platform-specific setup ### Linux -Install a compiler and cmake with your package manager -- for example `apt install build-essential cmake` -- then check `cmake --version` against the 3.26 note above. +Ubuntu or Debian: + +```bash +sudo apt update +sudo apt install -y build-essential cmake +cmake --version # must be 3.26 or newer for hdf5/netcdf/geonav +``` -**Performance Note**: For best performance, consider building with native CPU optimizations: +Fedora or RHEL: ```bash -RUSTFLAGS="-C target-cpu=native" cargo build --release +sudo dnf install -y gcc gcc-c++ cmake ``` ### macOS -Apple Silicon (M1/M2/M3) is fully supported. The ARM64 architecture provides excellent performance for navigation algorithms. - -Xcode Command Line Tools provide the compiler (`xcode-select --install`) and `brew install cmake` provides the rest. +```bash +xcode-select --install # the C/C++ compiler +brew install cmake +``` ### Windows -The MSVC toolchain from Visual Studio Build Tools, plus cmake, is all that is required. vcpkg is no longer needed: the C libraries are built from source rather than located on the system. - -Alternatively, use WSL2 (Windows Subsystem for Linux) for a native Linux environment. - -## Development Requirements +Install the MSVC toolchain from Visual Studio Build Tools, plus +[cmake](https://cmake.org/download/). vcpkg is not needed: the C libraries are built from +source rather than located on the system. WSL2 with the Linux instructions above also works. -If you plan to contribute to Strapdown-rs development: - -- **Git**: Version control -- **clippy**: Rust linter (`rustup component add clippy`) -- **rustfmt**: Code formatter (`rustup component add rustfmt`) -- **cargo-edit**: For managing dependencies (`cargo install cargo-edit`) - -## Testing Your Environment - -After installing dependencies, verify your environment: +## Checking your environment ```bash -# Check Rust version -rustc --version - -# Check the C toolchain -cc --version -cmake --version # must be 3.26 or newer - -# Make sure no stale HDF5 override is in the environment +rustc --version # 1.91 or newer +cc --version # any C compiler, if you build plotting/mcap/hdf5/netcdf/geonav +cmake --version # 3.26 or newer, if you build hdf5/netcdf/geonav echo "HDF5_DIR=${HDF5_DIR:-(unset, good)}" - -# Test compilation. The first run compiles libhdf5 and libnetcdf from source, which takes -# roughly a minute; later runs reuse them. -cargo build --workspace ``` -## Next Steps +## Next steps -- Proceed to [Installing from Crates.io](./crates-io.md) -- Or learn about [Building from Source](./building.md) +- [Installation](./installation.md): install the simulator or add the library. +- [Quick Start](../quick-start.md): run every simulation mode on a synthetic trajectory. diff --git a/book/src/introduction.md b/book/src/introduction.md index 4d6a3032..5a7c39bd 100644 --- a/book/src/introduction.md +++ b/book/src/introduction.md @@ -1,56 +1,114 @@ # Introduction -Welcome to the **Strapdown-rs** documentation! This guide will help you understand and use the Strapdown-rs library, a high-performance Rust implementation of strapdown inertial navigation system (INS) algorithms. - -[![Crates.io](https://img.shields.io/crates/v/strapdown-rs.svg)](https://crates.io/crates/strapdown-rs) -[![Documentation](https://docs.rs/strapdown-rs/badge.svg)](https://docs.rs/strapdown-rs) -[![License](https://img.shields.io/crates/l/strapdown-rs.svg)](https://crates.io/crates/strapdown-rs) - -## What is Strapdown-rs? - -Strapdown-rs is a straightforward strapdown inertial navigation system (INS) implementation in Rust. It is designed to be simple, performant, and easy to understand, making it an excellent starting point for those interested in learning about or implementing strapdown INS algorithms. - -The project provides: - -1. **A source library** of strapdown INS algorithms and utilities (`strapdown-core`) -2. **A simulation program** for simulating INS performance in various GNSS conditions (`strapdown-sim`) -3. **Experimental geophysical navigation** capabilities for GNSS-denied environments (`strapdown-geonav`) - -## Key Features - -- **Modern Rust Implementation**: Memory-safe, high-performance code with cross-platform support -- **Multiple Navigation Filters**: Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Particle Filters -- **GNSS Degradation Scenarios**: Simulate dropouts, reduced update rates, and measurement corruption -- **Geophysical Navigation**: Experimental work on gravity and magnetic anomaly navigation -- **Comprehensive Documentation**: Well-documented code with examples and tutorials -- **Research-Focused**: Designed for research, teaching, and development purposes - -## Target Audience - -This library is intended for: - -- **Researchers** working on navigation systems and sensor fusion -- **Engineers** developing autonomous systems, robotics, or aerospace applications -- **Students** learning about inertial navigation and state estimation -- **Developers** needing a high-performance INS implementation in Rust - -## Project Status - -Strapdown-rs is under active development as part of ongoing PhD research. The project prioritizes correctness, numerical stability, and performance while maintaining extensibility for researchers and engineers. - -## Getting Help - -- **User Guide**: Start with the [Quick Start](./quick-start.md) guide -- **API Documentation**: See the [API Reference](./api/core.md) for detailed function documentation -- **Examples**: Check out the [Examples and Tutorials](./examples/configurations.md) -- **Issues**: Report bugs or request features on [GitHub](https://github.com/jbrodovsky/strapdown-rs/issues) +[![JOSS review status](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4/status.svg)](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4) +[![CI](https://github.com/jbrodovsky/strapdown-rs/actions/workflows/rust.yml/badge.svg)](https://github.com/jbrodovsky/strapdown-rs/actions/workflows/rust.yml) +[![Crates.io](https://img.shields.io/crates/v/strapdown-core.svg)](https://crates.io/crates/strapdown-core) +[![docs.rs](https://docs.rs/strapdown-core/badge.svg)](https://docs.rs/strapdown-core) +[![License: MIT](https://img.shields.io/badge/license-MIT-blue.svg)](https://github.com/jbrodovsky/strapdown-rs/blob/main/LICENSE) + +`strapdown-rs` is a Rust library and command-line simulator for research on strapdown inertial +navigation systems (INS), with an emphasis on what happens when GNSS is unavailable or cannot be +trusted. It mechanizes recorded or synthetic IMU data, fuses it with GNSS and other aiding +sources through one of four navigation filters, and lets you remove, thin or corrupt the GNSS +stream according to a seeded, repeatable scenario. + +> The crates.io and docs.rs badges show the most recent *published* release, which predates +> 1.0. Version 1.0.0 is published together with the JOSS paper; until then, install from git +> as described in [Installation](./installation/installation.md). + +## What it provides + +The repository is a Cargo workspace of three crates: + +| Crate | Imported as | What it is | +| --- | --- | --- | +| `strapdown-core` | `strapdown` | Mechanization, Earth model, filters, measurement models, the scenario engine and CSV I/O | +| `strapdown-sim` | (binary) | The `strapdown-sim` command-line simulator | +| `strapdown-geonav` | `geonav` | **Experimental** gravity and magnetic anomaly map aiding | + +The pieces that matter most: + +- **Strapdown mechanization** in the local-level (north, east, vertical) frame, following + Groves, *Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems*, 2nd + ed., §5.4-5.5. See [The Navigation Model](./user-guide/concepts.md). +- **Four navigation filters**, all behind the `NavigationFilter` trait: + - the error-state Kalman filter (ESKF), the default for closed-loop runs; + - the extended Kalman filter (EKF); + - the unscented Kalman filter (UKF); + - a Rao-Blackwellized particle filter (RBPF) after Canciani & Raquet (2017), the only + particle filter in the crate. +- **Aiding**: GNSS position and velocity, barometric relative altitude (with a barometric + bias state, on by default in the simulator) and magnetometer heading. The library adds + zero-velocity and zero-angular-rate updates driven by a stationary detector. +- **GNSS degradation**: schedulers that decide *when* fixes arrive (pass-through, fixed + interval, duty-cycled outages) and fault models that decide *what* they contain (AR(1) + degradation, slow bias drift, position hijack). See [Fault Simulation](./gnss/fault-simulation.md). +- **Synthetic trajectories** with IMU error models by sensor grade (`strapdown-sim syn`), so + every example in this book runs without a dataset. +- **Reproducibility**: the random draws are seeded (`--seed`, or `seed` in a configuration + file), so the same inputs and seed give a byte-identical result, and a whole run can be + described by one TOML, YAML or JSON file. + +## What it is not + +The boundaries, stated up front: + +- **Loosely coupled only.** Aiding enters as position, velocity, altitude or heading + measurements. There are no pseudorange or carrier-phase models, no satellite geometry and no + raw-signal processing. +- **Not IMU firmware or a driver.** It reads logged samples (CSV) or samples you hand to the + library; it does not talk to hardware. +- **No coning or sculling compensation.** Each IMU sample is integrated to first order, with + the attitude averaged across the interval when the specific force is resolved. That suits + the sample rates and dynamics the project targets. It does not suit high-rate, + high-vibration, navigation-grade work. +- **Local-level frame only.** The mechanization is valid from 11 km below to 30 km above the + ellipsoid; there is no ECEF or ECI mechanization. +- **The CLI writes CSV only.** HDF5, NetCDF and MCAP writers exist as library methods on + `NavigationResult`, behind cargo features. There is no Parquet support. +- **One input format.** The simulator reads the + [Sensor Logger](https://www.tszheichoi.com/sensorlogger) CSV layout, which `syn` also + writes. Other datasets must be converted to that layout first; see + [Input Data Format](./user-guide/data-format.md). +- **Geophysical navigation is experimental.** `strapdown-geonav` is a research tool held at + 0.x so its API can change. + +## Who it is for + +- **Researchers** comparing navigation filters or aiding strategies under identical, repeatable + GNSS outages and faults. +- **Graduate students** learning strapdown navigation. Variables are named after the + quantities in Groves rather than after single letters, and every equation can be run. +- **Engineers** prototyping INS/GNSS integration who want a tested reference implementation to + compare their own against. + +## How this book is organised + +| Part | Read it when | +| --- | --- | +| [Installation](./installation/installation.md) and [Quick Start](./quick-start.md) | You want to run something in the next ten minutes | +| [User Guide](./user-guide/overview.md) | You want the model, the frames, the state vector, and every subcommand | +| [Navigation Filters](./filters/kalman.md) | You need to know how a particular filter works and how to tune it | +| [GNSS Degradation](./gnss/fault-simulation.md) | You are designing outage or spoofing scenarios | +| [Geophysical Navigation](./geonav/overview.md) | You work with gravity or magnetic anomaly maps (experimental) | +| [API Documentation](./api/index.md) | You are calling the library from Rust | +| [Examples and Tutorials](./examples/configurations.md) | You learn best from complete worked runs | +| [Development](./development/contributing.md) | You want to build, test or contribute | + +## Getting help + +- Questions and bug reports: [GitHub issues](https://github.com/jbrodovsky/strapdown-rs/issues). +- Frequently asked questions: [FAQ](./faq.md). +- Every subcommand documents its own flags: `strapdown-sim --help`. ## Citation -If you use Strapdown-rs in your research, please cite: - -[![JOSS Paper](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4/status.svg)](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4) +If you use `strapdown-rs` in research, please cite it using the repository's +[`CITATION.cff`](https://github.com/jbrodovsky/strapdown-rs/blob/main/CITATION.cff) (GitHub's +"Cite this repository" button). The JOSS paper is under review +([openjournals/joss-reviews#11377](https://github.com/openjournals/joss-reviews/issues/11377)); +its DOI will be added on acceptance. ## License -Strapdown-rs is licensed under the MIT License. See the [LICENSE](https://github.com/jbrodovsky/strapdown-rs/blob/main/LICENSE) file for details. +MIT. See [LICENSE](https://github.com/jbrodovsky/strapdown-rs/blob/main/LICENSE). diff --git a/book/src/quick-start.md b/book/src/quick-start.md index afb057f4..0f88752c 100644 --- a/book/src/quick-start.md +++ b/book/src/quick-start.md @@ -1,136 +1,262 @@ # Quick Start -This guide will get you up and running with Strapdown-rs in minutes. +This page runs every simulation mode on one synthetic trajectory, start to finish, in about +ten commands. Nothing needs downloading: `strapdown-sim syn` generates the input. Every command +and every line of output below was produced by `strapdown-sim 1.0.0`. -## Installation - -Add Strapdown-rs to your `Cargo.toml`: +If you have not installed the simulator yet: ```bash -cargo add strapdown-core +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim ``` -Or install the simulation binary: +See [Installation](./installation/installation.md) for the details and the build requirements. -```bash -cargo install strapdown-sim -``` +## 1. Generate a trajectory -## Running Your First Simulation +```console +$ strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +2026-10-01 20:37:53.960 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 20:37:53.971 [INFO] - Sensor records written to synthetic.csv +``` -### 1. Prepare Input Data +With no other flags, `syn` writes ten minutes of a **stationary** platform at 0° N, 0° E and +zero altitude, sampled at 10 Hz, with a consumer-grade IMU error model (`--imu-grade consumer`) +and a noisy GNSS fix on every row (2.5 m horizontal, 5 m vertical, one sigma). The flags to +move it, rotate it, change the IMU grade or the sample rate are listed by +`strapdown-sim syn --help`; see [Synthetic Trajectories](./user-guide/synthetic.md). -Strapdown-sim expects CSV files with IMU and GNSS data. The format follows the [Sensor Logger](https://github.com/tszheichoi/awesome-sensor-logger) app convention: +The file has the same 31 columns as a Sensor Logger export, which is the format every +simulation mode reads: -```csv -timestamp,gyro_x,gyro_y,gyro_z,accel_x,accel_y,accel_z,latitude,longitude,altitude -1234567890.0,0.01,-0.02,0.03,0.5,-0.2,9.81,45.5,-122.6,100.0 -... +```text +time,bearingAccuracy,speedAccuracy,verticalAccuracy,horizontalAccuracy,speed,bearing,altitude,longitude,latitude,qz,qy,qx,qw,roll,pitch,yaw,acc_z,acc_y,acc_x,gyro_z,gyro_y,gyro_x,mag_z,mag_y,mag_x,relativeAltitude,pressure,grav_z,grav_y,grav_x ``` -### 2. Declare the frame +The ones the filters use most: + +| Columns | Meaning | +| --- | --- | +| `time` | ISO 8601 UTC timestamp, e.g. `2025-01-01T00:00:00.100Z` | +| `acc_x`, `acc_y`, `acc_z` | Specific force in the body frame, m/s², **including** the reaction to gravity | +| `gyro_x`, `gyro_y`, `gyro_z` | Angular rate in the body frame, rad/s | +| `latitude`, `longitude`, `altitude` | GNSS position: degrees, degrees, metres | +| `speed`, `bearing` | GNSS ground speed (m/s) and course (degrees), turned into a north/east velocity fix | +| `horizontalAccuracy`, `verticalAccuracy`, `speedAccuracy` | GNSS one-sigma accuracies, used as the measurement noise | +| `qw`, `qx`, `qy`, `qz` | Attitude quaternion, used for the initial attitude | +| `relativeAltitude` | Barometric altitude change, m | +| `mag_x`, `mag_y`, `mag_z` | Magnetometer, µT, used for heading | + +A blank or `NaN` cell means "no measurement on this row". [Input Data +Format](./user-guide/data-format.md) documents every column. -Every command below assumes **NED** -- north, east, down -- which is the library default and -what `strapdown-sim syn` emits. +**This file is in the NED frame**, the library default: at rest its accelerometer reads about +−9.8 m/s² on `acc_z`, the down axis. Sensor Logger exports from a phone are ENU and need +`--enu` on every command. Declaring the wrong frame is refused before anything is integrated; +[Coordinate Frames](./user-guide/coordinate-frames.md) shows the error. -Sensor Logger exports are **ENU**: at rest the device reports `+g` along its up axis, not -`-g` along a down axis. For those, add `--enu` (or `is_enu = true` in a config file): +## 2. Closed loop: the ESKF ```bash -strapdown-sim cl --enu -i data/sensor_logger.csv -o results/output.csv +strapdown-sim cl -i synthetic.csv -o eskf.csv ``` -You will not get this wrong silently. The tool checks the declared frame against the leading -records' sensed specific force and refuses a mismatch, naming the flag to pass -- mechanizing -ENU records as NED adds the sensed force to the gravity model instead of cancelling it, and -integrates at 2 g (#296). +`cl` corrects the inertial solution with GNSS position and velocity, barometric altitude and +magnetometer heading. Its default filter is the error-state Kalman filter. As `strapdown-sim` +configures it, that filter carries **16 error states**: the 15 of position, velocity, +attitude, accelerometer bias and gyroscope bias, plus a barometric altitude bias. The last is +on by default and `--no-estimate-baro-bias` removes it. [State +Representation](./user-guide/state-representation.md) lays the states out. + +The log begins: + +```text +2026-10-01 20:39:38.048 [INFO] - Running in closed-loop mode with Error-State Kalman Filter (ESKF) +2026-10-01 20:39:38.048 [INFO] - Processing file: synthetic.csv +2026-10-01 20:39:38.076 [INFO] - Read 6000 records from synthetic.csv +2026-10-01 20:39:38.076 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +2026-10-01 20:39:38.077 [INFO] - Initialized event stream with 13198 events +2026-10-01 20:39:38.077 [INFO] - Initialized ESKF +2026-10-01 20:39:38.077 [INFO] - Starting closed-loop navigation filter with 13198 events +``` -### 3. Run a Simulation +followed by a progress line every ten events and, at the end, `Results written to eskf.csv`. +(One line, `Using aiding config: ...`, is omitted above for width.) The progress lines run to +over a thousand on this file. `--log-level warn` silences them, and the commands below use it: -**Dead Reckoning** (`dr`; `ol` is a different, unimplemented mode): ```bash -strapdown-sim dr -i data/input.csv -o results/output.csv +strapdown-sim --log-level warn cl -i synthetic.csv -o eskf.csv +``` + +### What came out + +Every mode writes the same CSV layout. The first ten columns are the navigation solution; the +last row of `eskf.csv` is: + +```text +timestamp,latitude,longitude,altitude,velocity_north,velocity_east,velocity_vertical,roll,pitch,yaw +2025-01-01T00:09:59.900Z,-2.097736146341953e-6,-1.9488296989009557e-6,-0.6980524721961896,-0.06949807183747997,-0.08759779166041143,0.05404302021868538,-0.00384387940069942,-0.0007849300822555636,0.005312090469945534 ``` -**Closed-Loop (16-state ESKF, the default)**: +Latitude and longitude are in degrees, altitude in metres (positive up), velocities in m/s and +the angles in radians. `velocity_vertical` follows the frame: positive **down** here, because +the run is NED. The remaining 30 columns are the IMU bias estimates, the covariance, and the +optional bias states (`gravity_bias`, `magnetic_bias`, `baro_bias` and their variances). The +map-bias cells are empty on this run because it carried no maps. [Output +Format](./user-guide/output-format.md) lists them all. + +### The same seed gives the same file + +Runs are deterministic. Running the same command again produces a byte-identical file: + +```console +$ strapdown-sim --log-level warn cl -i synthetic.csv -o eskf_again.csv +$ cmp eskf.csv eskf_again.csv && echo identical +identical +``` + +## 3. The other Kalman filters + ```bash -strapdown-sim cl -i data/input.csv -o results/output.csv +strapdown-sim --log-level warn cl -i synthetic.csv -o ekf.csv --filter ekf +strapdown-sim --log-level warn cl -i synthetic.csv -o ukf.csv --filter ukf ``` -**Closed-Loop with EKF**: +The EKF and UKF carry the full navigation state rather than an error state, with the same 15 +elements plus the barometric bias. See [Kalman Filters](./filters/kalman.md). + +## 4. Dead reckoning + ```bash -strapdown-sim cl -i data/input.csv -o results/output.csv --filter ekf +strapdown-sim --log-level warn dr -i synthetic.csv -o dr.csv ``` -**Closed-Loop with UKF**: +`dr` propagates the IMU from the first record with no aiding at all, so its error grows without +bound. (`ol` is a different subcommand, reserved for an open-loop mode that is **not +implemented**: it writes no output. Use `dr` for dead reckoning.) + +## 5. The particle filter + ```bash -strapdown-sim cl -i data/input.csv -o results/output.csv --filter ukf +strapdown-sim --log-level warn pf -i synthetic.csv -o pf.csv ``` -**Particle Filter**: +`pf` runs the Rao-Blackwellized particle filter (100 particles by default, `--num-particles` +to change it). Its output has the same columns; its six IMU-bias columns are zero because the +filter does not estimate IMU biases. See [Rao-Blackwellized Particle Filter](./filters/rbpf.md). + +## 6. A GNSS outage + ```bash -strapdown-sim pf -i data/input.csv -o results/output.csv --num-particles 100 +strapdown-sim --log-level warn cl -i synthetic.csv -o eskf_outage.csv \ + --sched duty --on-s 100 --off-s 50 ``` -### 4. View Results +The duty-cycle scheduler withholds GNSS fixes in 50 s windows separated by 100 s of normal +service. With the default `--duty-phase-s 0` the cycle **starts with an outage**: fixes are +withheld over 0-50 s, 150-200 s, 300-350 s and 450-500 s. At the default log level the +difference is visible in the event count: 11199 events here against 13198 for the +uninterrupted run, which is 200 s of 10 Hz GNSS fixes. The barometer and magnetometer are +scheduled separately and keep arriving. [Fault Simulation](./gnss/fault-simulation.md) covers +the other schedulers and the fault models (`--fault degraded|slowbias|hijack`). + +## 7. Score the runs -The output CSV contains the estimated navigation state: +`syn --no-noise` writes the truth for the same trajectory, in the output format: -```csv -timestamp,latitude,longitude,altitude,velocity_north,velocity_east,velocity_down,roll,pitch,yaw -1234567890.0,45.50001,-122.59999,100.1,1.2,0.5,-0.1,0.01,0.02,90.5 -... +```bash +strapdown-sim syn -o truth.csv --duration-s 600 --seed 42 --no-noise ``` -## Using Configuration Files +This short Python script (standard library only) compares each run's horizontal position with +the truth, row by row: + +```python +# score.py +import csv, math, sys + +R = 6_371_000.0 # mean Earth radius, m; adequate for metre-level errors + -For more complex scenarios, use TOML configuration files: +def load(path): + with open(path) as f: + return list(csv.DictReader(f)) + + +truth = load("truth.csv") +for path in sys.argv[1:]: + est = load(path) + errors = [] + for t, e in zip(truth, est): + d_north = math.radians(float(e["latitude"]) - float(t["latitude"])) * R + d_east = (math.radians(float(e["longitude"]) - float(t["longitude"])) + * R * math.cos(math.radians(float(t["latitude"])))) + errors.append(math.hypot(d_north, d_east)) + rms = math.sqrt(sum(x * x for x in errors) / len(errors)) + print(f"{path:16s} horizontal RMS {rms:10.3f} m final {errors[-1]:10.3f} m") +``` + +```console +$ python3 score.py dr.csv eskf.csv ekf.csv ukf.csv pf.csv eskf_outage.csv +dr.csv horizontal RMS 41518.686 m final 109415.047 m +eskf.csv horizontal RMS 0.739 m final 0.318 m +ekf.csv horizontal RMS 0.737 m final 0.318 m +ukf.csv horizontal RMS 0.737 m final 0.318 m +pf.csv horizontal RMS 1.006 m final 0.667 m +eskf_outage.csv horizontal RMS 18.347 m final 0.318 m +``` + +Read these as a demonstration of the workflow, not as a comparison of the filters: they come +from one stationary trajectory and one seed. Unaided, the consumer-grade IMU drifts by about +110 km in ten minutes. Every aided run stays at the metre level. The outage run's final error +matches the uninterrupted run's because the last 100 s have GNSS again, while its RMS carries +the four outages. The filters' measured accuracy on the reference scenarios is on the +[Performance Baselines](./development/performance.md) page. + +## 8. The same run from a scenario file + +Everything above can be written into one TOML, YAML or JSON file, which is the reproducible +record of an experiment. This is the outage run of step 6: ```toml -# config.toml -- a whole `SimulationConfig`. `mode` is the only required field. -mode = "closed-loop" # dead-reckoning | open-loop | closed-loop | particle-filter | synthetic -input = "data/trajectory.csv" -output = "results/navigation.csv" +# outage.toml: the duty-cycle run above, as a scenario file +mode = "closed-loop" +input = "synthetic.csv" +output = "eskf_outage_from_config.csv" seed = 42 -is_enu = false # NED is the default; Sensor Logger exports need `true` [logging] -level = "info" +level = "warn" [closed_loop] -filter = "eskf" # eskf (default) | ukf | ekf - -[aiding] -seed = 42 +filter = "eskf" -[aiding.scheduler] # when GNSS fixes arrive -kind = "fixed_interval" -interval_s = 5.0 -phase_s = 0.0 +[aiding.scheduler] +kind = "duty_cycle" +on_s = 100.0 +off_s = 50.0 +start_phase_s = 0.0 -[aiding.fault] # how they are corrupted +[aiding.fault] kind = "none" ``` -Run it with the config alone — `--config` supplies the whole run, so a subcommand and its -`-i`/`-o`/`--filter` arguments are ignored when it is present: - -```bash -strapdown-sim --config config.toml +```console +$ strapdown-sim --config outage.toml +$ cmp eskf_outage.csv eskf_outage_from_config.csv && echo identical +identical ``` -`core/tests/example_configs.rs` parses every file under `examples/configs/` and asserts it -deserializes into the scenario it describes, because every field has a default: a misspelled -section is dropped silently and the run simulates nothing. - -Run with: -```bash -strapdown-sim --config config.toml -``` +`--config` supplies the whole run, so no subcommand is given. The `[aiding]` section is also +accepted under its older name, `[gnss_degradation]`. Every field has a default, so a misspelled +section name is not an error: it is silently ignored and the run uses defaults. Check a new +file against the keys in [Configuration Files](./user-guide/configuration.md). -## Next Steps +## Next steps -- Read the [User Guide](./user-guide/overview.md) for detailed usage instructions -- Explore [Example Configurations](./examples/configurations.md) -- Learn about [Navigation Filters](./filters/kalman.md) -- Check the [API Reference](./api/core.md) for programmatic usage +- [User Guide Overview](./user-guide/overview.md): the crates, subcommands and filters, and + where each is documented. +- [The Navigation Model](./user-guide/concepts.md): what the mechanization computes. +- [Example Configurations](./examples/configurations.md): scenario files for every scheduler + and fault. +- [Using the Library](./user-guide/library.md): the same machinery from Rust. diff --git a/book/src/resources/glossary.md b/book/src/resources/glossary.md index ca3c8276..cc6112f5 100644 --- a/book/src/resources/glossary.md +++ b/book/src/resources/glossary.md @@ -1 +1,73 @@ # Glossary + +- **ARW / VRW** (angle / velocity random walk): The integrated effect of white noise on a + gyroscope or accelerometer. Integrating white angular-rate noise gives an attitude error whose + standard deviation grows with the square root of time; ARW is that growth, in rad/√s (often + quoted in deg/√h). VRW is the same for velocity, in m/s/√s. The RBPF's velocity and attitude + process noise are VRW and ARW. +- **CEP** (circular error probable): The radius of a circle, centred on truth, containing a + given share of horizontal position errors. CEP50 contains half of them, CEP95 95%. + `strapdown::metrics` computes the empirical (nearest-rank) value from the errors, not the + Rayleigh approximation. +- **DCM** (direction cosine matrix): The 3×3 rotation matrix between two frames, here body to + navigation frame. The attitude representation the mechanization propagates; roll, pitch and + yaw are derived from it. +- **EKF** (extended Kalman filter): A Kalman filter for a nonlinear system that propagates the + full state through the nonlinear model and the covariance through its Jacobians. See + [EKF](../filters/ekf.md). +- **ENU / NED**: The two local-level frame conventions: East-North-Up and North-East-Down. NED + is this crate's default; ENU is opt-in with `--enu` or `is_enu`. Altitude is positive up in + both; vertical velocity is positive down in NED and up in ENU. See [Coordinate + Frames](../user-guide/coordinate-frames.md). +- **ESKF** (error-state Kalman filter): A Kalman filter that estimates the *error* in a + separately propagated nominal solution, folds the estimate back in after each update, and + resets. This crate's ESKF carries a multiplicative attitude error and is the closed-loop + default. See [ESKF](../filters/eskf.md). +- **Gauss-Markov process** (first order): A random process whose autocorrelation decays + exponentially, $R(\Delta t) = \sigma^2 e^{-|\Delta t|/\tau}$, with steady-state standard + deviation $\sigma$ and correlation time $\tau$. In discrete time, an AR(1) process with $\rho + = e^{-\Delta t/\tau}$. Used for correlated GNSS errors (`degraded` with `tau_pos_s`) and for + map-bias variation. +- **GNSS** (global navigation satellite system): GPS, Galileo, GLONASS, BeiDou and the like. + Here, the source of the position and velocity fixes that aid the INS, and the thing the + degradation scenarios take away. +- **IMU** (inertial measurement unit): An accelerometer triad and a gyroscope triad measuring + specific force and angular rate in the body frame. +- **INS** (inertial navigation system): An IMU plus the computation that integrates its output + into position, velocity and attitude. +- **Lever arm**: The vector from the IMU to another sensor on the same body, typically the GNSS + antenna. A rotating vehicle moves the two points differently, so a fix is translated through + the lever arm before it is compared with the INS solution. `InsEngine` applies this + compensation. +- **Loosely coupled**: An integration that aids the INS with the GNSS receiver's *solution* + (position and velocity) rather than with its raw pseudorange and carrier-phase measurements + (tightly coupled). This crate is loosely coupled only. +- **MEMS** (micro-electro-mechanical systems): The silicon sensor technology of consumer and + low-cost IMUs, such as the one in a phone. Cheap and small, with biases and noise far larger + than navigation-grade sensors. +- **NEES / NIS** (normalized estimation error squared / normalized innovation squared): + Consistency statistics. NEES is $e^\top P^{-1} e$ for the true estimation error $e$ and the + filter's covariance $P$, so it needs truth; its expected value is the dimension of $e$. NIS is + $\nu^\top S^{-1} \nu$ for the innovation $\nu$ and its predicted covariance $S$, so it needs + no truth and can be computed online; innovation gating thresholds it against a chi-squared + quantile. +- **PNT** (positioning, navigation and timing): The services GNSS provides, and the usual name + for the field of providing them by other means when GNSS is unavailable. +- **RBPF** (Rao-Blackwellized particle filter): A particle filter that samples only the strongly + nonlinear part of the state (here, horizontal position) and handles the rest with a Kalman + filter conditioned on each sample. The crate's only particle filter. See + [RBPF](../filters/rbpf.md). +- **Specific force**: What an accelerometer measures: the non-gravitational force per unit mass, + that is, acceleration minus gravitational acceleration. At rest it points up with magnitude + $g$. The mechanization adds the gravity model back. +- **Strapdown**: An INS whose sensors are fixed ("strapped down") to the vehicle body, so that + attitude is computed by integrating the gyroscopes rather than maintained by a gimballed + platform. +- **UKF** (unscented Kalman filter): A Kalman filter that propagates a set of deterministically + chosen sigma points through the nonlinear model instead of linearizing it. See + [UKF](../filters/ukf.md). +- **ZUPT / ZARU** (zero-velocity update / zero angular-rate update): Pseudo-measurements applied + while the vehicle is known to be stationary: velocity is zero, and the gyroscopes should read + only Earth rotation. `strapdown::stationary` detects such periods and + `strapdown::measurements` provides both models. + diff --git a/book/src/resources/publications.md b/book/src/resources/publications.md index 5d202aee..f7f33069 100644 --- a/book/src/resources/publications.md +++ b/book/src/resources/publications.md @@ -1 +1,53 @@ -# Publications +# Publications and Links + +## Papers about this software + +- **J. Brodovsky**, "strapdown-rs: Strapdown inertial navigation and GNSS-degradation + simulation in Rust." Submitted to the *Journal of Open Source Software*; under review at + [openjournals/joss-reviews#11377](https://github.com/openjournals/joss-reviews/issues/11377). + The source is in `papers/joss/` in the repository. Until it is published, cite the software + through the repository's `CITATION.cff`. + +## Papers that use it + +The geophysical-navigation work that `strapdown-geonav` supports (see +[Geophysical Navigation](../geonav/overview.md)): + +- J. Brodovsky and P. Dames, "Navigation in GNSS-denied environments using MEMS-grade sensors + and geophysical anomalies: A UKF approach," in *Proceedings of the 2026 International + Technical Meeting of the Institute of Navigation*, Anaheim, California, Jan. 2026, + pp. 155–164. [doi:10.33012/2026.20508](https://doi.org/10.33012/2026.20508) +- J. Brodovsky and P. Dames, "Navigation in GNSS-Denied Environments Using MEMS-Grade Sensors + and Geophysical Anomalies: A Particle Filter Approach," in *Proceedings of the ION 2026 + Pacific PNT Meeting*, Honolulu, Hawaii, Apr. 2026, pp. 399–410. + [doi:10.33012/2026.20618](https://doi.org/10.33012/2026.20618) + +## References the implementation follows + +- P. D. Groves, *Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems*, + 2nd ed. Boston: Artech House, 2013. ISBN 978-1-60807-005-3. The mechanization, the Earth + model and the filters cite it by section and equation number throughout. +- A. Canciani and J. Raquet, "Airborne Magnetic Anomaly Navigation," *IEEE Transactions on + Aerospace and Electronic Systems*, vol. 53, no. 1, pp. 67–80, 2017. + [doi:10.1109/TAES.2017.2649238](https://doi.org/10.1109/TAES.2017.2649238). The design of + the Rao-Blackwellized particle filter; see [Rao-Blackwellized Particle Filter](../filters/rbpf.md). +- National Geospatial-Intelligence Agency, *Department of Defense World Geodetic System 1984: + Its Definition and Relationships with Local Geodetic Systems*, NGA.STND.0036_1.0.0_WGS84, + 2014. The ellipsoid in `strapdown::earth`. +- NOAA NCEI Geomagnetic Modeling Team and British Geological Survey, *World Magnetic Model + 2025*, 2024. [doi:10.25921/aqfd-sd83](https://doi.org/10.25921/aqfd-sd83). The reference + field for magnetometer heading and magnetic anomalies. + +## Links + +| | | +|---|---| +| Source repository | | +| This book | | +| API documentation | [published with this book](../api/index.md); on docs.rs as [strapdown-core](https://docs.rs/strapdown-core) and [strapdown-geonav](https://docs.rs/strapdown-geonav) once v1.0.0 is on crates.io | +| Issue tracker | | +| Sensor Logger (the app the input format comes from) | | +| World Magnetic Model | | +| `world_magnetic_model` crate (the WMM implementation used) | | +| `nav-types` crate (geodetic and ECEF types used by `earth`) | | +| GMT remote datasets (the source of the gravity and magnetic maps) | | diff --git a/book/src/user-guide/closed-loop.md b/book/src/user-guide/closed-loop.md index b7f35bae..e46e0618 100644 --- a/book/src/user-guide/closed-loop.md +++ b/book/src/user-guide/closed-loop.md @@ -1 +1,236 @@ -# Closed-Loop Mode +# Closed Loop (Kalman Filters) + +`strapdown-sim cl` runs the INS with a Kalman filter that corrects it from aiding measurements, +feeding each correction back into the navigation solution. It is the mode for GNSS-aided +navigation and for studying what happens when the GNSS is withheld or corrupted. + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim cl -i synthetic.csv -o results/eskf.csv +``` + +```console +[INFO] - Running in closed-loop mode with Error-State Kalman Filter (ESKF) +[INFO] - Read 6000 records from synthetic.csv +[INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +[INFO] - Initialized event stream with 13198 events +[INFO] - Initialized ESKF +[INFO] - Starting closed-loop navigation filter with 13198 events +... +[INFO] - Results written to results/eskf.csv +``` + +(Timestamps trimmed; at `info` the filter also logs a progress line every few events, which +`--log-level warn` silences.) The shared flags -- `-i`, `-o`, `--enu`, the execution and health +limits -- are described in [Running Simulations](./simulations.md). This page covers the flags +specific to `cl`. + +## Choosing the filter + +| Flag | Default | Values | +|---|---|---| +| `--filter ` | `eskf` | `eskf`, `ukf`, `ekf` | + +- **`eskf`** -- the 15-state error-state Kalman filter with a multiplicative attitude error. The + default. See [ESKF](../filters/eskf.md). +- **`ekf`** -- the extended Kalman filter. See [EKF](../filters/ekf.md). +- **`ukf`** -- the unscented Kalman filter. See [UKF](../filters/ukf.md). + +All three estimate accelerometer and gyroscope biases (the `acc_bias_*` and `gyro_bias_*` output +columns are populated for each) and, by default, a barometric bias. How they compare is on the +[Comparison](../filters/comparison.md) page. + +```bash +strapdown-sim cl -i synthetic.csv -o results/ukf.csv --filter ukf +strapdown-sim cl -i synthetic.csv -o results/ekf.csv --filter ekf +``` + +### UKF sigma-point parameters + +Read only with `--filter ukf`. + +| Flag | Default | Meaning | +|---|---|---| +| `--ukf-alpha ` | `0.1` | sigma-point spread | +| `--ukf-beta ` | `2` | prior-distribution parameter; 2 is optimal for a Gaussian | +| `--ukf-kappa ` | `0` | secondary spread parameter | + +`--ukf-alpha` defaults to `0.1`, the same value a configuration file uses, not the textbook +`1e-3`. At `1e-3` the weighted sigma-point mean is formed from terms about $10^6$ times the +answer, which costs six significant digits to cancellation on every step; the measurement behind +the choice is recorded on `ClosedLoopConfig::ukf_alpha` in the API documentation. + +## What aids the filter + +`cl` builds its measurements from the input records. Which columns feed which measurement is +covered in [Input Data Format](./data-format.md#which-columns-drive-what); in summary: + +| Measurement | Built from | Rate | Noise | +|---|---|---|---| +| GNSS position and velocity | `latitude`, `longitude`, `altitude`, `speed`, `bearing` | every record that carries all five, subject to `--sched` | the record's `horizontalAccuracy`, `verticalAccuracy`, `speedAccuracy`, then `--fault` | +| Barometric altitude | `relativeAltitude`, referenced to the first record's altitude | 1 Hz | 2.236 m one-sigma | +| Magnetometer heading | `mag_x`, `mag_y`, `mag_z`, with declination from the World Magnetic Model at the record's date | 1 Hz | 0.2 rad one-sigma | + +The GNSS channel is the one the command line can degrade. The barometer and magnetometer +schedules, and the barometer's noise, have **no command-line flags**: they take the defaults +above, and a configuration file can change them (`aiding.baro_scheduler`, +`aiding.magnetometer_scheduler`, `aiding.baro_noise_std_m`; see +[Configuration Files](./configuration.md#the-barometer-and-magnetometer)). Neither is ever +corrupted; fault models apply to GNSS only. + +### The barometric bias state + +A barometer converts pressure to altitude against a reference pressure that drifts, and a filter +that models the reading as unbiased has nowhere to put that drift except into altitude. `cl` +therefore estimates a barometric bias as an extra filter state, **on by default**, and writes it +to the `baro_bias` and `baro_bias_cov` output columns. + +| Flag | Effect | +|---|---| +| `--no-estimate-baro-bias` | do not carry the barometric bias state; the two output columns are left empty | + +The flag is spelled negatively because the default is on. The old `--estimate-baro-bias` was +removed rather than kept as a no-op, so a script still passing it fails with clap's "unexpected +argument" error instead of silently meaning nothing. The measurements behind the default, and the +one case where the state hurts (an AR(1)-degraded GNSS fault, issue #410), are on the +[Configuration Files](./configuration.md#the-barometric-bias) page. + +## GNSS scheduling: when fixes arrive + +The scheduler decides which GNSS fixes reach the filter at all. A fix the scheduler withholds is +simply absent; the filter coasts on the IMU, still aided by the barometer and magnetometer. + +| Flag | Default | Used by | Meaning | +|---|---|---|---| +| `--sched ` | `passthrough` | -- | `passthrough`, `fixed` or `duty` | +| `--interval-s ` | `1` | `fixed` | seconds between delivered fixes | +| `--phase-s ` | `0` | `fixed` | offset of the first delivered fix | +| `--on-s ` | `10` | `duty` | length of each available window | +| `--off-s ` | `10` | `duty` | length of each denied window | +| `--duty-phase-s ` | `0` | `duty` | length of an initial available window before the cycle starts | + +- **`passthrough`** delivers every fix. +- **`fixed`** delivers at most one fix per interval: the first usable fix at or after each tick. + It models a reduced update rate. +- **`duty`** alternates denial and availability. The timeline is `--duty-phase-s` of + availability, then `--off-s` denied and `--on-s` available, repeating. **With the default + phase of 0 the run therefore starts in an outage**: `--sched duty --on-s 100 --off-s 50` denies + GNSS for the first 50 s, delivers it from 50 s to 150 s, denies it from 150 s to 200 s, and so + on. Set `--duty-phase-s` to start with GNSS available. + +```bash +strapdown-sim cl -i synthetic.csv -o results/fixed.csv --sched fixed --interval-s 5 +strapdown-sim cl -i synthetic.csv -o results/duty.csv --sched duty --on-s 100 --off-s 50 +``` + +On the 600 s, 10 Hz trajectory above, the pass-through run's event stream holds 13,198 events +and the duty-cycle run's 11,199 -- the difference is the 1,999 GNSS fixes the two outages +withhold, as `Initialized event stream with ... events` reports at `info` level. + +## GNSS faults: what the fixes say + +A fault model corrupts the content of each fix the scheduler lets through. Scheduler and fault +compose: a duty cycle can deliver degraded fixes during its available windows. + +| Flag | Default | Meaning | +|---|---|---| +| `--fault ` | `none` | `none`, `degraded`, `slowbias` or `hijack` | + +**`degraded`** adds AR(1)-correlated error to position and velocity and inflates the accuracy the +fix advertises. It models low signal-to-noise or multipath. + +| Flag | Default | Meaning | +|---|---|---| +| `--rho-pos` | `0.99` | AR(1) correlation coefficient of the position error, per fix | +| `--sigma-pos-m` | `3` | position error, metres: the per-fix innovation, or the steady-state sigma with `--tau-pos-s` | +| `--rho-vel` | `0.95` | AR(1) correlation coefficient of the velocity error, per fix | +| `--sigma-vel-mps` | `0.3` | velocity error, m/s, read the same way as `--sigma-pos-m` | +| `--r-scale` | `5` | factor on the advertised one-sigma accuracies, so $R$ grows by its **square** (25 by default) | +| `--tau-pos-s` | unset | position-error correlation time, seconds | +| `--tau-vel-s` | unset | velocity-error correlation time, seconds | + +Without `--tau-pos-s`, `--rho-pos` is applied once per delivered fix, so the error's correlation +time is $-\Delta t / \ln \rho$ with $\Delta t$ the fix interval: changing `--interval-s` changes +the error model too. With `--tau-pos-s`, the coefficient is $e^{-\Delta t/\tau}$ for the actual +interval between fixes and `--sigma-pos-m` becomes the steady-state standard deviation, so the +error keeps the same timescale whatever the schedule. `--tau-vel-s` does the same for velocity. +Leave both unset to reproduce results made before they existed. + +**`slowbias`** adds a north/east offset that grows steadily, so each fix looks plausible while +the trajectory is nudged away. It models a soft spoof. + +| Flag | Default | Meaning | +|---|---|---| +| `--drift-n-mps` | `0.02` | northward drift rate of the offset, m/s | +| `--drift-e-mps` | `0` | eastward drift rate of the offset, m/s | +| `--q-bias` | `1e-6` | random-walk density of the offset, m²/s: each step adds variance `q_bias * dt` | +| `--rotate-omega-rps` | `0` | rate at which the drift direction rotates, rad/s | + +**`hijack`** applies a constant north/east offset over a fixed window. It models a hard spoof. + +| Flag | Default | Meaning | +|---|---|---| +| `--hijack-offset-n-m` | `50` | northward offset, metres | +| `--hijack-offset-e-m` | `0` | eastward offset, metres | +| `--hijack-start-s` | `120` | window start, seconds from the first record | +| `--hijack-duration-s` | `60` | window length, seconds | + +```bash +strapdown-sim cl -i synthetic.csv -o results/degraded.csv \ + --fault degraded --sigma-pos-m 10 --tau-pos-s 60 --tau-vel-s 30 +strapdown-sim cl -i synthetic.csv -o results/hijack.csv \ + --fault hijack --hijack-offset-n-m 100 --hijack-start-s 200 --hijack-duration-s 60 +strapdown-sim cl -i synthetic.csv -o results/both.csv \ + --sched duty --on-s 100 --off-s 50 --fault degraded --sigma-pos-m 10.0 --seed 7 +``` + +Only one fault can be chosen on the command line. Several applied in sequence -- a slow drift +followed by a hijack, say -- is the `combo` fault, which exists only in a configuration file; see +[Configuration Files](./configuration.md#fault-models). Every random draw a fault makes comes from +`--seed` (default `42`), so the same seed and flags give the same corrupted fixes. The +[GNSS Degradation](../gnss/fault-simulation.md) chapter describes the models in more depth. + +## Innovation gating + +By default every measurement is applied. An innovation gate rejects a measurement whose +normalized innovation squared (NIS) is implausibly large for the filter's own covariance. + +| Flag | Default | Meaning | +|---|---|---| +| `--gate-confidence

` | unset (no gate) | reject a measurement whose NIS exceeds the $\chi^2$ quantile at probability `P`, for that measurement's own degrees of freedom. Must lie strictly inside (0, 1) | +| `--gate-inflation ` | `2` | each rejection multiplies the filter's uncertainty by this factor, in the directions the rejected measurement observed. At least 1.0; 1.0 disables it | +| `--gate-force-after ` | `5` | after this many consecutive rejections, apply the next measurement regardless. `0` disables it; `1` is refused, because it would be no gate at all | + +Because the threshold is the $\chi^2$ quantile at each measurement's own dimension, one +confidence level means the same thing for a 1-DOF barometer reading and a multi-DOF GNSS fix. +The two recovery flags exist because a gate with no way back is self-reinforcing: a filter that +rejects one fix keeps drifting while the covariance it judges the next one against stays put, so +one rejection leads to the next. The recovery flags are validated even when no gate is +installed. + +```console +$ strapdown-sim cl -i synthetic.csv -o results/gated.csv --gate-confidence 0.999 --log-level warn +[WARN] - closed-loop run completed with 78 of 13198 events gated out by the innovation test +$ strapdown-sim cl -i synthetic.csv -o results/bad.csv --gate-confidence 1.5 +Error: OutOfRange { what: "innovation gate confidence", value: 1.5, min: 0.0, max: 1.0 } +``` + +The gate is distinct from the run-level `--nis-pos-max`/`--nis-pos-consec-fail` health check, +which does not reject anything: it fails the whole run when outliers persist. See +[Measurement Models and Integrity](../filters/measurements.md) for the gate's theory. + +## Seeds and reproducibility + +| Flag | Default | Seeds | +|---|---|---| +| `--seed ` | `42` | the GNSS fault models | + +With `--fault none` nothing in a `cl` run is random, and the output depends only on the input +and the flags. + +## Geophysical aiding (experimental) + +A binary built with `--features geonav` adds `--geo` and the `--gravity-*`, `--magnetic-*` and +`--geo-interval-s` flags, which aid the UKF or EKF with gravity- or magnetic-anomaly +measurements against a map. The ESKF has no geophysical implementation, so `--geo` requires +`--filter ukf` or `--filter ekf`. See [Geophysical Navigation](../geonav/overview.md). diff --git a/book/src/user-guide/concepts.md b/book/src/user-guide/concepts.md index c587b444..ba54dcfd 100644 --- a/book/src/user-guide/concepts.md +++ b/book/src/user-guide/concepts.md @@ -1 +1,255 @@ -# Core Concepts +# The Navigation Model + +This page describes what `strapdown-core` computes: the strapdown mechanization that turns IMU +samples into position, velocity and attitude, the Earth model it relies on, and how aiding +measurements correct it. The equations are those of Groves, *Principles of GNSS, Inertial, and +Multisensor Integrated Navigation Systems*, 2nd ed., §5.4-5.5, and they are written here as the +code implements them (`mechanize`, `attitude_update`, `position_update` and the private +`velocity_update_ned` in `core/src/lib.rs`). + +## Strapdown mechanization + +In a *strapdown* system the inertial sensors are fixed rigidly to the vehicle, so they measure +in the vehicle's own **body frame** ($b$): + +- the gyroscopes measure the **angular rate** $\boldsymbol\omega_{ib}^b$ of the body relative + to inertial space; +- the accelerometers measure the **specific force** $\mathbf f_{ib}^b$: the non-gravitational + force per unit mass acting on the body. This is not acceleration. An accelerometer at rest + on a table senses the table pushing up against gravity; one in free fall senses nothing. + +*Mechanization* integrates these measurements forward in time. The attitude comes from the +angular rate, the velocity from the specific force once it is resolved into the navigation +frame and gravity is restored, and the position from the velocity. No external information is +used, so any error in the measurements or the initial state is integrated too, and the +solution drifts without bound. That is why the filters exist. The [Quick +Start](../quick-start.md) shows the effect: unaided, its consumer-grade synthetic IMU drifts +about 110 km in ten minutes, while every aided run stays at the metre level. + +## The navigation state + +The mechanization works in the **local-level navigation frame** ($n$): north, east and down +(NED) axes attached to the vehicle's position on the WGS84 ellipsoid. It propagates: + +| Symbol | Quantity | Units in the library | +| --- | --- | --- | +| $L$, $\lambda$ | Geodetic latitude and longitude | radians | +| $h$ | Height above the ellipsoid, positive up | m | +| $\mathbf v_{eb}^n = (v_N, v_E, v_D)$ | Velocity relative to the Earth, resolved in $n$ | m/s | +| $\mathbf C_b^n$ | Attitude: the direction cosine matrix from body to navigation axes | -- | + +This is the library's `StrapdownState`. [Coordinate Frames](./coordinate-frames.md) covers the +frame conventions, including the ENU option, and [State +Representation](./state-representation.md) covers how the filters extend this state. + +## Inputs: increments over an interval + +The mechanization consumes an `ImuSample`: the integrated specific force +$\Delta\mathbf v^b = \int \mathbf f_{ib}^b dt$, the integrated angular rate +$\Delta\boldsymbol\theta^b = \int \boldsymbol\omega_{ib}^b dt$, and the interval $\tau$ they +were accumulated over. A CSV record carries instantaneous rates, which `ImuSample::from_rates` +turns into increments by rectangular integration: $\Delta\mathbf v^b = \mathbf f_{ib}^b \tau$, +$\Delta\boldsymbol\theta^b = \boldsymbol\omega_{ib}^b \tau$. That is exact only for rates +constant across the interval. No coning or sculling compensation is applied. + +A filter that estimates IMU biases subtracts its current estimate, integrated over $\tau$, +from both increments before mechanizing. + +## The Earth model + +Three Earth quantities appear in the update equations (Groves, Chapter 2; `core/src/earth.rs`). + +**Radii of curvature.** With equatorial radius $a = 6378137$ m and eccentricity +$e = 0.0818191908425$, the meridian (north-south) and transverse (east-west) radii are + +$$ +R_N(L) = \frac{a (1 - e^2)}{\left(1 - e^2 \sin^2 L\right)^{3/2}}, \qquad +R_E(L) = \frac{a}{\sqrt{1 - e^2 \sin^2 L}} . +$$ + +**Earth rate.** The Earth turns at $\omega_{ie} = 7.2921159 \times 10^{-5}$ rad/s +(`earth::RATE`). Resolved in NED axes: + +$$ +\boldsymbol\omega_{ie}^n = \omega_{ie} \left(\cos L, 0, -\sin L\right)^\top . +$$ + +**Transport rate.** Moving over the curved Earth rotates the local-level frame itself, at +(Groves eq. 5.44) + +$$ +\boldsymbol\omega_{en}^n = \left( \frac{v_E}{R_E(L) + h}, \quad \frac{-v_N}{R_N(L) + h}, \quad \frac{-v_E \tan L}{R_E(L) + h} \right)^\top . +$$ + +Only the horizontal velocity enters it. Each rate has a skew-symmetric matrix form, +$\boldsymbol\Omega = [\boldsymbol\omega \times]$, so that $\boldsymbol\Omega \mathbf x = +\boldsymbol\omega \times \mathbf x$. + +**Gravity.** `earth::gravity` gives the magnitude of normal gravity from Somigliana's formula, +reduced with height by the free-air gradient: + +$$ +g(L, h) = g_e \frac{1 + k \sin^2 L}{\sqrt{1 - e^2 \sin^2 L}} - 3.08 \times 10^{-6} h, +\qquad k = \frac{b g_p - a g_e}{a g_e}, +$$ + +with $g_e = 9.7803253359$ m/s² at the equator, $g_p = 9.8321849378$ m/s² at the poles, and +polar radius $b = 6356752.31425$ m. Somigliana's formula describes *normal gravity*: the +gravitational attraction combined with the centrifugal effect of the Earth's rotation, which +is the $\mathbf g_b^n$ the velocity update needs. It acts along the ellipsoid normal, so in NED +it is $\mathbf g^n = (0, 0, g)$: positive down. At 0° latitude and zero height it is +9.780 m/s², the "local gravity" the simulator logs when it checks the input frame. + +## The update equations + +One mechanization step takes the state at the start of an interval, marked $(-)$, to its end, +marked $(+)$. The steps run in this order because each one uses the result of the previous one. + +### Attitude (Groves eq. 5.46) + +$$ +\mathbf C_b^n(+) \approx \mathbf C_b^n(-)\left(\mathbf I_3 + [\Delta\boldsymbol\theta^b \times]\right) - \left(\boldsymbol\Omega_{ie}^n + \boldsymbol\Omega_{en}^n\right) \mathbf C_b^n(-) \tau +$$ + +The first term applies the body's sensed rotation, $[\Delta\boldsymbol\theta^b\times] = +\boldsymbol\Omega_{ib}^b \tau$. The second removes the part of that rotation that is only the +navigation frame turning underneath it: the Earth's rotation, plus the transport rate. Both +rates are evaluated at the $(-)$ state. The first-order result is not exactly orthonormal, so +it is projected back onto a rotation matrix before it is stored. + +### Specific force resolution (Groves eq. 5.47) + +$$ +\Delta\mathbf v^n = \tfrac{1}{2}\left(\mathbf C_b^n(-) + \mathbf C_b^n(+)\right)\Delta\mathbf v^b +$$ + +Averaging the attitude over the interval is the mechanization's one second-order term. + +### Velocity (Groves eq. 5.54) + +$$ +\mathbf v_{eb}^n(+) \approx \mathbf v_{eb}^n(-) + \Delta\mathbf v^n + \left[\mathbf g^n - \left(\boldsymbol\Omega_{en}^n + 2 \boldsymbol\Omega_{ie}^n\right)\mathbf v_{eb}^n(-)\right]\tau +$$ + +The sensed increment is added directly; gravity and the Coriolis and transport-rate terms +accumulate over the interval. The last term is a plain cross product of the local-level rates +with the local-level velocity, $(\boldsymbol\omega_{en}^n + 2\boldsymbol\omega_{ie}^n) \times +\mathbf v_{eb}^n$; no frame transformation is applied to it. The unit test +`velocity_update_coriolis_term_is_a_bare_cross_product` checks it against an explicit cross +product at 45° N. + +### Position (Groves eq. 5.56) + +Height, latitude and longitude are updated in that order, each from the trapezoidal average of +the old and new velocity: + +$$ +h(+) = h(-) - \tfrac{1}{2}\left(v_D(-) + v_D(+)\right)\tau +$$ + +$$ +L(+) = L(-) + \frac{\tau}{2}\left(\frac{v_N(-)}{R_N(L(-)) + h(-)} + \frac{v_N(+)}{R_N(L(-)) + h(+)}\right) +$$ + +$$ +\lambda(+) = \lambda(-) + \frac{\tau}{2}\left(\frac{v_E(-)}{\left(R_E(L(-)) + h(-)\right)\cos L(-)} + \frac{v_E(+)}{\left(R_E(L(+)) + h(+)\right)\cos L(+)}\right) +$$ + +The minus sign in the height update is there because $h$ is positive *up* while $v_D$ is +positive *down*. The code guards $\cos L$ away from zero near the poles, wraps the longitude to +$[-\pi, \pi]$ and keeps the latitude on $[-\pi/2, \pi/2]$. + +## The specific force sign convention + +This is the most common way to get a first integration wrong. At rest, the accelerometer +senses the support force, so the specific force is $\mathbf f = -\mathbf g$. **In NED, an +accelerometer at rest reads about −9.8 m/s² on its down axis.** The velocity update then adds +$\mathbf g^n = (0, 0, +g)$, and the two cancel: the velocity stays at zero, as it should. In +free fall the accelerometer reads zero, and the velocity update integrates $+g$ downward. + +The `syn` output follows this convention: its stationary records carry `acc_z` ≈ −9.76 m/s² +with level attitude. The input must not be gravity-compensated beforehand. Data with gravity +already removed would make a parked vehicle fall at $1g$. + +The ENU convention flips the sign of the vertical axis, so there the same accelerometer reads ++9.8 m/s² on its up axis. The library does not branch on the frame inside these equations. An +ENU state and sample are reflected into NED, propagated, and reflected back, so there is +exactly one implementation of each equation. [Coordinate Frames](./coordinate-frames.md) +describes the reflection and the check that refuses data declared in the wrong frame. + +## Aiding: loosely coupled filtering + +Every filter here is **loosely coupled**: aiding sensors contribute already-solved quantities +(position, velocity, altitude, heading), not raw GNSS observables. There are no pseudorange or +carrier-phase models. + +A filter alternates two steps: + +- **Predict.** Mechanize the IMU sample as above, and propagate the uncertainty with it. For + the Kalman filters, $\mathbf P \leftarrow \mathbf F \mathbf P \mathbf F^\top + \mathbf Q_k$, + where $\mathbf F$ is the error-state transition matrix (the analytic Jacobians in + `core/src/linearize.rs`; the UKF propagates sigma points instead). +- **Update.** When a measurement $\mathbf z$ arrives, compare it with the prediction + $\hat{\mathbf z} = h(\hat{\mathbf x})$, weigh the residual $\boldsymbol\nu = \mathbf z - + \hat{\mathbf z}$ against the predicted uncertainty $\mathbf S = \mathbf H \mathbf P + \mathbf H^\top + \mathbf R$, and correct the state with the gain + $\mathbf K = \mathbf P \mathbf H^\top \mathbf S^{-1}$. + +In **closed loop** (`cl`, `pf`) the correction is fed back into the mechanization. The ESKF +injects its estimated errors into the nominal state and resets them to zero; the EKF and UKF +update their full state directly; the RBPF folds its weighted-mean error into its nominal +trajectory. The Kalman filters also apply their updated IMU-bias estimates to the next sample. +`dr` only mechanizes. + +In `cl` the measurements come from the input records: + +| Source | Model | Measures | Rate | +| --- | --- | --- | --- | +| GNSS | `GPSPositionAndVelocityMeasurement` | $L$, $\lambda$, $h$, $v_N$, $v_E$ (velocity from `speed` and `bearing`) | as the GNSS scheduler allows (every fix by default) | +| Barometer | `RelativeAltitudeMeasurement` | Height: the record's `relativeAltitude` added to the first record's `altitude`, compared with $h$ plus the barometric bias when that state is carried | 1 Hz | +| Magnetometer | `MagnetometerYawMeasurement` | Heading, corrected by World Magnetic Model declination | 1 Hz | + +The particle filter takes GNSS and magnetometer heading the same way, but uses the barometer +differently: its readings drive an altitude-aiding loop inside the mechanization rather than +entering as measurement updates ([RBPF](../filters/rbpf.md)). + +The GNSS measurement noise comes from the record's `horizontalAccuracy`, `verticalAccuracy` and +`speedAccuracy` columns. The GNSS degradation machinery acts on this stream: the scheduler +decides which fixes reach the filter, and the fault model alters the ones that do ([Fault +Simulation](../gnss/fault-simulation.md)). The barometer and magnetometer have their own +schedulers and are unaffected by GNSS outages. All the models, and the innovation gating that +can reject an outlier, are documented in [Measurement Models and +Integrity](../filters/measurements.md). + +## Process noise is a spectral density + +Process noise models what the mechanization does not: sensor noise, unmodelled dynamics, bias +drift. The library specifies it as a **spectral density** $q$, a variance per second, and +every Kalman filter forms the per-step covariance as + +$$ +\mathbf Q_k = \mathbf q \tau . +$$ + +This makes the uncertainty a trajectory accumulates a function of elapsed time rather than of +how often it was sampled. Until #374 the same numbers were added once per IMU sample with no +$\tau$, so a 50 Hz recording received fifty times the process noise of a 1 Hz one from the same +constant. + +The defaults are `sim::DEFAULT_PROCESS_NOISE_DENSITY`, one entry per state of the 15-element +layout: + +| States | Density | Units | +| --- | --- | --- | +| Latitude, longitude | $(0.1\ \text{m})^2$/s converted to radians: `(0.1 * METERS_TO_RADIANS)^2` | rad²/s | +| Altitude | $10^{-2}$ | m²/s | +| Velocity (3) | $10^{-3}$ | (m/s)²/s | +| Attitude (3) | $10^{-5}$ | rad²/s | +| Accelerometer bias (3) | $10^{-6}$ | (m/s²)²/s | +| Gyroscope bias (3) | $10^{-8}$ | (rad/s)²/s | + +The barometric bias state, when carried, adds a random walk of +$(8.3\ \text{m})^2$ per hour, `sim::BARO_BIAS_PROCESS_NOISE_M2_PER_S`, which is the drift of +about one hectopascal of reference pressure per hour. The particle filter's noise parameters +are configured separately, as standard deviations per $\sqrt{\text{s}}$; see the +[RBPF page](../filters/rbpf.md). diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index 8482e600..b3ff92ba 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -1,251 +1,512 @@ # Configuration Files -A scenario configuration describes how the aiding sensors behave during a simulation: **when** -their measurements are delivered and, for GNSS, **how** they are corrupted. It is a single -document, and it can be written as YAML, JSON or TOML — the format is chosen by the file -extension. - -`--config` parses a whole `SimulationConfig`, so the aiding scenario is **nested under -`aiding:`** and the document must set `mode`. A bare aiding fragment is rejected with -``missing field `mode` `` rather than half-loaded: - -```yaml -mode: closed-loop # required -- see the mode table below -input: data/input.csv -output: results/output.csv -is_enu: false # NED is the default; Sensor Logger exports need `true` - -aiding: - scheduler: # when GNSS fixes arrive - kind: duty_cycle - on_s: 120.0 - off_s: 30.0 - start_phase_s: 0.0 - fault: # how they are corrupted - kind: none - baro_scheduler: # when barometric altitude arrives; 1 Hz if omitted - kind: fixed_interval - interval_s: 1.0 - phase_s: 0.0 - magnetometer_scheduler: # when a magnetometer heading arrives; 1 Hz if omitted - kind: fixed_interval - interval_s: 1.0 - phase_s: 0.0 - baro_noise_std_m: 2.23606797749979 # barometer one-sigma, metres; this is the default - seed: 42 # for reproducibility -``` +A configuration file describes a whole `strapdown-sim` run: the input and output, the mode, the +filter and its tuning, how the aiding sensors are scheduled and corrupted, and the limits that +stop a run. It is one `SimulationConfig` (`strapdown::sim`), written as TOML, YAML or JSON -- the +format is chosen by the file extension (`.toml`, `.yaml`/`.yml`, `.json`). ```bash -strapdown-sim --config scenario.yaml +strapdown-sim --config scenario.toml ``` -`--config` supplies the entire run, so a subcommand and its arguments — `-i`, `-o`, `--enu`, -`--seed` — are **ignored** when it is present. Set them in the file, as above. - -**`mode` does not take the CLI subcommand names.** The subcommands are abbreviations; the -config spellings are the `SimulationMode` variant names in kebab-case, and anything else is -rejected as an unknown variant: +`--config` supplies the entire run. A subcommand given alongside it, and every flag belonging to +that subcommand (`-i`, `-o`, `--enu`, `--seed`, `--filter`, ...), is **ignored**. Only the global +options still act: `--log-level`, `--log-file`, `--parallel` and `--plot`. Writing a scenario into +a file once it settles is how a run is made exactly repeatable. -| `mode:` in a config | CLI subcommand | -|---|---| -| `dead-reckoning` | `dr` | -| `open-loop` | `ol` (not implemented -- writes no output) | -| `closed-loop` | `cl` | -| `particle-filter` | `pf` | -| `synthetic` | `syn` | - -## Names must be exact - -Every field has a default, so a misspelled `kind` **does not error**. The section is silently -dropped and the run proceeds with no degradation at all — a scenario that looks like it ran -and simulated nothing. This is the single most common way to get a wrong result out of the -simulator. - -Config files use the names of the `MeasurementScheduler` and `GnssFaultModel` variants: +`strapdown-sim config` writes a complete starting file interactively; see +[Running Simulations](./simulations.md#config-the-configuration-wizard). Worked files live in +[`examples/configs/`](https://github.com/jbrodovsky/strapdown-rs/tree/main/examples/configs). -| Section | Valid `kind` values | -|---|---| -| `scheduler`, `baro_scheduler`, `magnetometer_scheduler` | `pass_through`, `fixed_interval`, `duty_cycle` | -| `fault` | `none`, `degraded`, `slow_bias`, `hijack`, `combo` | +## A complete example + +Every key below was run through `strapdown-sim --config` on a synthetic trajectory. Most are +optional; a minimal file needs only `mode`. + +```toml +mode = "closed-loop" # required +input = "synthetic.csv" +output = "cfg_out/cl_full.csv" +is_enu = false +generate_plot = true + +[logging] +level = "warn" +file = "cfg_out/cl_full.log" + +[execution_limits] +max_wall_clock_ratio = 0.5 +max_wall_clock_s = 600.0 +max_no_progress_s = 60.0 + +[health_limits] +lat_rad = [-1.0, 1.0] # radians, not degrees +lon_rad = [-1.0, 1.0] +alt_m = [-500.0, 5000.0] +speed_mps_max = 100.0 +cov_diag_max = 1e12 +nis_pos_max = 200.0 +nis_pos_consec_fail = 50 + +[closed_loop] +filter = "ukf" +ukf_alpha = 0.1 +ukf_beta = 2.0 +ukf_kappa = 0.0 +estimate_baro_bias = true +innovation_gate = { chi_squared = { confidence = 0.999 } } +gate_recovery = { rejection_inflation = 4.0, forced_update_after = 3 } + +[aiding] +seed = 7 +baro_noise_std_m = 2.0 +max_imu_gap_s = 2.0 + +[aiding.scheduler] +kind = "duty_cycle" +on_s = 100.0 +off_s = 50.0 +start_phase_s = 30.0 + +[aiding.fault] +kind = "degraded" +rho_pos = 0.99 +sigma_pos_m = 5.0 +rho_vel = 0.95 +sigma_vel_mps = 0.3 +r_scale = 2.0 +tau_pos_s = 60.0 +tau_vel_s = 30.0 + +[aiding.baro_scheduler] +kind = "fixed_interval" +interval_s = 2.0 +phase_s = 0.0 + +[aiding.magnetometer_scheduler] +kind = "pass_through" +``` -**The CLI flags use different spellings for the same things**: `--sched passthrough|fixed|duty` -and `--fault none|degraded|slowbias|hijack`. The duty-cycle phase is `--duty-phase-s` on the -command line and `start_phase_s` in a config file, while `phase_s` in a config file belongs to -`fixed_interval` only. +The same structure in YAML nests the tables as mappings (`aiding:` with `scheduler:` beneath it, +and so on), and in JSON as objects. The one place the three spellings differ is the innovation +gate; see [below](#innovation_gate). + +## Names must be exact -- in one direction + +Two kinds of mistake behave differently: + +- **A misspelled value is an error.** An unknown `mode`, `filter` or `kind` stops the run before + anything happens, naming the valid choices: + ``unknown variant `degradd`, expected one of `none`, `degraded`, `slow_bias`, `hijack`, `combo` ``. + So does a missing required field, such as one of a fault model's parameters. +- **A misspelled key or section is silently ignored.** Unknown keys are not rejected, so + `ukf_alfa = 0.1` leaves `ukf_alpha` at its default, and `[aiding.faults]` in place of + `[aiding.fault]` leaves the run with no fault at all -- a scenario that looks configured and + simulates nothing. Check the names against this page, and against the `Mode:`/`Input:` lines + the run logs at `info` level. + +The two exceptions are the `[particle_filter]` keys that were removed when the filter was +restructured, which are refused by name; see [below](#particle_filter). + +## Top-level keys + +| Key | Default | Meaning | +|---|---|---| +| `mode` | **required** | what to run; see the table below | +| `input` | `"input.csv"` | input CSV file or directory, as for `-i` | +| `output` | `"output.csv"` | output CSV file or directory, as for `-o` | +| `seed` | `42` | seed of the **particle filter**; see [Seeds](#seeds) | +| `is_enu` | `false` | the input records are ENU rather than NED; as `--enu` | +| `parallel` | `false` | process a directory's files concurrently | +| `generate_plot` | `false` | write a PNG plot beside each closed-loop or particle-filter result | + +The input and output path rules -- directory versus file, CSV only, refusing to overwrite an input +-- are the command line's; see [Running Simulations](./simulations.md#input-and-output-paths). + +`mode` takes the `SimulationMode` variant names in kebab-case, not the CLI subcommand names: + +| `mode` | CLI subcommand | Section it reads | +|---|---|---| +| `dead-reckoning` | `dr` | -- | +| `closed-loop` | `cl` | `[closed_loop]` | +| `particle-filter` | `pf` | `[particle_filter]` | +| `synthetic` | `syn` | `[synthetic]` | +| `open-loop` | `ol` | -- (**not implemented**: exits with an error) | + +`mode = "dr"` is rejected as an unknown variant. Every mode reads `[aiding]`, +`[execution_limits]`, `[health_limits]` and `[logging]`, except that dead reckoning ignores the +aiding and the limits as it does on the command line, and synthetic mode ignores everything but +`[synthetic]`. + +## `[aiding]` + +How the aiding measurements are delivered, and how GNSS is corrupted. The section was called +`gnss_degradation` before the v1.0 freeze, and that name is still accepted as an alias -- older +files keep working -- but `aiding` is the name to write. + +| Key | Default | Meaning | +|---|---|---| +| `scheduler` | `pass_through` | when GNSS fixes are delivered; see [Schedulers](#schedulers) | +| `fault` | `none` | how GNSS fixes are corrupted; see [Fault models](#fault-models) | +| `baro_scheduler` | `fixed_interval`, 1 s | when barometric altitude is delivered | +| `magnetometer_scheduler` | `fixed_interval`, 1 s | when magnetometer heading is delivered | +| `baro_noise_std_m` | `2.23606797749979` | barometric altitude one-sigma, metres | +| `seed` | `42` | seed of the fault models | +| `max_imu_gap_s` | `5.0` | longest tolerated gap in the inertial stream, seconds; `0` or less disables the check | + +`max_imu_gap_s` bounds the IMU stream only. A record with a missing accelerometer or gyroscope +value produces no propagation step, and the filter coasts across the hole; a hole longer than +this limit is not a scenario but a broken recording, and the run is refused with a +`SensorStreamGap` error. Shorter gaps are tolerated with one warning. A missing GNSS fix is never +an error -- outages are what the scheduler is for. There is no command-line flag for it. + +The library type also has a `baro_bias_index` field. `strapdown-sim` overwrites it from the +filter it builds, so setting it in a file has no effect. + +Every snippet in the next three subsections is a **fragment of the `[aiding]` section**. Copied to +the top level of a file, it is an unknown key and is ignored. + +### Schedulers + +The three scheduler keys take the same three kinds, each with its own independent clock, so a +GNSS outage, a barometer outage and a heading outage can be set separately or made to overlap. +All fields of a kind are required. + +```toml +[aiding.scheduler] +kind = "pass_through" # every fix + +[aiding.scheduler] +kind = "fixed_interval" # at most one fix per interval +interval_s = 10.0 +phase_s = 0.0 # time of the first emitted fix + +[aiding.scheduler] +kind = "duty_cycle" # alternate availability and denial +on_s = 120.0 +off_s = 30.0 +start_phase_s = 0.0 +``` -`core/tests/example_configs.rs` guards every file in `examples/configs/` against this class of -mistake: it parses each one and asserts the result is the scenario the file describes, rather -than a silently-defaulted pass-through. +(Each block is an alternative; a file has one `[aiding.scheduler]`.) -## Schedulers +`fixed_interval` takes the first usable fix at or after each tick, so a record without a fix +does not use up the tick. `duty_cycle` runs `start_phase_s` of initial availability, then `off_s` +denied and `on_s` available, repeating -- so **with `start_phase_s = 0` the run opens with an +outage** of `off_s` seconds. -Every snippet in this section is a **fragment of the `aiding:` block**, shown unindented -for readability. Copying one to the root of a config file leaves `aiding.scheduler` and -`aiding.fault` at their defaults, and the unknown root key is ignored — a scenario that -looks configured and runs with no degradation at all. +The command line spells these differently: `--sched passthrough|fixed|duty`, and the duty-cycle +phase is `--duty-phase-s`, while `phase_s` in a file belongs to `fixed_interval` only. +### Fault models -### `pass_through` +A fault corrupts the content of each GNSS fix the scheduler delivers; the two compose. Faults +apply to GNSS only. **Every field of a fault is required** except the two `tau_*` keys of +`degraded`: a fault given with a field missing is refused with ``missing field ...``. -Every fix reaches the filter. The baseline for comparing degraded runs. +```toml +# No corruption (the default) +[aiding.fault] +kind = "none" -```yaml -scheduler: - kind: pass_through +# AR(1)-correlated position and velocity error, with inflated advertised accuracy. +# Models low signal-to-noise or multipath. +[aiding.fault] +kind = "degraded" +rho_pos = 0.99 # AR(1) coefficient of the position error, per fix +sigma_pos_m = 3.0 # position error, m +rho_vel = 0.95 # AR(1) coefficient of the velocity error, per fix +sigma_vel_mps = 0.3 # velocity error, m/s +r_scale = 5.0 # factor on the advertised sigmas, so R grows by its square +tau_pos_s = 60.0 # optional: position-error correlation time, s +tau_vel_s = 30.0 # optional: velocity-error correlation time, s + +# A slowly drifting offset. Models a soft spoof. +[aiding.fault] +kind = "slow_bias" +drift_n_mps = 0.02 # northward drift of the offset, m/s +drift_e_mps = 0.0 # eastward drift, m/s +q_bias = 1e-6 # random-walk density of the offset, m^2/s +rotate_omega_rps = 0.0 # rotation rate of the drift direction, rad/s + +# A constant offset over a fixed window. Models a hard spoof. +[aiding.fault] +kind = "hijack" +offset_n_m = 50.0 +offset_e_m = 0.0 +start_s = 120.0 # seconds from the first record +duration_s = 60.0 ``` -### `fixed_interval` - -One fix every `interval_s` seconds, the rest discarded. Models a reduced update rate. - -```yaml -scheduler: - kind: fixed_interval - interval_s: 10.0 - phase_s: 0.0 # offset before the first emitted fix +Without `tau_pos_s`, `rho_pos` applies once per delivered fix, so the error's correlation time +depends on the fix interval; with it, the coefficient is $e^{-\Delta t/\tau}$ for the actual +interval and `sigma_pos_m` is the steady-state standard deviation. `tau_vel_s` does the same for +velocity. The [Closed Loop](./closed-loop.md#gnss-faults-what-the-fixes-say) page explains each +parameter, and [GNSS Degradation](../gnss/fault-simulation.md) the models. + +**`combo`** applies several faults in sequence, each feeding the next. It exists only in a +configuration file -- `--fault` has no combo choice. Its members go in a `faults` list: + +```toml +[aiding.fault] +kind = "combo" + +[[aiding.fault.faults]] +kind = "slow_bias" +drift_n_mps = 0.02 +drift_e_mps = 0.0 +q_bias = 1e-6 +rotate_omega_rps = 0.0 + +[[aiding.fault.faults]] +kind = "hijack" +offset_n_m = 50.0 +offset_e_m = 0.0 +start_s = 120.0 +duration_s = 60.0 ``` -### `duty_cycle` - -Alternating windows of availability and denial. The timeline is `start_phase_s` of initial -availability, then `off_s` denied and `on_s` available, repeating. - ```yaml -scheduler: - kind: duty_cycle - on_s: 120.0 # 2 minutes available - off_s: 30.0 # 30 seconds denied - start_phase_s: 0.0 +aiding: + fault: + kind: combo + faults: + - kind: slow_bias + drift_n_mps: 0.02 + drift_e_mps: 0.0 + q_bias: 1.0e-6 + rotate_omega_rps: 0.0 + - kind: hijack + offset_n_m: 50.0 + offset_e_m: 0.0 + start_s: 120.0 + duration_s: 60.0 ``` -Every fix inside an ON window is delivered, and none inside an OFF window. - -## The other two aiding channels - -### The barometer's noise +Both run on a synthetic trajectory, and the solution shows each stage: pulled about 50 m north +during the hijack window, and several metres off after it ends, as the slow drift accumulates. The +members share one fault state, so combine *different* kinds; two members of the same kind step +the same error states. -`baro_noise_std_m` is the one-sigma barometric altitude uncertainty in **metres**, and it is -the only aiding-noise knob a config file has: the Sensor Logger format carries no -pressure-accuracy column, so unlike GNSS — whose noise comes from each record's -`horizontal_accuracy` — nothing in a log can supply one. Set it to model a good barometer or a -bad one. +### The barometer and magnetometer -It is a *standard deviation*. The value it replaced lived in a trait impl as `diag([5.0])`, -which is a **variance**, so the default here is its square root and the filter sees the same -$R$ it always did. Setting `baro_noise_std_m: 5.0` is not the old behaviour — it is five times -looser. +`baro_scheduler` and `magnetometer_scheduler` take the same kinds and fields as `scheduler`. +**Both default to `fixed_interval` at 1 Hz, not to `pass_through`.** Delivering one measurement +per record ties a sensor's update rate to the log's rate: right by coincidence on a 1 Hz +recording, but on a 50 Hz synthetic trajectory it delivered fifty pressure readings and fifty +derived headings a second, each entering the filter as an independent measurement with full +weight -- one heading counted fifty times. Set `kind = "pass_through"` when the log's rate really +is the sensor's rate: -### The barometer's bias - -A barometer does not read altitude, it reads pressure, and the reference pressure it is -converted against drifts — roughly a hectopascal an hour, which is about 8.3 m. A filter that -models the reading as unbiased has nowhere to put that drift except into altitude. - -`estimate_baro_bias` gives the filter a state for it. It belongs to the **`closed_loop`** -section, not to the aiding config above, because it changes the filter rather than the sensor: - -```yaml -closed_loop: - filter: ukf - estimate_baro_bias: true +```toml +[aiding.magnetometer_scheduler] +kind = "pass_through" ``` -It is **on by default** as of the v1.0 freeze, so this section only needs writing to turn it -*off* (`estimate_baro_bias: false`, or `--no-estimate-baro-bias` on the command line when not -using `--config`). The output carries `baro_bias` and -`baro_bias_cov` columns; without it both are empty, which is how a reader tells "no bias state" -from "bias estimated at zero". - -Measured on `core/tests/test_data.csv`, switching it on moves the vertical channel in all -three Kalman filters at once: - -| | 3σ containment | vertical bias | vertical RMSE | -|---|---|---|---| -| off | 0.40 | +0.40 m | 2.58 m | -| on | 0.84 | −0.02 m | 1.41 m | - -Containment is the fraction of epochs whose true altitude lies inside the filter's own 3σ -band, so its ideal is 0.9973. The three filters converge independently on a bias of about -−0.6 m, which is the corroboration a single filter could not give. - -The one measured case that gets worse is an AR(1)-degraded GNSS position fault, where the bias -state is unidentifiable — it is aided by a second source that is itself biased, so it absorbs -the GNSS fault instead of the barometer's drift, and vertical RMSE rises about 30%. That is -tracked as issue #410; `--no-estimate-baro-bias` turns the state off for such a run. - -`baro_scheduler` and `magnetometer_scheduler` take the same three kinds and the same fields, -each with its own independent clock, so a GNSS outage, a barometer outage and a heading outage -can be configured separately or made to overlap. Neither channel is corrupted: `fault` governs -GNSS only. - -**Both default to `fixed_interval` at 1 Hz, not to `pass_through`.** Before #375 they were not -scheduled at all — one measurement per record, whatever the log's sample rate happened to be. -On a 1 Hz Sensor Logger export that is right by coincidence; on a 50 Hz synthetic trajectory it -delivered fifty pressure readings and fifty derived headings every second, each entering the -filter as an independent fix with full weight. A heading re-read from the same field vector -fifty times is one measurement counted fifty times, and the UKF diverges on it. - -Set `kind: pass_through` to get the old behaviour back, when your log's rate really is the -sensor's rate: - -```yaml -magnetometer_scheduler: - kind: pass_through +`baro_noise_std_m` is the barometer's one-sigma in **metres**, and it is the only aiding-noise +setting in a file: GNSS noise comes from each record's accuracy columns +([Input Data Format](./data-format.md#which-columns-drive-what)), and the input format has no +pressure-accuracy column. It is a standard deviation. The value it replaced was a variance of 5, +so the default is its square root and the filter sees the same $R$; setting +`baro_noise_std_m = 5.0` is five times looser, not the old behaviour. + +None of these three has a command-line flag; they are configurable from a file only. + +## `[closed_loop]` + +Read when `mode = "closed-loop"`. Every key is optional; an omitted key takes the default below, +and an omitted section is the same as an empty one. + +| Key | Default | Meaning | +|---|---|---| +| `filter` | `"eskf"` | `"eskf"`, `"ukf"` or `"ekf"` | +| `ukf_alpha` | `0.1` | UKF sigma-point spread | +| `ukf_beta` | `2.0` | UKF prior-distribution parameter | +| `ukf_kappa` | `0.0` | UKF secondary spread parameter | +| `estimate_baro_bias` | `true` | carry a barometric bias state; as the inverse of `--no-estimate-baro-bias` | +| `innovation_gate` | none (accept everything) | reject measurements by NIS; see below | +| `gate_recovery` | inflation 2.0, force after 5 | how the filter recovers from rejections; see below | + +The UKF parameters are ignored by the other two filters. `ukf_alpha` is `0.1` rather than the +textbook `1e-3` for a numerical reason: at `1e-3` the weighted sigma-point mean cancels away six +significant digits on every step (see `ClosedLoopConfig::ukf_alpha` in the API documentation). +The `--ukf-alpha` flag has the same default, so a file and the command line run the same filter. + +### The barometric bias + +A barometer does not read altitude, it reads pressure, and the reference pressure it is converted +against drifts -- roughly a hectopascal an hour, about 8.3 m. A filter that models the reading as +unbiased has nowhere to put that drift except into altitude. `estimate_baro_bias` gives the +filter a state for it, and the output carries it as `baro_bias` and `baro_bias_cov`; with the +state off both columns are empty, which is how a reader tells "no bias state" from "bias +estimated at zero". + +It is **on by default** as of the v1.0 freeze, so the key only needs writing to turn it off. The +measurement that justified the default -- improved vertical error and vertical three-sigma +containment on the reference recording, in all three filters -- is recorded on +`ClosedLoopConfig::estimate_baro_bias` in the API documentation. The known case where it hurts is +an AR(1)-degraded GNSS fault, where the bias state absorbs the GNSS fault instead of the +barometer's drift (issue #410); turn it off for such a run. + +### `innovation_gate` + +Absent, every measurement is accepted. Present, it takes one of two forms: + +- `chi_squared` with a `confidence` strictly between 0 and 1: the threshold is the $\chi^2$ + quantile at that probability, for each measurement's own degrees of freedom. Prefer this one. +- `fixed` with a `threshold`: one NIS limit regardless of the measurement's dimension. + +This is the one key whose spelling depends on the format. TOML and JSON nest it as a map; YAML +needs a tag: + +```toml +[closed_loop] +innovation_gate = { chi_squared = { confidence = 0.999 } } +# or: innovation_gate = { fixed = { threshold = 25.0 } } ``` -There are no CLI flags for these two; they are configurable from a file only. - -## Fault models - -Every snippet in this section is a **fragment of the `aiding:` block**, shown unindented -for readability. Copying one to the root of a config file leaves `aiding.scheduler` and -`aiding.fault` at their defaults, and the unknown root key is ignored — a scenario that -looks configured and runs with no degradation at all. - - -Faults corrupt the content of a fix; the scheduler decides whether it arrives at all. The two -compose, so a duty cycle can deliver degraded fixes during its ON windows. - ```yaml -# No corruption -fault: - kind: none - -# AR(1)-correlated position and velocity error, with inflated reported covariance. -# Models low signal-to-noise or multipath. -fault: - kind: degraded - rho_pos: 0.99 # correlation of the position error, per step - sigma_pos_m: 3.0 - rho_vel: 0.95 - sigma_vel_mps: 0.3 - r_scale: 5.0 # factor the advertised covariance is inflated by - -# A slowly drifting offset. Models soft spoofing: the trajectory is nudged -# gradually, and each individual fix looks plausible. -fault: - kind: slow_bias - drift_n_mps: 0.02 - drift_e_mps: 0.0 - q_bias: 1e-6 - rotate_omega_rps: 0.0 - -# A hard offset during a fixed window. Models hard spoofing. -fault: - kind: hijack - offset_n_m: 50.0 - offset_e_m: 0.0 - start_s: 120.0 - duration_s: 60.0 +closed_loop: + innovation_gate: !chi_squared + confidence: 0.999 + # or: innovation_gate: !fixed { threshold: 25.0 } ``` -`combo` takes a list of the above and applies them in sequence, each feeding into the next. +```json +{ "closed_loop": { "innovation_gate": { "chi_squared": { "confidence": 0.999 } } } } +``` -## Reproducibility +The untagged YAML form `innovation_gate: { chi_squared: { confidence: 0.999 } }` is rejected with +``invalid type: map, expected a YAML tag starting with '!'``. + +### `gate_recovery` + +Consulted only when a gate is installed. Either field may be omitted and keeps its default. + +| Key | Default | Meaning | +|---|---|---| +| `rejection_inflation` | `2.0` | each rejection multiplies the filter's uncertainty by this factor in the directions the rejected measurement observed; `1.0` disables it | +| `forced_update_after` | `5` | after this many consecutive rejections, apply the next measurement regardless; `0` disables it (YAML may also write `null`) | + +The command-line flags `--gate-confidence`, `--gate-inflation` and `--gate-force-after` are +checked before a run starts -- a confidence outside (0, 1) or an inflation below 1 is refused. The +same values in a file are **not** checked: `confidence = 1.5` or `rejection_inflation = 0.5` runs +without an error. Keep file values inside the ranges above. + +## `[particle_filter]` + +Read when `mode = "particle-filter"`. The keys mirror the `pf` flags; the +[Particle Filter](./particle-filter.md) page explains them and +[Rao-Blackwellized Particle Filter](../filters/rbpf.md) the model behind them. + +| Key | Default | Unit | +|---|---|---| +| `num_particles` | `100` | -- | +| `position_init_std_m` | `[10.0, 10.0, 5.0]` | m, north/east/up | +| `velocity_init_std_mps` | `1.0` | m/s | +| `attitude_init_std_rad` | `0.1` | rad | +| `velocity_process_noise_std_mps` | `0.001` | m/s per √s | +| `attitude_process_noise_std_rad` | `0.01` | rad per √s | +| `horizontal_process_noise_std_m` | `[1.0, 1.0]` | m/√s, north/east | +| `baro_loop_time_constant_s` | `10.0` | s | +| `baro_error_std_m` | `8.3` | m | +| `baro_error_time_constant_s` | `3600.0` | s | +| `vertical_accel_error_init_std_mps2` | `0.1` | m/s² | +| `effective_sample_threshold` | `1.0` | fraction of the particle count | +| `roughening_factor` | `0.2` | -- | +| `gravity_variation_std` | derived | mGal | +| `gravity_variation_time_constant_s` | `300.0` | s | +| `magnetic_variation_std` | derived | nT | +| `magnetic_variation_time_constant_s` | `300.0` | s | + +- `horizontal_process_noise_std_m` defaults to 1 m/√s per axis, **not** the zero of Canciani & + Raquet's eq. 19 that the library's `RbpfConfig` keeps: zero diverges with GNSS-rate fixes on + MEMS data. It must hold exactly two values; any other length is refused. +- `position_init_std_m` must hold three values. A list of any other length is silently replaced + by the library default, which is also `[10, 10, 5]`. +- The particle seed is the top-level `seed`, not a key of this section. + +Four keys from before the restructure are refused by name, with a message saying where the +setting went: `geo_bias_init_std` and `geo_bias_process_noise_std` (now per channel in +`[geophysical]`), `position_process_noise_std_m` (now `horizontal_process_noise_std_m`), and +`zero_vertical_velocity` / `zero_vertical_velocity_std_mps` (now the barometer loop). + +## `[geophysical]` + +Present, it turns on map-aided navigation for `closed-loop` and `particle-filter` runs. It needs a +binary built with `--features geonav`; without that feature a file carrying the section is +refused rather than run without the maps. In closed-loop mode the filter must be `ukf` or `ekf` -- +the ESKF has no geophysical implementation, and since it is the default, a file that omits +`filter` is refused too. See [Geophysical Navigation](../geonav/overview.md). + +| Key | Unit | Meaning | +|---|---|---| +| `gravity_resolution` | -- | gravity map resolution; absent, gravity is not used | +| `gravity_map_file` | -- | gravity map path; absent, `_gravity.nc` beside the input file | +| `gravity_noise_std` | mGal | measurement noise | +| `gravity_bias` | mGal | initial value of the gravity map-bias state | +| `gravity_bias_init_std` | mGal | its initial standard deviation; defaults to `gravity_noise_std` | +| `gravity_bias_process_noise_std` | mGal/√s | its random-walk rate; defaults to the prior spread over an hour | +| `magnetic_resolution`, `magnetic_map_file`, `magnetic_noise_std`, `magnetic_bias`, `magnetic_bias_init_std`, `magnetic_bias_process_noise_std` | nT | the same for the magnetic map; the default map path is `_magnetic.nc` | +| `geo_interval_s` | s | seconds **between** geophysical measurements, for both maps | + +Resolutions are written in snake_case in a file (`"one_minute"`, `"thirty_seconds"`) and in +kebab-case on the command line (`one-minute`). `geo_interval_s` is a period, not a rate: a larger +value means fewer measurements. Its old name, `geo_frequency_s`, is still accepted as an alias. + +## `[synthetic]` + +Read when `mode = "synthetic"`, and the only section that mode reads. `output` and `duration_s` +are required; the rest mirror the `syn` flags, with the IMU grade spelled `imu_quality` and the +initial state in a `[synthetic.initial_state]` table. The full key list and a worked example are +on [Synthetic Trajectories](./synthetic.md#the-synthetic-configuration-section). + +## `[execution_limits]` and `[health_limits]` + +The wall-clock and divergence limits of [Running Simulations](./simulations.md#execution-limits), +with the same defaults. Each key may be given alone. + +| Section | Key | Default | +|---|---|---| +| `[execution_limits]` | `max_wall_clock_ratio` | `0.25` | +| | `max_wall_clock_s` | `1200.0` | +| | `max_no_progress_s` | `600.0` | +| `[health_limits]` | `lat_rad` | `[-π/2, π/2]` | +| | `lon_rad` | `[-π, π]` | +| | `alt_m` | `[-1e8, 1e8]` | +| | `speed_mps_max` | `500.0` | +| | `cov_diag_max` | `1e15` | +| | `nis_pos_max` | `100.0` | +| | `nis_pos_consec_fail` | `20` | + +The latitude and longitude bands are **radians** here, as `[min, max]` pairs, where the command +line takes degrees (`--health-lat-min-deg`). + +## `[logging]` + +| Key | Default | Meaning | +|---|---|---| +| `level` | `"info"` | `off`, `error`, `warn`, `info`, `debug` or `trace` | +| `file` | none (stderr) | log file path, appended to | + +`--log-file` on the command line overrides `file`. `--log-level` overrides `level` only when it +is something other than `info`: the flag's default is `info`, so `--log-level info` cannot be told +apart from not passing the flag, and the file's level wins. See [Logging](./logging.md). + +## Seeds + +A configuration file has up to three seeds, one per random process, all defaulting to `42`: + +| Key | Seeds | +|---|---| +| `aiding.seed` | the GNSS fault models | +| `seed` (top level) | the particle filter's sampling; read by `particle-filter` mode only | +| `synthetic.seed` | the generated trajectory's noise | -`seed` drives every stochastic part of the fault models. The same seed with the same -configuration produces identical output, which is what makes a reported error metric -meaningful. Vary the seed to generate realizations for Monte Carlo work. +On the command line `pf --seed` sets both of the first two at once. In a file they are +independent, so a particle-filter Monte Carlo study can vary the filter's seed while holding the +GNSS corruption fixed, or the reverse. Changing the top-level `seed` of a closed-loop file has no +effect: vary `aiding.seed` instead. -Worked files for all of these live in -[`examples/configs/`](https://github.com/jbrodovsky/strapdown-rs/tree/main/examples/configs). +The same seeds with the same file and input produce identical output, which is what makes a +reported error meaningful. diff --git a/book/src/user-guide/coordinate-frames.md b/book/src/user-guide/coordinate-frames.md index 532c9290..4cd22292 100644 --- a/book/src/user-guide/coordinate-frames.md +++ b/book/src/user-guide/coordinate-frames.md @@ -1 +1,145 @@ # Coordinate Frames + +Every number in an input or output file is resolved in some frame, and a wrong frame does not +produce a slightly worse answer: it produces one that falls out of the sky at twice gravity. +This page lists the frames the library uses, the one choice you have to make (NED or ENU), +and the check that catches a wrong choice. + +## The frames + +Following Groves' notation: + +| Frame | Symbol | Used for | +| --- | --- | --- | +| Inertial | $i$ | The reference the IMU measures against: $\boldsymbol\omega_{ib}^b$ and $\mathbf f_{ib}^b$ are motion of the body relative to inertial space | +| Earth-fixed | $e$ | The rotating Earth. Velocity is Earth-relative, $\mathbf v_{eb}^n$; the Earth's rotation enters as $\boldsymbol\omega_{ie}$ | +| Local-level navigation | $n$ | Where position, velocity and attitude are expressed: axes north, east and vertical at the vehicle's position on the WGS84 ellipsoid | +| Body | $b$ | The IMU's own sensor axes, in which `acc_*` and `gyro_*` are measured | + +Position is geodetic: latitude and longitude on the WGS84 ellipsoid, and height above that +ellipsoid. The library holds latitude and longitude in **radians** (`StrapdownState`); CSV +inputs and outputs carry them in **degrees**. The local-level mechanization is meant for +positions near the Earth's surface, roughly 11 km below to 30 km above the ellipsoid; +`StrapdownState::new` rejects an altitude beyond ±30 km. The simulator's default altitude +health limits are far wider (±10⁸ m) and are set with `--health-alt-min-m` and +`--health-alt-max-m`. There is no ECEF or ECI mechanization. + +## NED is the default; ENU is an opt-in + +The local-level axes are always ordered **north, east, vertical**. The frame choice only +decides which way the vertical axis points: + +| | NED (default) | ENU (opt-in) | +| --- | --- | --- | +| Vertical axis points | down | up | +| `velocity_vertical` | positive **down**: a climbing vehicle has negative vertical velocity | positive **up** | +| Gravity in the navigation frame | $(0, 0, +g)$ | $(0, 0, -g)$ | +| Accelerometer at rest, level | ≈ −9.8 m/s² on the body's vertical axis | ≈ +9.8 m/s² on the body's vertical axis | +| `altitude` | height above the ellipsoid, positive **up** | height above the ellipsoid, positive **up** | + +Note the last row: **`altitude` is positive up in both frames.** It is a height, not a "down" +coordinate. Only the vertical velocity, the gravity vector and the vertical sensor axis change +sign with the frame. Note also that "ENU" here keeps the north-then-east order of the +horizontal axes; it does not swap them to east-then-north. The horizontal velocities are +`velocity_north` and `velocity_east` in both. + +NED matches Groves and aerospace practice, and it is what `strapdown-sim syn` writes. Data +from the Sensor Logger phone app is ENU: at rest its specific force lands on the device's up +axis at +9.8 m/s². + +### Declaring the frame + +A CSV file carries no frame tag, so the frame is always the caller's declaration: + +| Where | NED (default) | ENU | +| --- | --- | --- | +| `strapdown-sim dr`, `cl`, `pf` | omit the flag | `--enu` | +| `strapdown-sim syn` (to *write* ENU records) | omit the flag | `--enu` | +| Scenario file | `is_enu = false`, or omit it | `is_enu = true` | +| `StrapdownState` | `is_enu: false` (the `Default`) | `is_enu: true` | +| `kalman::InitialState::new` | last argument `None` or `Some(false)` | `Some(true)` | +| `sim::UkfConfig`, `EkfConfig`, `EskfConfig` | `is_enu: false` (the default) | `is_enu: true` | +| `sim::dead_reckoning(records, is_enu)` | `false` | `true` | + +`syn --enu` output is what `dr --enu` expects, and plain `syn` output is what plain `dr` +expects. + +### Converting a state + +`StrapdownState::to_ned` and `StrapdownState::to_enu` reinterpret an existing state in the +other convention, and are no-ops when the state is already in the target frame. Converting: + +- negates `velocity_vertical`; +- leaves `altitude`, `latitude`, `longitude` and the horizontal velocities untouched; +- conjugates the attitude by the reflection $\mathbf F = \mathrm{diag}(1, 1, -1)$: + $\mathbf C' = \mathbf F \mathbf C \mathbf F$. The reflection is applied to the navigation + frame's vertical axis *and* to the body frame's, which keeps $\mathbf C'$ a proper rotation. + In Euler angles this leaves yaw unchanged and negates roll and pitch; +- flips the `is_enu` flag. + +Internally the mechanization uses exactly this: an ENU state and its IMU sample are reflected +into NED, propagated with the NED equations of [The Navigation Model](./concepts.md), and +reflected back. There is one implementation of each equation rather than a frame branch +inside each. + +## The frame check + +Declaring the wrong frame is refused before anything is integrated. For every mode, +`sim::check_declared_frame` takes the first 10 records (`sim::FRAME_CHECK_SAMPLES`), rotates +each one's specific force into the navigation frame using the record's attitude quaternion, +and averages the vertical component. At rest that should be $-g$ in NED and $+g$ in ENU. If +the mean lies on the wrong side by more than half of local gravity +(`sim::FRAME_CHECK_MARGIN_G = 0.5`), the run stops with `StrapdownError::InvalidConfiguration` +on the field `is_enu`, and the message names the flag to change. + +`syn` output is NED. Declaring it ENU: + +```console +$ strapdown-sim --log-level warn cl --enu -i synthetic.csv -o eskf_enu.csv +2026-10-01 20:42:51.596 [ERROR] - Error running closed-loop simulation on synthetic.csv: invalid configuration for `is_enu`: mean vertical specific force over the first 10 record(s) is -9.75 m/s^2, but ENU mechanization expects +9.78 m/s^2 at rest: these look like NED records. Mechanizing them as ENU would double-count gravity and integrate at 2 g. Declare the frame that matches the data (drop `--enu`, or set `is_enu = false` in the config file), or re-record it in ENU. +Error: InvalidConfiguration { field: "is_enu", reason: "mean vertical specific force over the first 10 record(s) is -9.75 m/s^2, but ENU mechanization expects +9.78 m/s^2 at rest: these look like NED records. Mechanizing them as ENU would double-count gravity and integrate at 2 g. Declare the frame that matches the data (drop `--enu`, or set `is_enu = false` in the config file), or re-record it in ENU." } +``` + +The command exits with status 1 and writes no output file. `dr` and `pf` refuse the same way. +When the declaration is right, the check logs what it measured at the `info` level: + +```text +[INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +``` + +The reason for refusing rather than warning: mechanizing NED records as ENU adds the gravity +model to the sensed specific force instead of cancelling it, so a stationary solution falls at +$2g$. The check fails *open* on data it cannot judge (empty input, non-finite values), and the +half-gravity margin leaves room for a vehicle that is not at rest when the recording starts. + +## The body frame + +The body frame is whatever the IMU's sensor axes are; the library does not assume +forward-right-down. The attitude $\mathbf C_b^n$ maps a body-frame vector into the +navigation frame. With all three Euler angles zero, the body axes coincide with the navigation +axes, so in NED a level, north-facing IMU has $x$ north, $y$ east and $z$ down. That is why the +`syn` records, which are level with zero heading, read about −9.76 m/s² on `acc_z`. + +The initial attitude of a run comes from the first record's quaternion (`qw`, `qx`, `qy`, +`qz`), via `TestDataRecord::attitude`, not from its `roll`, `pitch` and `yaw` columns. In +Sensor Logger data those columns use the app's own angle convention, which disagrees with the +one below. An all-NaN or zero-norm quaternion is read as the identity. + +## Attitude: DCM and Euler angles + +Attitude is held as a direction cosine matrix $\mathbf C_b^n$ (a `nalgebra::Rotation3`; the +ESKF carries a unit quaternion internally). Euler angles are used for input and output only. +They are roll $\phi$, pitch $\theta$ and yaw $\psi$, with + +$$ +\mathbf C_b^n = \mathbf R_z(\psi) \mathbf R_y(\theta) \mathbf R_x(\phi), +$$ + +the convention of `nalgebra::Rotation3::from_euler_angles(roll, pitch, yaw)`: roll about $x$ +is applied first, then pitch about $y$, then yaw about $z$, all about fixed axes. This is the +same matrix as Groves' yaw-pitch-roll Euler convention for $\mathbf C_b^n$. + +In output files the angles are radians, on the branch `Rotation3::euler_angles` returns: roll +and yaw on $[-\pi, \pi]$, pitch on $[-\pi/2, \pi/2]$. Yaw is measured from north; in NED a +negative yaw is west of north. The EKF and UKF carry pitch as a plain state element, so their +pitch is wrapped to $[-\pi, \pi]$ rather than limited to $\pm\pi/2$. diff --git a/book/src/user-guide/data-format.md b/book/src/user-guide/data-format.md index 42ad44e0..2f6fd8c8 100644 --- a/book/src/user-guide/data-format.md +++ b/book/src/user-guide/data-format.md @@ -1,53 +1,106 @@ # Input Data Format -## Overview - -The strapdown-rs project uses CSV data files for simulation and testing. The primary data format is compatible with the **Sensor Logger** mobile application, which records IMU, GNSS, and other sensor measurements. - -## CSV Column Descriptions - -The data files contain the following columns (not all columns may be present in every file): - -### Timestamp -* **time** - ISO UTC timestamp of the form `YYYY-MM-DD HH:mm:ss.ssss+HH:MM` (where the `+HH:MM` is the timezone offset) - -### GNSS Measurements -* **latitude** - Latitude measurement in degrees -* **longitude** - Longitude measurement in degrees -* **altitude** - Altitude measurement in meters -* **speed** - Speed measurement in meters per second -* **bearing** - Bearing measurement in degrees - -### IMU Measurements (Body Frame) -* **acc_x** - Acceleration in the X direction (meters per second squared) -* **acc_y** - Acceleration in the Y direction (meters per second squared) -* **acc_z** - Acceleration in the Z direction (meters per second squared) -* **gyro_x** - Angular velocity around the X axis (radians per second) -* **gyro_y** - Angular velocity around the Y axis (radians per second) -* **gyro_z** - Angular velocity around the Z axis (radians per second) - -### Attitude (Orientation) -* **roll** - Roll angle in degrees -* **pitch** - Pitch angle in degrees -* **yaw** - Yaw angle in degrees -* **qw** - Quaternion scalar component -* **qx** - Quaternion X component -* **qy** - Quaternion Y component -* **qz** - Quaternion Z component - -### Magnetometer -* **mag_x** - Magnetic field strength in the X direction (micro teslas) -* **mag_y** - Magnetic field strength in the Y direction (micro teslas) -* **mag_z** - Magnetic field strength in the Z direction (micro teslas) - -### Barometric Pressure -* **pressure** - Atmospheric pressure measurement (milli bar) -* **relativeAltitude** - Relative altitude measurement (meters) - -### Gravity Vector (Computed) -* **grav_x** - Gravitational acceleration in the X direction (meters per second squared) -* **grav_y** - Gravitational acceleration in the Y direction (meters per second squared) -* **grav_z** - Gravitational acceleration in the Z direction (meters per second squared) +Every simulation mode reads one CSV layout: the export format of the +[Sensor Logger](https://www.tszheichoi.com/sensorlogger) phone app, with one row per sample. In +the library a row is a `TestDataRecord` (`strapdown::sim`), and `TestDataRecord::from_csv` is +the loader `strapdown-sim` calls for every input file. `strapdown-sim syn` writes the same +layout, so a synthetic trajectory and a phone recording are interchangeable inputs. + +## Columns + +The header must name **all 31 columns**; their order does not matter, and extra columns are +ignored. A *cell* may be empty, `NaN` or `null`, all of which read as NaN, so a channel a +recording lacks is expressed by leaving its cells blank -- not by dropping its column. A file +missing a column parses no rows at all and is refused with `No usable records read from ...`; +run with `--log-level warn` to see the per-row reason (``missing field `bearingAccuracy` ``, for +example). + +| Column | Unit | Meaning | +|---|---|---| +| `time` | -- | timestamp with a UTC offset, see [Timestamps](#timestamps) | +| `latitude`, `longitude` | degrees | GNSS position, WGS84 | +| `altitude` | m | GNSS height | +| `speed` | m/s | GNSS ground speed | +| `bearing` | degrees | GNSS ground track, clockwise from north | +| `horizontalAccuracy` | m | one-sigma horizontal position accuracy the receiver reports | +| `verticalAccuracy` | m | one-sigma vertical position accuracy | +| `speedAccuracy` | m/s | one-sigma speed accuracy | +| `bearingAccuracy` | degrees | one-sigma bearing accuracy | +| `acc_x`, `acc_y`, `acc_z` | m/s² | accelerometer specific force, body frame, gravity **included** | +| `gyro_x`, `gyro_y`, `gyro_z` | rad/s | gyroscope angular rate, body frame | +| `qw`, `qx`, `qy`, `qz` | -- | device attitude quaternion | +| `roll`, `pitch`, `yaw` | rad | device attitude as Euler angles, in the app's own convention | +| `mag_x`, `mag_y`, `mag_z` | µT | magnetometer, body frame | +| `relativeAltitude` | m | barometric altitude change since the recording started | +| `pressure` | mbar in a Sensor Logger export; Pa in `syn` output | barometric pressure | +| `grav_x`, `grav_y`, `grav_z` | m/s² | the device's gravity estimate, body frame | + +The IMU data is raw: the accelerometer columns include gravity, and the strapdown mechanization +removes it during propagation. Do not feed it gravity-compensated "linear acceleration". + +## Which columns drive what + +Not every column is read by an estimator. This is what each one feeds, taken from +`build_event_stream` (`core/src/messages.rs`), which turns records into the events `cl` and `pf` +process, and from the initialization code all three modes share. + +| Columns | Used for | When missing (NaN) | +|---|---|---| +| `acc_*`, `gyro_*` | one IMU propagation step per record | `cl`/`pf`: the step is skipped and the filter coasts across the gap; a gap longer than `aiding.max_imu_gap_s` (5 s) is an error. `dr`: the run fails | +| `latitude`, `longitude`, `altitude`, `speed`, `bearing` | one GNSS position-and-velocity fix, subject to the GNSS scheduler and fault model; also the initial position and horizontal velocity | no fix from that record -- all five must be present -- and the scheduler waits for the next usable one | +| `horizontalAccuracy` | the fix's horizontal one-sigma, floored at 1 mm | 15 m | +| `verticalAccuracy` | the fix's vertical one-sigma, floored at 1 mm | 1000 m | +| `speedAccuracy` | the fix's velocity one-sigma, floored at 0.1 m/s | 100 m/s | +| `relativeAltitude` | one barometric altitude measurement, referenced to the first record's `altitude`, 1 Hz by default | no barometer measurement from that record | +| `mag_x`, `mag_y`, `mag_z` | one magnetometer heading measurement, with declination from the World Magnetic Model at the record's date, 1 Hz by default | no heading measurement from that record | +| `qw`, `qx`, `qy`, `qz` | the initial attitude, from the first record | an all-NaN or zero quaternion gives the identity attitude | +| `grav_*` | gravity-anomaly measurements, geophysical builds only (`--geo`) | -- | +| `bearingAccuracy`, `pressure`, `roll`, `pitch`, `yaw` | nothing; read and carried, never used by an estimator | -- | + +Points worth knowing: + +- **The accuracy columns are standard deviations**, not variances: the measurement model + squares them to build $R$. They are the only way per-fix GNSS quality reaches the filter, and + the `degraded` fault model works by scaling them (see + [Closed Loop](./closed-loop.md#gnss-faults-what-the-fixes-say)). A column of zeros does not + produce a singular $R$, because of the floors above; a column of NaN makes every fix very + loose. +- **The barometer's noise is not in the file.** There is no pressure-accuracy column, so the + barometric one-sigma comes from configuration (`aiding.baro_noise_std_m`, default 2.236 m). +- **Attitude comes from the quaternion.** The `roll`/`pitch`/`yaw` columns are radians but not in + the intrinsic XYZ sequence the library uses, and the two disagree by more than a sign on real + data, so they are not read. +- **The first record seeds every mode**: its position, its `speed`/`bearing` as north and east + velocity (zero if either is NaN), zero vertical velocity, and its quaternion. +- **Rates are taken from the timestamps.** Each IMU step uses the time since the previous + record. Barometer and magnetometer measurements are scheduled at 1 Hz by default whatever the + record rate, and GNSS at every record that carries a fix unless a scheduler says otherwise. + +## Timestamps + +`time` must carry a UTC offset. Both RFC 3339 (`2025-01-01T00:00:00Z`, which `syn` writes) and +the space-separated form Sensor Logger exports (`2025-01-01 00:00:00+00:00`) are accepted, and any +offset is converted to UTC -- an input stamped `-05:00` comes out five hours later, in UTC, in the +output. A timestamp with no offset fails to parse, and so does its row. + +Timestamps must increase strictly: the time step between records is the IMU integration +interval, and a duplicate or out-of-order timestamp gives a non-positive step, which the +mechanization rejects. + +## Rows that cannot be read + +A row that fails to parse -- a malformed number, a bad timestamp -- is skipped with a warning +(`Skipping row N due to parse error: ...`) and the rest of the file is read. A file that yields +no rows at all is an error, so a wrong schema cannot silently produce an empty result. + +## Synthetic data + +`strapdown-sim syn` writes exactly this layout, in NED, with GNSS on every row and the accuracy +columns set to the configured noise levels. Its truth is a separate file, written with +`--no-noise` in the [output format](./output-format.md) rather than this one. The details of what +`syn` puts in each column -- including that its `relativeAltitude` is derived from the noisy GNSS +altitude and its `speed`/`bearing` are noise-free -- are on +[Synthetic Trajectories](./synthetic.md#what-it-writes). ## Frame Convention @@ -77,80 +130,22 @@ Getting this wrong is not a small error: mechanizing NED records as ENU adds the to the sensed specific force instead of cancelling it, and the solution falls at 2 g. So the declaration is checked rather than trusted. Before propagating, the tool rotates the leading records' accelerometer readings into the navigation frame and compares the vertical component -against the sign the declared frame requires; if they contradict each other it stops with +against the sign the declared frame requires; if they contradict each other it stops. Declaring +`syn` output as ENU, for example: -``` -InvalidConfiguration { field: "is_enu", reason: "mean vertical specific force over the first -10 record(s) is +9.73 m/s^2, but NED mechanization expects -9.80 m/s^2 at rest: these look -like ENU records. ... Declare the frame that matches the data (pass `--enu`, ...)" } +```console +$ strapdown-sim dr -i synthetic.csv -o results/wrong.csv --enu +Error: InvalidConfiguration { field: "is_enu", reason: "mean vertical specific force over the first 10 record(s) is -9.75 m/s^2, but ENU mechanization expects +9.78 m/s^2 at rest: these look like NED records. Mechanizing them as ENU would double-count gravity and integrate at 2 g. Declare the frame that matches the data (drop `--enu`, or set `is_enu = false` in the config file), or re-record it in ENU." } ``` The check only rejects; it never guesses. It is deliberately generous -- it takes roughly 1.5 g of sustained downward acceleration for a correctly declared file to trip it, so free fall and ordinary manoeuvring are fine -- and it passes anything ambiguous through. -## Data Directory Structure - -The project organizes data files into several directories based on processing type and GNSS condition: - -### Input Data -- **`input/`**: Contains pre-processed input files ready for processing, along with route visualization images. Raw recordings are available upon request but not stored in version control due to size. - -### Ground Truth -- **`truth/`**: Contains ground truth data files of processed trajectories used for validation and testing. These files serve as reference data for comparing alternative processing methods and navigation algorithms. - -### GNSS Degradation Scenarios - -The following directories contain processed trajectory data that simulate various degraded GNSS conditions: - -- **`degraded/`**: Simulates degraded GPS conditions with reduced accuracy -- **`spoofed/`**: Simulates GPS spoofing with fixed position offsets to mislead the navigation system -- **`intermittent/`**: Simulates intermittent GPS with periodic outages -- **`combo/`**: Simulates a combination of degraded, spoofed, and intermittent GPS conditions - -## Usage - -### Loading Data - -The `strapdown-core` library provides functions to load CSV data: - -```rust -use strapdown_core::sim::load_test_data; - -let data = load_test_data("path/to/data.csv")?; -``` - -### Testing with Degraded GNSS - -To test alternative processing methods and navigation algorithms under degraded conditions: - -1. Design your navigation algorithm to leverage the available data in the input files -2. Configure your experiment's `AidingConfig` to match the specific characteristics of the degraded condition you want to simulate -3. Process your experiment accordingly - -### Example Usage - -```bash -# Run closed-loop simulation with input data -strapdown-sim cl -i data/input/trajectory.csv -o results/output.csv - -# Test with degraded GNSS -strapdown-sim cl -i data/degraded/trajectory.csv -o results/degraded_output.csv -``` - -## Data Collection - -Data can be collected using the **Sensor Logger** mobile application: -- Available for iOS and Android -- Records synchronized IMU and GNSS data -- Exports in CSV format compatible with this project -- Website: https://www.tszheichoi.com/sensorlogger - -## Notes +## Collecting data -- Not all columns need to be present in every file -- Missing values are typically represented as empty strings or `NaN` -- The simulation functions will skip rows with invalid or missing critical data (e.g., IMU measurements) -- Timestamps should be monotonically increasing -- IMU data is typically recorded at ~100 Hz -- GNSS data is typically recorded at ~1 Hz +Sensor Logger is available for iOS and Android. Its export holds one file per sensor, which has +to be merged onto a single timeline with one row per sample, in the column set above, before +`strapdown-sim` can read it. The repository's `analysis` package does that merge -- along with +resampling and splitting recordings at IMU dropouts -- for its own recordings; see +[Python Analysis Tooling](../development/analysis.md). diff --git a/book/src/user-guide/dead-reckoning.md b/book/src/user-guide/dead-reckoning.md new file mode 100644 index 00000000..57d64207 --- /dev/null +++ b/book/src/user-guide/dead-reckoning.md @@ -0,0 +1,102 @@ +# Dead Reckoning + +`strapdown-sim dr` propagates the strapdown mechanization from the first record using the IMU +alone. No GNSS, barometer or magnetometer measurement is applied, and there is no filter: the +output is the free-inertial solution, with all of its drift. + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim dr -i synthetic.csv -o results/dr.csv +``` + +```console +[INFO] - Running dead reckoning simulation on 6000 records +[INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +[INFO] - Generated 6000 navigation results +[INFO] - Results written to results/dr.csv +``` + +(Timestamps trimmed from the log lines.) The flags are only the shared ones -- `-i`, `-o`, +`--enu` and the limit flags -- described in [Running Simulations](./simulations.md). `dr` +takes no seed, because nothing in it is random. + +## What it computes + +The initial state is taken from the first record: + +| State | Source in the first record | +|---|---| +| latitude, longitude, altitude | `latitude`, `longitude`, `altitude` | +| north and east velocity | `speed` and `bearing` (a NaN in either gives zero) | +| vertical velocity | zero; the format carries no vertical rate | +| attitude | the quaternion `qw, qx, qy, qz` (not the `roll`/`pitch`/`yaw` columns) | + +Every later record contributes one IMU step, with the time step taken from consecutive +timestamps, through the same mechanization the filters use (Groves §5.4-5.5; see +[The Navigation Model](./concepts.md)). The output has **one row per input record**, the first +being the initial state. + +Because there is no filter, there is no covariance. In the output CSV the bias columns are zero, +every covariance column is `NaN`, and the six optional bias columns are empty; see +[Output Format](./output-format.md). + +Three practical consequences of having no filter: + +- **Every record must carry all six inertial channels.** The filters skip a record with a + missing accelerometer or gyroscope value and coast across the gap; `dr` has no notion of a gap + and fails on it. Blanking a single `acc_x` cell in a synthetic file stops the run with + `Error: NonFinite { what: "propagated attitude matrix" }`, where `cl` on the same file logs a + tolerated 0.2 s gap and finishes. +- **The limit flags are inert.** `dr` accepts `--max-wall-clock-*`, `--health-*` and + `--nis-pos-*` and applies none of them, so a run that drifts a long way is written out in full + instead of being cut off. +- **The frame check still runs.** Declaring the wrong frame is the one mistake that would make + dead reckoning fail at 2 g rather than drift, so it is refused before propagation exactly as in + the other modes; see [Input Data Format](./data-format.md#frame-convention). + +## What it is good for + +- **An error-growth baseline.** Dead reckoning is the reference every aided result is measured + against: the gap between a `dr` run and a `cl` run on the same input is what the aiding bought. +- **Checking an IMU and a dataset.** A wrong frame, a units mistake or a mis-scaled channel shows + up in the first seconds of a `dr` run, before any filter tuning can hide it. +- **Seeing what a sensor grade means.** On synthetic data the only error source is the IMU model + `syn --imu-grade` selects, so the same trajectory at different grades shows how quickly each + grade drifts; see [Synthetic Trajectories](./synthetic.md#imu-grades). + +## How much it drifts + +The repository's accuracy suite carries one dead-reckoning scenario, `syn_dead_reckoning`: the +first 120 s of a synthetic 50 m/s cruise with a consumer-grade IMU, scored against the +trajectory's exact truth. Its current numbers are the `syn_dead_reckoning` rows of the tables +under [Performance Baselines: Current values](../development/performance.md#current-values), +which are regenerated by `core/tests/perf_baseline.rs` rather than written by hand. + +The same input can be produced from the command line. These two commands write the scenario's +sensor records and its truth; dead reckoning the first and scoring it against the second +reproduces that row's vertical RMSE and vertical bias exactly: + +```bash +strapdown-sim syn -o cruise.csv --duration-s 120 --sample-rate-hz 50 \ + --latitude-deg 40 --longitude-deg=-75 --altitude-m 200 \ + --velocity-north-mps 40 --velocity-east-mps 30 --yaw-deg 36.86989764584402 \ + --gnss-horizontal-noise-m 3 --gnss-vertical-noise-m 5 --baro-noise-std-pa 30 --seed 42 +strapdown-sim syn -o cruise_truth.csv --no-noise --duration-s 120 --sample-rate-hz 50 \ + --latitude-deg 40 --longitude-deg=-75 --altitude-m 200 \ + --velocity-north-mps 40 --velocity-east-mps 30 --yaw-deg 36.86989764584402 \ + --gnss-horizontal-noise-m 3 --gnss-vertical-noise-m 5 --baro-noise-std-pa 30 --seed 42 +strapdown-sim dr -i cruise.csv -o cruise_dr.csv +``` + +The suite deliberately stops at 120 s. Unaided consumer-grade dead reckoning over a long +recording runs away -- its altitude eventually reaches a value at which the transport-rate +denominator $r_e + h$ approaches zero, where whether the numbers stay finite is a floating-point +accident that has differed between platforms. Expect a long `dr` run on MEMS data to diverge, +and read it as a demonstration of why aiding exists rather than as a navigation result. + +## `ol` is not dead reckoning + +The `ol` subcommand is a separate, **unimplemented** mode. It validates its paths, creates the +output directory, prints `Open-loop mode is not yet fully implemented` and writes nothing. Dead +reckoning is `dr`. In a configuration file the corresponding mode names are `dead-reckoning` and +`open-loop`; the latter exits with an error. diff --git a/book/src/user-guide/library.md b/book/src/user-guide/library.md new file mode 100644 index 00000000..9cbf172b --- /dev/null +++ b/book/src/user-guide/library.md @@ -0,0 +1,304 @@ +# Using the Library + +`strapdown-sim` is one consumer of `strapdown-core`; your own program can be another. This page +covers the library's user-facing layer: the `InsEngine` most programs should start from, the +pieces that sit in front of it (calibration, coarse alignment, an initial covariance from the +IMU grade, the antenna lever arm), error handling, and scoring a run against truth. + +The package is `strapdown-core` and the crate is imported as **`strapdown`**. Until v1.0.0 is on +crates.io it is added from git; [Installation](../installation/installation.md) has the exact +`Cargo.toml` line and the feature list. + +Every Rust snippet on this page is included from an example under `core/examples/` that is +compiled and linted with the rest of the workspace: + +| example | shows | +| --- | --- | +| `basic_ins` | the `InsEngine` loop | +| `gnss_outage` | coasting through a GNSS outage | +| `lever_arm` | calibration, alignment, `auto_covariance`, the lever arm, recoverable errors | +| `kalman_filters` | the three Kalman filters behind `NavigationFilter` | +| `eskf` | `initialize_eskf`, an event stream, `run_closed_loop`, `metrics::evaluate` | +| `aiding` | ZUPT, ZARU, barometer, magnetometer, innovation gating | +| `score_run` | scoring a `strapdown-sim` output CSV against a truth CSV | + +```bash +cargo run -p strapdown-core --example lever_arm +``` + +## The `InsEngine` + +`engine::InsEngine` (re-exported at the crate root) owns a `NavigationFilter` (the 15-state +[ESKF](../filters/eskf.md) by default), advances it at the IMU rate, corrects it whenever aiding +arrives, and reports a `NavSolution` in degrees, metres and metres per second rather than the +radians the filter carries. The loop is the same whatever the data source: + +1. build an engine from an initial state; +2. `predict` for every IMU sample; +3. `update_gnss` (or `update`) whenever a measurement arrives; +4. read `nav_solution` whenever you need the estimate. + +From [`core/examples/basic_ins.rs`](https://github.com/jbrodovsky/strapdown-rs/blob/main/core/examples/basic_ins.rs), +which [Tutorial: Basic INS](../examples/tutorial-basic.md) walks through: + +```rust,ignore +{{#include ../../../core/examples/basic_ins.rs:build}} +``` + +```rust,ignore +{{#include ../../../core/examples/basic_ins.rs:loop}} +``` + +Aiding is asynchronous: the engine's clock advances only on `predict`, and an update applies at +whatever epoch the last `predict` left it at. Propagate up to a measurement's time of validity +before applying it. + +### Builder and configuration + +`InsEngine::builder()` returns an `InsEngineBuilder`; every setter is optional. + +| builder method | sets | +| --- | --- | +| `with_initial_state(InitialState)` | the initial state; its `is_enu` becomes the engine's frame unless one was named | +| `with_frame(is_enu)` | the frame explicitly; a contradicting initial state is then a build error | +| `with_config(InsEngineConfig)` | the whole configuration (counts as naming a frame) | +| `with_lever_arm([x, y, z])` | antenna offset, metres, body frame | +| `with_initial_covariance(Vec)` | $P_0$, 15 entries | +| `with_process_noise(Vec)` | the process-noise density $q$, 15 entries | +| `with_imu_biases(accel, gyro)` | initial bias estimates, m/s² and rad/s | +| `with_filter(Box)` | drive a filter you built yourself instead of the default ESKF | +| `build()` | returns `Result` | + +`build` returns `StrapdownError::InvalidConfiguration` for a non-finite lever arm or one longer +than 100 m, a covariance or noise diagonal that is not 15 finite non-negative entries, a +contradiction between the configured frame and the initial state's, or a supplied filter with +fewer than nine states. With `with_filter`, the covariance, noise and bias settings are not +applied (the filter was built with its own); the frame and lever arm still are. + +`InsEngineConfig` holds the same settings as a serializable struct (`is_enu`, `lever_arm`, +`process_noise_diagonal`, `initial_covariance_diagonal`, `initial_accel_bias`, +`initial_gyro_bias`). It is `#[non_exhaustive]`: start from `InsEngineConfig::default()` (NED, no +lever arm, default noise) and assign fields. Without `with_initial_covariance`, the default ESKF +starts from a fixed $P_0$: 10 m of horizontal and vertical position, $10^{-3}$ (m/s)² of +velocity, $10^{-5}$ rad² of attitude, and $10^{-6}$ (m/s²)² and $10^{-8}$ (rad/s)² on the +biases. For a $P_0$ that describes your sensor, pass `IMUQuality::auto_covariance` +([below](#imu-grades)). + +### Running the engine + +| method | does | +| --- | --- | +| `predict(&ImuSample)` | propagate by one sample of increments | +| `predict_rates(&IMUData, dt)` | the same from rates, through `ImuSample::from_rates` | +| `update_gnss(&GnssFix)` | apply an antenna-referred fix: position, then velocity if `GnssFix::with_velocity` gave one | +| `update(&dyn MeasurementModel)` | apply any other [measurement model](../filters/measurements.md) | +| `set_innovation_gate`, `set_gate_recovery` | install a gate and tune its recovery ([gating](../filters/measurements.md#innovation-gating)) | +| `nav_solution()` | the current `NavSolution` | +| `covariance()` | the filter's covariance, in its own units | +| `set_lever_arm([x, y, z])` | change the antenna offset on a running engine, validated like the builder | +| `filter()` | borrow the underlying `&dyn NavigationFilter` | + +`update_gnss` and `update` return an [`UpdateOutcome`](../filters/kalman.md#outputs-updateoutcome). +When a gate rejects the position leg of a fix, its velocity leg is skipped too. + +`NavSolution` carries `elapsed_s`; `latitude` and `longitude` (degrees); `altitude` (m, above +the ellipsoid); `velocity_north`, `velocity_east` and `velocity_vertical` (m/s; vertical is +positive down in NED); `roll`, `pitch` and `yaw` (degrees, yaw on -180..180, with +`heading_deg()` for 0..360); `accel_bias` and `gyro_bias` (zero if the filter carries no bias +states); `position_std_m` and `velocity_std_mps` (one-sigma north, east and vertical, converted +to metres); and `is_enu`. + +## Antenna lever arm + +The filter estimates the IMU's position, but a receiver reports its antenna's. The two differ by +the body-frame offset $r^b_{ant}$ rotated into the navigation frame, and a rotating vehicle adds +a velocity difference: + +$$ +p_{GNSS} = p_{IMU} + C_b^n r^b_{ant}, \qquad +v_{GNSS} = v_{IMU} + C_b^n \left(\omega^b_{ib} \times r^b_{ant}\right) . +$$ + +Set the lever arm (`with_lever_arm`, `InsEngineConfig::lever_arm` or `set_lever_arm`) and +`update_gnss` inverts both relations before the filter sees the fix, using the current attitude +estimate and the bias-corrected angular rate from the last `predict`. The offset is measured +from the IMU centre to the antenna phase centre, in metres, along the IMU's own axes (x forward, +y right, z down). A zero lever arm, the default, makes the compensation an exact identity. +`update` applies no lever arm: another sensor's mounting geometry belongs to its own model. The +free functions `engine::lever_arm_position_offset`, `lever_arm_velocity_offset` and +`shift_position_by_offset` expose the geometry. + +[`core/examples/lever_arm.rs`](https://github.com/jbrodovsky/strapdown-rs/blob/main/core/examples/lever_arm.rs) +puts the antenna 1.5 m ahead of and 0.8 m above a stationary, tilted IMU, and feeds the same +antenna fixes to an engine that knows the lever arm and to one that does not: + +```rust,ignore +{{#include ../../../core/examples/lever_arm.rs:engine}} +``` + +```rust,ignore +{{#include ../../../core/examples/lever_arm.rs:run}} +``` + +The compensated engine places the IMU where it is; the uncompensated one places it at the +antenna (the [output](#output-of-lever_armrs) is at the end of this page). + +## IMU calibration + +`calibration::ImuCalibration` removes known, deterministic sensor errors before the +mechanization. It uses Groves's sensor model (Section 4.4.1, Eq. 4.17 and 4.18), + +$$ +\tilde f^b = b + (I + M) f^b + w , +$$ + +stored as the forward parameters a calibration report quotes: a bias $b$, per-axis scale-factor +errors (the diagonal of $M$) and misalignment coefficients (the off-diagonal of $M$, with a zero +diagonal). The model is inverted once, at construction, and `SensorCalibration::new` returns an +error for non-finite values, a non-zero misalignment diagonal or a singular $I + M$. If you have +the whole $M$ as one matrix, use `SensorCalibration::from_error_matrix`. The random noise $w$ +cannot be removed; that is what the filter's process noise is for. + +```rust,ignore +{{#include ../../../core/examples/lever_arm.rs:calibration}} +``` + +`correct(&ImuSample)` applies it to increments, where the bias enters as $b\ \Delta t$; +`correct_rates(&IMUData)` to rates. Neither the engine nor `strapdown-sim` calls it: apply it to +each sample yourself before `predict`, as `lever_arm.rs` does. + +## Coarse alignment + +A strapdown solution is relative to the attitude it starts from. `alignment` implements the +self-alignment estimators of Groves Section 5.6.3: + +| function | estimates | needs | +| --- | --- | --- | +| `average_imu(&[IMUData])` | the mean of a window | samples you trust to be stationary | +| `coarse_leveling(&specific_force)` | roll and pitch (`LevelAttitude`) | a stationary accelerometer reading | +| `gyrocompassing(&imu, latitude_deg, IMUQuality, &GyrocompassConfig)` | heading and its one-sigma (`HeadingEstimate`) | a stationary gyro good enough to see Earth rate | +| `heading_from_velocity(v_n, v_e, minimum_speed)` | heading | the vehicle moving faster than `minimum_speed` | +| `attitude_from_level_and_heading(level, heading)` | a `Rotation3` | | + +**Every function is NED**: a level IMU reads $-g$ on its down axis, and an ENU reading has to be +reflected first. `gyrocompassing` predicts its own heading error from the grade's gyro bias +instability and refuses, with `MeasurementUnavailable`, when that exceeds +`GyrocompassConfig::maximum_heading_uncertainty_radians` (45° by default). A consumer or +industrial MEMS gyro cannot gyrocompass, and the function says so instead of returning a number. +It also refuses above 75° of latitude by default. The functions take one averaged sample and do +not check for motion themselves; choosing stationary samples is the +[`StationaryDetector`](../filters/measurements.md#zero-velocity-and-zero-angular-rate-updates)'s +job: + +```rust,ignore +{{#include ../../../core/examples/lever_arm.rs:alignment}} +``` + +`lever_arm.rs` then seeds the engine from that attitude and widens the yaw entry of $P_0$ by the +heading uncertainty the gyrocompass reported (the `engine` snippet above). + +## IMU grades + +`IMUQuality` (`Consumer`, the default; `Industrial`; `Tactical`; `Navigation`; `Strategic`) +carries representative figures for each grade: + +| grade | gyro bias instability (°/h) | gyro ARW (°/√h) | accel bias instability (m/s²) | accel VRW (m/s/√h) | +| --- | --- | --- | --- | --- | +| `Consumer` | 100 | 1.0 | 0.1 | 0.1 | +| `Industrial` | 50 | 0.1 | 0.05 | 0.03 | +| `Tactical` | 1 | 0.01 | 0.001 | 0.01 | +| `Navigation` | 0.01 | 0.005 | 0.0001 | 0.005 | +| `Strategic` | 0.0001 | 0.0005 | 0.00001 | 0.0001 | + +The accessors return the gyro figures in radians (`gyro_bias_instability_rad_per_hour`, +`gyro_angle_random_walk`). The grade is used in three places: + +- `initial_bias_covariance()`: the six bias variances a filter should open with, each the + grade's bias instability squared (the gyro converted to rad/s). The `initialize_*` helpers use + it through their `imu_quality` field. +- `auto_covariance(InitialUncertainty, latitude_deg, altitude_m)`: a whole fifteen-entry $P_0$, + derived as tabulated on [Kalman Filters](../filters/kalman.md#the-constructors-with-p0-from-the-imu-grade). + `InitialUncertainty::default()` is 2.5 m horizontal, 5 m vertical and 0.5 m/s. +- `strapdown-sim syn --imu-grade`: the noise and bias levels of a synthetic trajectory. + +`velocity_process_noise(dt)` and `attitude_process_noise(dt)` return the variance the grade's +random walks accumulate over `dt` seconds. The filters take a variance **per second** +([process noise](../filters/kalman.md#process-noise-is-a-spectral-density)), which is the value +these return at `dt = 1.0`. `gyro_process_noise` and `accel_process_noise` are deprecated: a bias +instability belongs in $P_0$, not in $Q$. + +## Errors + +Every fallible function returns `StrapdownError` (`core/src/error.rs`, re-exported at the root). +Library code does not unwrap, expect or panic; the lints that forbid it are set to deny. The +enum is `#[non_exhaustive]`, and one method sorts its variants into the two kinds a caller has to +tell apart: + +| `is_recoverable()` | variants | meaning | +| --- | --- | --- | +| `true` | `OutOfMapBounds`, `MeasurementUnavailable`, `ExternalModel` | this one measurement could not be used; the filter state is untouched and valid, so skip it and continue | +| `false` | `DimensionMismatch`, `NotSquare`, `OutOfRange`, `NonFinite`, `UnsupportedInput`, `SingularMatrix`, `InconsistentTimestep`, `InvalidConfiguration`, `MapLoad`, `CovarianceDiagonal`, `FilterDiverged`, `SensorStreamGap`, `Timeout` | bad input or configuration, or a state that can no longer be trusted; stop rather than emit numbers that look like a trajectory | + +A gated-out measurement is not an error at all: it is `Ok` with `accepted == false`. +`sim::run_closed_loop` follows the same rule, counting and skipping recoverable failures. In your +own loop: + +```rust,ignore +{{#include ../../../core/examples/lever_arm.rs:recoverable}} +``` + +`StrapdownError` converts into `anyhow::Error`, so `?` composes with the file-I/O functions in +`sim`. + +## Scoring a run against truth + +`metrics::evaluate(&estimates, &truth, MetricOptions)` scores a `Vec` against a +truth series and returns `AccuracyMetrics`: horizontal RMSE, CEP50, CEP95 and maximum; vertical +RMSE and signed bias; horizontal and vertical velocity RMSE; roll, pitch, yaw and geodesic +attitude RMSE; the position NEES and its diagonal-only form; and three-sigma containment. Each is +an `Option`, `None` when the run cannot support it (a dead-reckoning run has no covariance). It +is the same reduction `core/tests/perf_baseline.rs` gates on. + +The truth comes from one of two places, and they are not equally good: + +- `metrics::truth_from_trajectory(&truth)` takes the exact trajectory `sim::generate_synthetic` + returns, or that `strapdown-sim syn --no-noise` writes. This is ground truth. +- `metrics::truth_from_records(&records)` takes the GNSS fixes of a recorded log. On any log + whose fixes also aided the filter, the result measures agreement with the aid, and it cannot + fall below the receiver's own noise. + +Estimates and truth are matched by timestamp; `MetricOptions` sets the largest gap allowed +(`max_match_gap_s`, zero by default) and a number of leading samples to discard +(`warmup_samples`). From `core/examples/eskf.rs`: + +```rust,ignore +{{#include ../../../core/examples/eskf.rs:score}} +``` + +`core/examples/score_run.rs` does the same for two CSV files on disk; see +[Tutorial: Particle Filter](../examples/tutorial-particle-filter.md#comparing-against-truth). + +## Below the engine + +- **Filters directly.** Construct any filter and drive it through `NavigationFilter`; see + [Kalman Filters](../filters/kalman.md) and the [RBPF](../filters/rbpf.md). +- **Simulation runners in `sim`.** `TestDataRecord::from_csv` loads a Sensor Logger CSV; + `dead_reckoning(&records, is_enu)` mechanizes it unaided; `messages::build_event_stream` plus + `run_closed_loop`, with a filter from `initialize_eskf`, `initialize_ekf` or `initialize_ukf`, + is the closed loop `strapdown-sim cl` runs; `generate_synthetic` makes a trajectory and its + truth. [ESKF](../filters/eskf.md#running-it-end-to-end) runs the whole chain. +- **Output.** `NavigationResult::to_csv` is always available. `to_hdf5`, `to_netcdf` and + `to_mcap` exist behind the `hdf5`, `netcdf` and `mcap` features (`full` enables all three + plus `clap`); the default build has no features and needs only a Rust toolchain. The CLI + writes CSV only. + +## Output of `lever_arm.rs` + +```text +consumer grade: gyrocompassing could not produce a measurement: a Consumer-grade gyro at latitude 39.950 deg gives a predicted one-sigma heading error of 496.9 deg, above the 45.0 deg limit; its bias instability is larger than the Earth rate it would have to measure +aligned from 2852 stationary samples: roll 2.000, pitch -1.000, heading 60.000 deg (claimed 1-sigma 5.0 deg) +with lever arm : IMU position off by 0.00 m horizontally, 0.00 m vertically +without lever arm: IMU position off by 1.51 m horizontally, 0.77 m vertically +skipped: GnssFix could not produce a measurement: fix contains a non-finite component +set_lever_arm: invalid configuration for `lever_arm`: magnitude 150.000 m exceeds the 100 m limit; the offset is measured in metres in the body frame +``` diff --git a/book/src/user-guide/logging.md b/book/src/user-guide/logging.md index b71b789d..28412a69 100644 --- a/book/src/user-guide/logging.md +++ b/book/src/user-guide/logging.md @@ -58,13 +58,18 @@ Log messages are formatted as: YYYY-MM-DD HH:MM:SS.mmm [LEVEL] - message ``` -Example: +Example, from `strapdown-sim pf -i synthetic.csv -o results/pf.csv`: ``` -2025-12-01 14:30:45.123 [INFO] - Read 1000 records from data/input.csv -2025-12-01 14:30:45.456 [INFO] - Running particle filter with 100 particles -2025-12-01 14:30:50.789 [INFO] - Results written to output.csv +2026-10-01 20:33:18.027 [INFO] - Read 6000 records from synthetic.csv +2026-10-01 20:33:18.028 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +2026-10-01 20:33:22.501 [INFO] - Results written to results/pf.csv ``` +At `info`, a closed-loop run also logs a progress line every few events (position, velocity +and their sigmas), which makes `info` verbose on long runs; `warn` keeps the end-of-run +summaries -- such as how many measurements an innovation gate rejected -- and the per-row +warnings about input rows that could not be parsed. + ## `RUST_LOG` is not read This page used to document `RUST_LOG` as an override that `--log-level` took precedence over. @@ -88,25 +93,20 @@ equivalent here. For developers extending the project, use the logging macros from the `log` crate: -```rust +```rust,ignore use log::{trace, debug, info, warn, error}; -// Informational messages info!("Processing {} records", count); - -// Warnings warn!("Skipping row {} due to parse error", row_num); - -// Errors error!("Failed to read config file: {}", err); - -// Debug information -debug!("UKF state: {:?}", ukf.get_mean()); - -// Detailed traces +debug!("filter state: {:?}", state); trace!("Entering function with params: {:?}", params); ``` +The library emits its own messages through the same macros, so anything `strapdown-core` logs +appears in `strapdown-sim`'s output, formatted the same way. A program that uses the library +directly sees those messages only if it installs a `log` backend of its own. + ## Python Logger Comparison This logging system provides a similar experience to Python's logging module: @@ -120,9 +120,29 @@ This logging system provides a similar experience to Python's logging module: | `--log-level info` | `--log-level info` | | Writing to file with FileHandler| `--log-file path/to/file.log` | +## Logging from a configuration file + +A configuration file can set the same two things in a `[logging]` section: + +```toml +[logging] +level = "debug" +file = "results/run.log" +``` + +When a run uses `--config`, the command line and the file are combined: + +- `--log-file` on the command line overrides `file`. +- `--log-level` overrides `level` **only when it is not `info`**. The flag's default is `info`, + so passing `--log-level info` explicitly cannot be told apart from not passing it, and the + file's level wins. To quieten a file that says `debug`, pass `--log-level warn`, not + `--log-level info`. + ## Notes -- Log files are opened in append mode, so multiple runs will append to the same log file -- Timestamps use the local system timezone -- The logger is initialized once at program startup -- Log messages from the core library (`strapdown-core`) are also captured and formatted consistently +- Log files are opened in append mode, so multiple runs append to the same file. Missing parent + directories are created. +- Timestamps use the local system timezone. +- An unrecognized level (`--log-level loud`) prints `Invalid log level 'loud', defaulting to + 'info'` and runs at `info`. +- The logger is initialized once at program startup. diff --git a/book/src/user-guide/output-format.md b/book/src/user-guide/output-format.md new file mode 100644 index 00000000..e76e73f2 --- /dev/null +++ b/book/src/user-guide/output-format.md @@ -0,0 +1,114 @@ +# Output Format + +Every simulation mode -- `dr`, `cl`, `pf` -- and `syn --no-noise` write the same thing: a CSV +with one `NavigationResult` (`strapdown::sim`) per row, written by `NavigationResult::to_csv`. +The schema is fixed at 40 columns whatever the mode, so files from different estimators line up +column for column; a mode that has nothing to put in a column leaves a defined placeholder rather +than dropping it. + +**The command line writes CSV and nothing else.** HDF5, NetCDF and MCAP writers exist as library +methods (see [below](#other-formats-library-only)); no `strapdown-sim` flag or file extension +reaches them. There is no Parquet support at either layer. + +## Rows + +There is one row per input record, stamped with that record's time in UTC +(`2025-01-01T00:09:59.900Z`). A row stamped $t_k$ holds the estimate after every event at or +before $t_k$ -- the IMU step ending at $t_k$ and any measurement updates applied there -- and none +after. The first row is the initial state, taken from the first record. On the 600 s, 10 Hz +synthetic trajectory used throughout this guide, `dr`, `cl` and `pf` each write 6,000 rows. + +## Columns + +In file order: + +| Column(s) | Unit | Meaning | +|---|---|---| +| `timestamp` | RFC 3339, UTC | epoch of the row | +| `latitude`, `longitude` | degrees | WGS84 position | +| `altitude` | m | height above the WGS84 ellipsoid, positive up in both frames | +| `velocity_north`, `velocity_east` | m/s | horizontal velocity | +| `velocity_vertical` | m/s | vertical velocity: positive **down** for an NED run, positive **up** for an ENU run | +| `roll`, `pitch`, `yaw` | rad | attitude; roll and yaw on $[-\pi, \pi]$, yaw negative west of north | +| `acc_bias_x`, `acc_bias_y`, `acc_bias_z` | m/s² | estimated accelerometer bias, body frame | +| `gyro_bias_x`, `gyro_bias_y`, `gyro_bias_z` | rad/s | estimated gyroscope bias, body frame | +| `latitude_cov`, `longitude_cov` | rad² | position variances | +| `altitude_cov` | m² | altitude variance | +| `latitude_longitude_cov` | rad² | position covariance | +| `latitude_altitude_cov`, `longitude_altitude_cov` | rad·m | position covariances | +| `velocity_n_cov`, `velocity_e_cov`, `velocity_v_cov` | (m/s)² | velocity variances | +| `roll_cov`, `pitch_cov`, `yaw_cov` | rad² | attitude variances | +| `acc_bias_x_cov`, `acc_bias_y_cov`, `acc_bias_z_cov` | (m/s²)² | accelerometer-bias variances | +| `gyro_bias_x_cov`, `gyro_bias_y_cov`, `gyro_bias_z_cov` | (rad/s)² | gyroscope-bias variances | +| `gravity_bias`, `gravity_bias_cov` | mGal, mGal² | gravity-map bias and its variance (geophysical runs only) | +| `magnetic_bias`, `magnetic_bias_cov` | nT, nT² | magnetic-map bias and its variance (geophysical runs only) | +| `baro_bias`, `baro_bias_cov` | m, m² | barometric bias and its variance | + +Two things to watch: + +- **Position and its covariance are in different units.** `latitude` and `longitude` are + degrees, but `latitude_cov` and the other position covariances are the filter's raw variances + in radians. To score a position error against its covariance, convert the error to radians; + do not convert the variance to degrees. +- **Only the position block is a full covariance.** The six position entries are the whole + symmetric 3×3 block, which is what a proper NEES needs. Every other `_cov` column is a + diagonal element only. + +## Empty and placeholder cells + +The last six columns are optional, and an empty cell means "this run did not estimate that +state", which is a different fact from an estimate of zero. + +| Written by | Bias columns | Covariance columns | `gravity_*`, `magnetic_*` | `baro_bias*` | +|---|---|---|---|---| +| `dr` | `0` | `NaN` -- dead reckoning has no covariance | empty | empty | +| `cl` | estimated | estimated | empty unless `--geo` | estimated; empty with `--no-estimate-baro-bias` | +| `pf` | `0`, with `0` variance -- the RBPF has no IMU-bias states | estimated | empty unless `--geo` | empty -- the RBPF has no barometric bias state | +| `syn --no-noise` | `0` | `0` | empty | empty | + +## Reading results back + +`NavigationResult::from_csv` reads one of these files back into the library type. The example +`core/examples/score_run.rs` uses it to score a run against the truth `syn --no-noise` writes, +with the same metrics the accuracy suite reports: + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim syn -o synthetic_truth.csv --duration-s 600 --seed 42 --no-noise +strapdown-sim cl -i synthetic.csv -o results/eskf.csv +cargo run -p strapdown-core --example score_run -- results/eskf.csv synthetic_truth.csv +``` + +The metrics it prints are defined on [Performance Baselines](../development/performance.md#the-metrics). +Any CSV reader works too: the header is a single row of the column names above, and the empty +optional cells read as missing values in pandas or MATLAB. + +## Other formats (library only) + +`NavigationResult` has three more writers, each with a matching reader, each behind a cargo +feature of `strapdown-core` that is off by default: + +| Method | Reader | Feature | Layout | +|---|---|---|---| +| `NavigationResult::to_hdf5` | `from_hdf5` | `hdf5` | group `navigation_results`, one 1-D dataset per column; `timestamp` as RFC 3339 strings | +| `NavigationResult::to_netcdf` | `from_netcdf` | `netcdf` | dimension `time`, one variable per column; `timestamp` as Unix seconds | +| `NavigationResult::to_mcap` | `from_mcap` | `mcap` | channel `navigation_results`, one MessagePack-encoded record per message, logged at the row's timestamp | + +All three carry the same 40 columns. HDF5 and NetCDF have no optional type, so an absent bias +column is written as `NaN` and read back as absent. The NetCDF `timestamp` is stored as whole +seconds, so rows less than a second apart share a value there; use HDF5 or MCAP when sub-second +times matter. + +Enable a writer on the dependency, from git until 1.0 is published: + +```toml +[dependencies] +strapdown-core = { git = "https://github.com/jbrodovsky/strapdown-rs", features = ["hdf5"] } +``` + +`hdf5` and `netcdf` compile libhdf5 and libnetcdf from vendored sources, so they need a C +compiler and cmake 3.26 or newer; `mcap` is Rust only. The `full` feature turns on all three +plus `clap`. See [System Requirements](../installation/requirements.md). + +`TestDataRecord`, the input record, has the same three writers and readers, behind the same +features. diff --git a/book/src/user-guide/overview.md b/book/src/user-guide/overview.md index 62017b4b..82f1df8f 100644 --- a/book/src/user-guide/overview.md +++ b/book/src/user-guide/overview.md @@ -1,151 +1,109 @@ # User Guide Overview -Welcome to the Strapdown-rs User Guide! This section provides comprehensive information on using the library and simulation tools. - -## What You'll Learn - -This guide covers: - -1. **Core Concepts**: Understanding strapdown INS, coordinate frames, and state representation -2. **Running Simulations**: How to use the `strapdown-sim` binary -3. **Data Formats**: Preparing and formatting your input data -4. **Configuration**: Setting up simulations with TOML config files -5. **Logging and Debugging**: Monitoring simulation progress - -## Quick Navigation - -### For Beginners - -If you're new to strapdown INS or this library: - -1. Start with [Core Concepts](./concepts.md) to understand the fundamentals -2. Learn about [Coordinate Frames](./coordinate-frames.md) used in the library -3. Review [State Representation](./state-representation.md) to understand the 9-state and 15-state models -4. Try the [Quick Start](../quick-start.md) tutorial - -### For Experienced Users - -If you're familiar with INS and want to jump in: - -1. Check [Input Data Format](./data-format.md) to prepare your data -2. Review [Configuration Files](./configuration.md) for advanced options -3. Explore [Running Simulations](./simulations.md) for different modes -4. See [Logging](./logging.md) for debugging and monitoring - -## Simulation Modes - -Strapdown-rs supports three main simulation modes: - -### Dead Reckoning (`dr`) - -Pure inertial navigation without corrections. Useful for: -- Understanding INS error growth -- Baseline comparisons -- Testing IMU data quality - -Note that dead reckoning is `dr`, not `ol`. The `ol` subcommand is a separate *feed-forward* -mode -- its own help describes a Kalman filter estimating and applying corrections, so it -consumes GNSS and is not "without corrections" -- and it is **not implemented**: it validates -its paths, writes no output and prints "Open-loop mode is not yet fully implemented". - -See: [Open-Loop Mode](./open-loop.md) - -### Closed-Loop (Kalman Filtering) - -INS with GNSS corrections using the ESKF (the default), UKF or EKF, selected with -`--filter`. Best for: -- Realistic navigation scenarios -- GNSS degradation studies -- Production-like simulations - -See: [Closed-Loop Mode](./closed-loop.md) - -### Particle Filter - -Non-parametric Bayesian filtering for non-Gaussian distributions. Useful for: -- Multimodal uncertainty -- Highly nonlinear scenarios -- Research applications - -See: [Particle Filter Mode](./particle-filter.md) - -## Navigation Filters - -The library provides multiple filter implementations: - -- **Error-State Kalman Filter (ESKF)**: multiplicative attitude error. **The default for `cl`.** -- **Extended Kalman Filter (EKF)**: Fast, efficient, works well for mildly nonlinear systems -- **Unscented Kalman Filter (UKF)**: Better accuracy for nonlinear systems, 2-3x slower -- **Rao-Blackwellized Particle Filter (RBPF)**: Canciani & Raquet's marginalized particle filter - -- horizontal position as particles; altitude, velocity, tilt, the barometer loop's two states - and the map biases as a Kalman filter shared by every particle. See - [the RBPF page](../filters/rbpf.md). The only particle filter implementation -- `ParticleFilterType` - has this one variant, and `particle.rs` is a module of building blocks rather than a filter - that can be selected on its own - -Learn more: [Navigation Filters](../filters/kalman.md) - -## State Models - -### 9-State Model - -The basic navigation-only model: -- **Position**: latitude, longitude, altitude -- **Velocity**: north, east, down -- **Attitude**: roll, pitch, yaw - -### 15-State Model - -Extended model with IMU bias estimation: -- 9 navigation states (as above) -- **Accelerometer biases**: 3 states -- **Gyroscope biases**: 3 states - -The 15-state model provides better long-term accuracy by estimating and correcting sensor biases. - -## Typical Workflow - -1. **Collect or prepare IMU/GNSS data** in CSV format -2. **Create a configuration file** specifying simulation parameters -3. **Run the simulation** using `strapdown-sim` -4. **Analyze results** from the output CSV -5. **Iterate** by adjusting parameters as needed - -## Common Use Cases - -### Research and Development - -- Testing new navigation algorithms -- Comparing filter performance -- Studying error characteristics -- Publishing research results - -### Education - -- Teaching INS fundamentals -- Demonstrating sensor fusion -- Illustrating error sources -- Hands-on learning - -### System Development - -- Prototyping navigation systems -- Evaluating sensor requirements -- Testing GNSS-denied scenarios -- Performance benchmarking - -## Getting Help - -- **FAQ**: Check the [Frequently Asked Questions](../faq.md) -- **Examples**: Browse [Example Configurations](../examples/configurations.md) -- **API Docs**: See [API Reference](../api/core.md) for detailed documentation -- **Issues**: Report problems on [GitHub](https://github.com/jbrodovsky/strapdown-rs/issues) - -## Next Steps - -Choose your path: - -- **New to INS?** → [Core Concepts](./concepts.md) -- **Ready to simulate?** → [Running Simulations](./simulations.md) -- **Need data format info?** → [Input Data Format](./data-format.md) -- **Want advanced features?** → [Configuration Files](./configuration.md) +This part of the book explains what the toolkit computes and how to drive it. This page is a +map: what the pieces are, and which page documents each one. + +## The crates + +| Crate | Use it for | Documented in | +| --- | --- | --- | +| `strapdown-core` (imported as `strapdown`) | Mechanization, filters, measurement models, scenario engine, CSV I/O | [Using the Library](./library.md), [API Documentation](../api/index.md) | +| `strapdown-sim` | Running simulations from the command line or a scenario file | [Running Simulations](./simulations.md) and the pages under it | +| `strapdown-geonav` | **Experimental** gravity and magnetic anomaly map aiding | [Geophysical Navigation](../geonav/overview.md) | + +The simulator is a thin layer over the library: every mode it offers is a library function +you can call from Rust. + +## The model + +Three pages describe what the numbers mean, and are worth reading before interpreting any +output: + +- [The Navigation Model](./concepts.md): strapdown mechanization in the local-level frame, + the gravity and Earth-rate models, and how aiding corrects the solution. +- [Coordinate Frames](./coordinate-frames.md): NED by default, ENU on request, the body frame, + and the check that refuses a wrongly declared frame. +- [State Representation](./state-representation.md): the nine navigation states, the filters' + 15-element bias-augmented states and their extra bias states, and how covariance reaches the + output. + +## The subcommands + +`strapdown-sim` has one subcommand per mode: + +| Subcommand | What it does | Page | +| --- | --- | --- | +| `dr` | Dead reckoning: propagates the IMU from the initial state with no aiding | [Dead Reckoning](./dead-reckoning.md) | +| `cl` | Closed loop: a Kalman filter corrects the INS with GNSS, barometer and magnetometer measurements, and feeds the corrections back. ESKF by default; `--filter ekf` or `--filter ukf` selects the others | [Closed Loop](./closed-loop.md) | +| `pf` | Closed loop with the Rao-Blackwellized particle filter | [Particle Filter](./particle-filter.md) | +| `syn` | Generates a synthetic trajectory: noisy sensor records, or with `--no-noise`, the truth | [Synthetic Trajectories](./synthetic.md) | +| `config` | An interactive wizard that writes a scenario file | [Configuration Files](./configuration.md) | +| `ol` | **Not implemented.** Reserved for an open-loop mode; it validates its paths and writes no output | -- | + +Dead reckoning is `dr`. It is not "open loop": `ol` names a different, unimplemented mode in +which a filter would estimate errors without feeding them back. + +A few options are shared: + +- **Global:** `--config ` runs a whole scenario from a TOML, YAML or JSON file instead + of subcommand flags; `--log-level` and `--log-file` control logging ([Logging](./logging.md)); + `--parallel` processes several input files at once; `--plot` draws a performance plot + against the GNSS track. +- **Every simulation mode:** `-i` takes a CSV file or a directory of them, `-o` a CSV file or a + directory; `--enu` declares ENU input; health limits (`--health-*`) and execution limits + (`--max-wall-clock-*`, `--max-no-progress-s`) stop a run that diverges or hangs. +- **`cl` and `pf`:** `--seed`, the GNSS scheduler (`--sched` and its parameters) and the GNSS + fault model (`--fault` and its parameters). + +`strapdown-sim --help` lists every flag with its default. The simulator writes +**CSV only**; see [Output Format](./output-format.md). + +## The filters + +All four implement the library's `NavigationFilter` trait, so the simulator drives them through +the same loop. + +| Filter | Selected by | State it carries | Page | +| --- | --- | --- | --- | +| Error-state Kalman filter (ESKF) | `cl` (the default) | Nominal navigation state plus a 15-element error state, 16 with the barometric bias | [ESKF](../filters/eskf.md) | +| Extended Kalman filter (EKF) | `cl --filter ekf` | Full 15-element state, plus extra bias states | [EKF](../filters/ekf.md) | +| Unscented Kalman filter (UKF) | `cl --filter ukf` | Full 15-element state, plus extra bias states | [UKF](../filters/ukf.md) | +| Rao-Blackwellized particle filter (RBPF) | `pf` | Particles over horizontal position error; one shared Kalman filter for the rest. No IMU-bias states | [RBPF](../filters/rbpf.md) | + +How they differ, and how they score on the reference scenarios, is on the +[Comparison](../filters/comparison.md) and [Performance Baselines](../development/performance.md) +pages. The RBPF is the only particle filter: `particle.rs` provides building blocks +(resampling, averaging), not a second filter ([Particle Building +Blocks](../filters/particle-filter.md)). + +## Aiding and degradation + +- **Measurement models** -- GNSS position and velocity, barometric altitude, magnetometer + heading, and the library's zero-velocity and zero-angular-rate updates -- and innovation + gating: [Measurement Models and Integrity](../filters/measurements.md). All aiding is loosely + coupled; there are no pseudorange or carrier-phase models. +- **GNSS degradation** -- schedulers decide when fixes arrive (`passthrough`, `fixed`, `duty`) + and fault models decide what they contain (`none`, `degraded`, `slowbias`, `hijack`): + [Fault Simulation](../gnss/fault-simulation.md) and the + [Schedulers and Faults Reference](../gnss/scenarios.md). + +## Data in, data out + +- [Input Data Format](./data-format.md): the Sensor Logger CSV layout that every mode reads and + `syn` writes. +- [Output Format](./output-format.md): the `NavigationResult` columns, their units, and the + covariance columns. +- [Configuration Files](./configuration.md): every key of a scenario file. + +## A typical workflow + +1. Get input: a recording in the Sensor Logger layout, or `strapdown-sim syn`. +2. Declare its frame: nothing for NED (what `syn` writes), `--enu` for a phone export. +3. Run a baseline: `cl` with GNSS uninterrupted, and `dr` for the unaided bound. +4. Run the scenario you care about: an outage (`--sched duty`), a fault (`--fault ...`), or a + different filter, with a fixed `--seed`. +5. Write the scenario into a configuration file once it settles, so the run can be repeated + exactly. +6. Compare the CSVs, against the truth from `syn --no-noise` when you have it. + +The [Quick Start](../quick-start.md) walks through steps 1-6 on a synthetic trajectory. diff --git a/book/src/user-guide/particle-filter.md b/book/src/user-guide/particle-filter.md index d9a913c9..66bd272d 100644 --- a/book/src/user-guide/particle-filter.md +++ b/book/src/user-guide/particle-filter.md @@ -1 +1,164 @@ -# Particle Filter Mode +# Particle Filter + +`strapdown-sim pf` runs the Rao-Blackwellized particle filter (RBPF), after Canciani & Raquet +(IEEE TAES 53(1), 2017). It is the only particle filter the binary offers. Horizontal position +error is carried by particles; altitude, velocity, tilt, the barometer loop's states and any map +biases form one Kalman filter whose covariance all particles share. The filter exists for +map-aided navigation, where a gravity or magnetic map makes the measurement a multimodal function +of position; with GNSS alone it is a working, if more expensive, alternative to the Kalman +filters. The structure, the equations and the departures from the paper are on the +[Rao-Blackwellized Particle Filter](../filters/rbpf.md) page; this page is about running it. + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim pf -i synthetic.csv -o results/pf.csv +``` + +```console +[INFO] - Processing file: synthetic.csv +[INFO] - Read 6000 records from synthetic.csv +[INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +[INFO] - Results written to results/pf.csv +[INFO] - Particle filter simulation complete +``` + +(Timestamps trimmed.) The shared flags -- `-i`, `-o`, `--enu`, the execution and health limits -- +are described in [Running Simulations](./simulations.md). `pf` also takes the same GNSS +scheduler and fault flags as `cl` (`--sched`, `--fault` and their parameters, including +`--tau-pos-s`/`--tau-vel-s`), with the same meanings and defaults; they are documented once, on +the [Closed Loop](./closed-loop.md#gnss-scheduling-when-fixes-arrive) page. + +## What differs from `cl` + +- **Measurements.** GNSS fixes and magnetometer headings are measurement updates, as in `cl`. + Barometer readings are not: they drive a third-order barometer loop inside the mechanization, + which holds the vertical channel, so they never reweight the particles. +- **No IMU bias states.** The filter follows the paper's thirteen-state model. Adding bias states + made it diverge on degraded-GNSS runs. The output's `acc_bias_*` and `gyro_bias_*` columns and + their covariances are therefore zero. +- **No barometric bias state.** The `baro_bias` and `baro_bias_cov` columns are empty; the + barometer loop's own error states take that role. +- **No innovation gate.** The library's RBPF accepts one, but `strapdown-sim` installs none for + `pf`: `--gate-confidence` and its recovery flags belong to `cl`, and a `[closed_loop]` gate in a + configuration file is not read in particle-filter mode. The health monitor checks the state + and covariance bounds after every event, but no NIS is passed to it, so `--nis-pos-max` and + `--nis-pos-consec-fail` are accepted and have no effect on `pf`. +- **One seed for two things.** `--seed` seeds both the GNSS fault models and the filter's own + sampling. + +## Flags + +### Filter and initial uncertainty + +| Flag | Default | Meaning | +|---|---|---| +| `--filter-type ` | `rao-blackwellized` | the only value | +| `--num-particles ` | `100` | particle count | +| `--seed ` | `42` | seeds the particles and the fault models | +| `--position-std ` | `10` | initial position standard deviation, metres, applied to north, east and up alike | +| `--velocity-std ` | `1` | initial velocity standard deviation, m/s | +| `--attitude-std ` | `0.1` | initial tilt standard deviation, radians | + +`--position-std` sets all three axes to one value. A configuration file's `position_init_std_m` +takes the three separately and defaults to `[10, 10, 5]`, so a flag run and a default config run +start from slightly different altitude priors. + +### Process noise + +| Flag | Default | Meaning | +|---|---|---| +| `--horizontal-process-noise-std-m ` | `1 1` | random walk on the sampled horizontal position error, north then east, m/√s | +| `--velocity-process-noise-std-mps ` | `0.001` | velocity random walk, m/s per √s | +| `--attitude-process-noise-std-rad ` | `0.01` | attitude random walk, rad per √s | + +All three are rates per root second: the per-step standard deviation is the value times +$\sqrt{\Delta t}$, so the variance grows linearly in elapsed time whatever the log's sample rate. + +**The horizontal default is not the paper's.** Canciani & Raquet's eq. 19 puts no process noise +on horizontal position, and the library's `RbpfConfig` keeps that zero. With GNSS-rate fixes on +MEMS data a zero-noise cloud collapses and the filter diverges, so `strapdown-sim` and its +configuration file default to 1 m/√s on each axis, the value the repository's `conf/` recipes +use. Pass `0 0` to run the paper's model -- on the 600 s synthetic trajectory above it diverges +until the health monitor stops the run (`Error: OutOfRange { what: "speed", ... max: 500.0 }`), +while the default `1 1` and `0.5 0.5` both finish. + +`--horizontal-process-noise-std-m` takes exactly two values, **separated by a space**: +`--horizontal-process-noise-std-m 0.5 0.5`. The `north,east` comma form that the flag's help text +shows is rejected by the argument parser with `2 values required ... but 1 was provided`. + +### Barometer loop + +| Flag | Default | Meaning | +|---|---|---| +| `--baro-loop-time-constant-s ` | `10` | time constant of the barometer loop in the mechanization | +| `--baro-error-std-m ` | `8.3` | steady-state standard deviation of the barometer-aiding error, the hourly drift the Kalman filters' barometric bias is sized from | +| `--baro-error-time-constant-s ` | `3600` | correlation time of the barometer-aiding error | +| `--vertical-accel-error-init-std-mps2 ` | `0.1` | initial standard deviation of the loop's vertical-acceleration error, the consumer-grade accelerometer bias instability | + +### Resampling + +| Flag | Default | Meaning | +|---|---|---| +| `--effective-sample-threshold ` | `1.0` | resample when the effective sample size falls below this fraction of the particle count; `1.0` resamples after every update, as the paper does | +| `--roughening-factor ` | `0.2` | jitter applied after resampling, as a fraction of the cloud's extent; `0` turns it off, as the paper has it | + +Roughening is on by default because resampling makes exact copies: when the weights degenerate +onto one particle the cloud collapses to a point and its reported covariance to floating-point +noise. The [RBPF](../filters/rbpf.md) page has the detail. + +### Map biases (geophysical builds only) + +With `--features geonav`, `pf` takes the same `--geo`, `--gravity-*`, `--magnetic-*` and +`--geo-interval-s` flags as `cl`, and four of its own that shape each map bias's temporal +variation: + +| Flag | Default | Meaning | +|---|---|---| +| `--gravity-variation-std ` | derived from `--gravity-bias-process-noise-std` | steady-state sigma of the gravity bias's time-varying part | +| `--gravity-variation-time-constant-s ` | `300` | its correlation time | +| `--magnetic-variation-std ` | derived from `--magnetic-bias-process-noise-std` | the same for the magnetic bias; the paper uses 5 nT | +| `--magnetic-variation-time-constant-s ` | `300` | its correlation time; the paper's is 300 s | + +See [Geophysical Navigation](../geonav/overview.md). + +### Removed flags + +Four flags from before the Canciani & Raquet restructure are still recognized, so that a script +passing one fails with a message saying where the setting went rather than a bare parse error: + +| Removed flag | Replacement | +|---|---| +| `--geo-bias-init-std` | `--gravity-bias-init-std` (mGal) and `--magnetic-bias-init-std` (nT) | +| `--geo-bias-process-noise-std` | `--gravity-bias-process-noise-std` and `--magnetic-bias-process-noise-std` | +| `--process-noise-std-m` | `--horizontal-process-noise-std-m`; altitude has no process noise (eq. 20) | +| `--zero-vertical-velocity`, `--zero-vertical-velocity-std-mps` | the barometer loop; see `--baro-loop-time-constant-s` | + +```console +$ strapdown-sim pf -i synthetic.csv -o results/pf_x.csv --process-noise-std-m 1,1,1 +Error: "`--process-noise-std-m` was removed: the particle filter follows Canciani & Raquet, whose altitude error has no process noise (eq. 20). Set the horizontal pair with `--horizontal-process-noise-std-m north,east` (m per sqrt(s))" +``` + +## Examples + +```bash +# More particles, a duty-cycled outage that starts after 100 s of GNSS, and a degraded fix +strapdown-sim pf -i synthetic.csv -o results/pf_s.csv --num-particles 200 \ + --sched duty --on-s 100 --off-s 50 --duty-phase-s 100 --fault degraded + +# Tighter horizontal process noise +strapdown-sim pf -i synthetic.csv -o results/pf_h.csv --horizontal-process-noise-std-m 0.5 0.5 +``` + +The same run from a configuration file uses the `[particle_filter]` section; see +[Configuration Files](./configuration.md#particle_filter). + +## Accuracy and cost + +The accuracy suite runs one RBPF scenario, `real_rbpf_slice__rbpf`: the first 1,200 records of +the reference recording with GNSS every epoch and 500 particles. Its numbers are in the tables +under [Performance Baselines: Current values](../development/performance.md#current-values); its +consistency metrics are reported but not yet gated. + +Runtime is not documented here. The cost of a run depends on the particle count, the trajectory's +length and rate, and the machine, and no repeatable measurement of it has been made for this +book. diff --git a/book/src/user-guide/simulations.md b/book/src/user-guide/simulations.md index 0a343407..b27a9ad2 100644 --- a/book/src/user-guide/simulations.md +++ b/book/src/user-guide/simulations.md @@ -1 +1,215 @@ # Running Simulations + +`strapdown-sim` is the command-line front end to the library. It reads sensor records from CSV, +runs one of the estimators over them, and writes the navigation solution back out as CSV. This +page is the reference for the binary as a whole: the options every command shares, how input and +output paths are resolved, and the limits that stop a run. Each mode has its own page: + +| Subcommand | What it does | Page | +|---|---|---| +| `dr` | Dead reckoning: the IMU alone, from the first record's state | [Dead Reckoning](./dead-reckoning.md) | +| `cl` | Closed loop: the ESKF (default), EKF or UKF, aided by GNSS, barometer and magnetometer | [Closed Loop](./closed-loop.md) | +| `pf` | The Rao-Blackwellized particle filter | [Particle Filter](./particle-filter.md) | +| `syn` | Generate a synthetic trajectory, as sensor records or as truth | [Synthetic Trajectories](./synthetic.md) | +| `config` | Interactive wizard that writes a configuration file | [below](#config-the-configuration-wizard) | +| `ol` | **Not implemented.** Validates its paths, writes nothing, prints `Open-loop mode is not yet fully implemented` | [below](#ol-not-implemented) | + +Everything here was derived from the clap definitions in `sim/src/main.rs` and checked against +`strapdown-sim --help` and `strapdown-sim --help` on the 1.0.0 binary. Those help +pages are the authority if this page and the binary ever disagree. + +The geophysical flags (`--geo` and the `--gravity-*`/`--magnetic-*` family on `cl` and `pf`) +exist only in a binary built with `--features geonav`; see +[Geophysical Navigation](../geonav/overview.md). + +## Two ways to run + +A run is described either by a subcommand and its flags, or by a configuration file: + +```bash +# Flags +strapdown-sim cl -i synthetic.csv -o results/eskf.csv + +# A file +strapdown-sim --config scenario.toml +``` + +The two are exclusive. When `--config` is given, **the subcommand and every flag that belongs to +it are ignored**, and the file supplies the whole run, including `input`, `output` and the mode +(see [Configuration Files](./configuration.md)). `--config` is a global option, but put it before +any subcommand: `strapdown-sim cl --config x.toml` is parsed as a `cl` invocation first and fails +on the missing `-i`/`-o`. + +Run with no subcommand and no `--config`, the binary exits with status 1 and +`Error: No command provided`. + +## Global options + +These are accepted before or after the subcommand. + +| Option | Default | Meaning | +|---|---|---| +| `-c`, `--config ` | -- | Run entirely from a TOML, YAML or JSON file. The format is chosen by extension (`.toml`, `.yaml`/`.yml`, `.json`). | +| `--log-level ` | `info` | `off`, `error`, `warn`, `info`, `debug` or `trace`. An unrecognized value prints a warning and falls back to `info`. See [Logging](./logging.md). | +| `--log-file ` | stderr | Append log lines to this file instead of stderr. Parent directories are created. | +| `--parallel` | off | Process the files of a directory input concurrently. **Only honoured with `--config`**; see below. | +| `--plot` | off | Write a PNG performance plot beside each result. **Only honoured with `--config`**; see below. | + +`--parallel` and `--plot` are read only on the configuration-file path, where they force +`parallel = true` and `generate_plot = true` respectively. A subcommand run accepts both flags +without error and does nothing with them: `cl -i synthetic.csv -o plotdir/eskf.csv --plot` writes +`eskf.csv` and no PNG. To get a plot or parallel processing, use a configuration file. + +On the configuration path the plot is drawn for closed-loop and particle-filter runs only (a +dead-reckoning config with `generate_plot = true` writes no PNG). It lands beside the CSV with the +extension replaced by `.png`, and needs the `plotting` feature, which is on by default; a failure +to draw it is logged and does not fail the run. + +## Input and output paths + +`dr`, `cl`, `pf` and `ol` share the same two required arguments. + +| Argument | Accepts | +|---|---| +| `-i`, `--input ` | A single `.csv` file, or a directory. A directory is searched (not recursively) for files with the extension `csv`, which are processed in sorted name order. | +| `-o`, `--output ` | A file path ending in `.csv`, or a directory. | + +The rules, all from `sim/src/common.rs`: + +- **An input file must have the extension `csv`.** Anything else is refused with + `Input file '...' is not a CSV file.` A directory with no CSV files in it is refused too. +- **Whether `--output` is a file or a directory is decided by its extension.** A path ending in + `.csv` (any case) names a file; any other path names a directory. An *existing* directory + always counts as a directory, even one named `results.csv`. +- **Output is CSV only.** There is no flag that selects another format, and no other extension + makes the output a file: `-o results.parquet` creates a *directory* called `results.parquet` + and writes `.csv` inside it. HDF5, NetCDF and MCAP exist only as library methods; + see [Output Format](./output-format.md). +- **Missing directories are created**: the output directory itself, or the parent of an output + file. +- **Naming.** With one input file, a file output is written exactly where it points, and a + directory output gets the input's file name. With several inputs, a directory output gets one + file per input under the input's name, and a file output `-o run.csv` becomes + `run_.csv` for each input, beside where `run.csv` would be. +- **Results are never written over an input.** If the resolved output path is the input file, + or any other file this run reads (compared after resolving symlinks and `..`), the run stops. + So `dr -i batch -o batch` is refused rather than overwriting the recordings. + +```console +$ strapdown-sim dr -i batch -o out_file/run.csv +$ ls out_file +run_a.csv run_b.csv +$ strapdown-sim dr -i batch/a.csv -o batch/a.csv +Error: "Refusing to write results to 'batch/a.csv': that is the input file. Pass a different --output path." +``` + +**Batches.** When the input is a directory, a file that yields no usable records is logged, +skipped and counted, and the rest of the batch runs. A file whose *run* fails is also logged and +skipped on `cl` and on any `--config` run; `dr` and `pf` stop at the first run that fails. A run +that fails writes no output file for that input. + +**Negative numbers** need the `=` form, because clap otherwise reads a leading `-` as a flag: +`--longitude-deg=-75`, not `--longitude-deg -75`. + +## Frame declaration + +`dr`, `cl`, `pf` and `ol` take `--enu`, which declares the input records to be in the ENU +convention rather than the default NED. Sensor Logger exports are ENU; `syn` output is NED unless +it was generated with `syn --enu`. The declaration is checked against the data before anything is +propagated, and a wrong one is refused with an `InvalidConfiguration` error naming the flag to +change. See [Input Data Format](./data-format.md#frame-convention). + +## Execution limits + +Every simulation subcommand carries three wall-clock budgets, so a run that hangs or slows to a +crawl ends with an error instead of occupying the machine. Each is disabled by a value of `0` or +less; whichever active one trips first ends the run. + +| Flag | Default | Fails the run when | +|---|---|---| +| `--max-wall-clock-ratio ` | `0.25` | wall-clock time exceeds `R` times the simulated duration (a 600 s trajectory gets 150 s) | +| `--max-wall-clock-s ` | `1200` | wall-clock time for one trajectory exceeds `S` seconds | +| `--max-no-progress-s ` | `600` | `S` seconds pass with no event processed | + +The failure is a `Timeout` error, and no output file is written for that trajectory. + +## Health limits + +The health monitor checks the estimate after every event and fails the run when it leaves a +plausible envelope. The defaults are deliberately permissive -- they catch numerical divergence, +not a poor result -- so narrow them to the scenario when you want them to act as a gate. + +| Flag | Default | Fails the run when | +|---|---|---| +| `--health-lat-min-deg`, `--health-lat-max-deg` | `-90`, `90` | latitude leaves the band (degrees) | +| `--health-lon-min-deg`, `--health-lon-max-deg` | `-180`, `180` | longitude leaves the band (degrees) | +| `--health-alt-min-m`, `--health-alt-max-m` | `-1e8`, `1e8` | altitude above the ellipsoid leaves the band (metres) | +| `--health-speed-mps-max` | `500` | the magnitude of the full velocity vector, vertical included, exceeds this (m/s) | +| `--health-cov-diag-max` | `1e15` | any variance on the covariance diagonal exceeds this | +| `--nis-pos-max` | `100` | (sets the threshold above which an update's NIS counts as an outlier) | +| `--nis-pos-consec-fail` | `20` | this many outliers occur in a row; one in-bounds update resets the count | + +The NIS test applies to every measurement update, barometer and magnetometer included, despite +the `_pos` in the name. It is a run-level divergence check, separate from the +[innovation gate](./closed-loop.md#innovation-gating) that `cl` can install to reject individual +measurements. + +```console +$ strapdown-sim cl -i cruise.csv -o hl/out.csv --health-speed-mps-max 10 +Error: OutOfRange { what: "speed", value: 49.999902224914834, min: 0.0, max: 10.0 } +``` + +(`cruise.csv` is a 50 m/s synthetic trajectory; see +[Synthetic Trajectories](./synthetic.md#a-moving-trajectory).) + +**`dr` and `ol` accept all of the limit flags above and apply none of them.** Dead reckoning +never consults the execution or health monitor, so an unaided run that drifts far away is +reported in full rather than cut off. **`pf` applies every limit except the NIS pair**: its loop +checks the state and covariance bounds but passes no NIS to the monitor, so `--nis-pos-max` and +`--nis-pos-consec-fail` have no effect on it. + +## Seeds + +Every stochastic part of a run is seeded, and the default seed everywhere is `42`, so the same +command on the same input produces the same output. + +| Flag | Seeds | +|---|---| +| `cl --seed` | the GNSS fault models (AR(1) noise, slow-bias random walk) | +| `pf --seed` | the fault models and the particle filter's own sampling | +| `syn --seed` | the IMU, GNSS, barometer and magnetometer noise of the generated trajectory | + +`dr` takes no seed: there is nothing random in it. In a configuration file the two roles of `pf +--seed` are separate keys; see [Configuration Files](./configuration.md#seeds). + +## `config`: the configuration wizard + +`strapdown-sim config` takes no arguments. It asks a series of questions on the terminal -- file +name and directory, input and output paths, mode, seed, frame, parallel processing, log level and +file, the filter (for closed loop), a GNSS scheduler and fault model, and optional geophysical +aiding -- and writes a complete configuration file in the format its extension names. Answer `q` +at any prompt to quit. + +The file it writes spells out every section it covers with default values filled in, which makes +it a reasonable starting point to edit by hand: + +```console +$ strapdown-sim config +... +✓ Configuration file successfully created: wiz/template.toml + +You can now run the simulation with: + strapdown-sim --config wiz/template.toml +``` + +Two limits: the wizard offers dead reckoning, open loop, closed loop and particle filter but not +synthetic generation, and it offers open loop although that mode is not implemented (a config +with `mode = "open-loop"` exits with status 1 and `Error: "Open-loop mode is not yet fully +implemented"`). When geophysical aiding is enabled on top of the ESKF, the wizard switches the +filter to the UKF, because the ESKF has no geophysical implementation. + +## `ol`: not implemented + +`ol` takes the same arguments as `dr`. It checks that the input exists, creates the output +directory, prints `Open-loop mode is not yet fully implemented`, writes no result and exits with +status 0. It is not dead reckoning; use `dr` for that. diff --git a/book/src/user-guide/state-representation.md b/book/src/user-guide/state-representation.md index a7f7d7ed..40d01271 100644 --- a/book/src/user-guide/state-representation.md +++ b/book/src/user-guide/state-representation.md @@ -1 +1,187 @@ # State Representation + +Three layers of state appear in this library: the nine-element navigation state that the +mechanization propagates, the fifteen-element layout every filter reports, and the extra bias +states a filter may append. This page defines each one, with units, and shows how they reach +the output CSV. + +## The navigation state: `StrapdownState` + +`StrapdownState` is what [the mechanization](./concepts.md) propagates: + +| Field | Quantity | Units | +| --- | --- | --- | +| `latitude` | Geodetic latitude | **radians** | +| `longitude` | Geodetic longitude | **radians** | +| `altitude` | Height above the WGS84 ellipsoid, positive up in both frames | m | +| `velocity_north` | Earth-relative velocity, north component | m/s | +| `velocity_east` | Earth-relative velocity, east component | m/s | +| `velocity_vertical` | Earth-relative velocity, vertical component: positive down in NED, positive up in ENU | m/s | +| `attitude` | Body-to-navigation rotation $\mathbf C_b^n$ (`nalgebra::Rotation3`) | -- | +| `is_enu` | Frame flag: `false` for NED (the default), `true` for ENU | -- | + +Latitude and longitude are radians inside the library and degrees in every CSV file, input and +output. As a flat vector (`Vec` or `DVector` via `From`) the state is the nine +elements + +$$ +[L,\ \lambda,\ h,\ v_N,\ v_E,\ v_{\text{vert}},\ \phi,\ \theta,\ \psi] +$$ + +with the angles in radians and the attitude expressed as roll, pitch and yaw +([Coordinate Frames](./coordinate-frames.md) gives the Euler convention). The vector carries no +frame flag: converting nine numbers back with `TryFrom` produces an NED state. + +## The fifteen-element layout + +Every filter reports its estimate in one common layout, the navigation state followed by the +two IMU bias vectors: + +| Index | State | Units | +| --- | --- | --- | +| 0, 1 | Latitude, longitude | rad | +| 2 | Altitude | m | +| 3, 4, 5 | Velocity north, east, vertical | m/s | +| 6, 7, 8 | Roll, pitch, yaw | rad | +| 9, 10, 11 | Accelerometer bias $\mathbf b_a$, body $x, y, z$ | m/s² | +| 12, 13, 14 | Gyroscope bias $\mathbf b_g$, body $x, y, z$ | rad/s | + +The width is `sim::NAVIGATION_STATES` (15), and the crate root's `ERROR_STATE_DIMENSION` (15) +is the same count for the error-state form. `sim::DEFAULT_PROCESS_NOISE_DENSITY` has one entry +per row; see [The Navigation Model](./concepts.md#process-noise-is-a-spectral-density). + +The IMU biases are modelled as random walks. The Kalman filters subtract their bias estimates +from each IMU sample before mechanizing it. + +Extra states, when a filter carries them, are appended after index 14. + +## What each filter carries + +### ESKF (the `cl` default) + +The error-state Kalman filter keeps two things apart: + +- a **nominal state**: position (radians and metres), velocity, attitude as a unit + quaternion, the two IMU biases, and the barometric bias when it is carried; +- an **error state** $\delta\mathbf x$ and its covariance: 15 elements, or 16 with the + barometric bias. + +| Index | Error state | Units | +| --- | --- | --- | +| 0, 1, 2 | $\delta L$, $\delta\lambda$, $\delta h$ | rad, rad, m | +| 3, 4, 5 | $\delta v_N$, $\delta v_E$, $\delta v_{\text{vert}}$ | m/s | +| 6, 7, 8 | $\delta\boldsymbol\theta$: attitude error as a body-frame rotation vector | rad | +| 9, 10, 11 | $\delta\mathbf b_a$ | m/s² | +| 12, 13, 14 | $\delta\mathbf b_g$ | rad/s | +| 15 | $\delta b_{\text{baro}}$ (only when the barometric bias is carried) | m | + +Only the error state is estimated. After each measurement update it is injected into the +nominal state and reset to zero. The attitude correction is composed on the right, +$\mathbf q \leftarrow \mathbf q \otimes \mathrm{Exp}(\delta\boldsymbol\theta)$, which is what +makes $\delta\boldsymbol\theta$ a rotation of the *body* frame, not a Euler-angle difference. +Position, velocity and the biases are corrected by addition. When reporting, the ESKF +converts its nominal state into the fifteen-element layout above, plus the barometric bias. + +**How many states, exactly.** The library's `EskfConfig` defaults to `estimate_baro_bias = +false`, which gives a 15-element error state. `strapdown-sim cl` and a `[closed_loop]` +configuration section default to `estimate_baro_bias = true`, so **the ESKF that `cl` runs has +16 error states**. `--no-estimate-baro-bias` (or `estimate_baro_bias = false`) takes it back +to 15. The ESKF has no geophysical arm, so it never carries map biases. + +### EKF and UKF (`cl --filter ekf|ukf`) + +These carry the **full state**, not an error state: a mean vector in the fifteen-element +layout and its covariance, with extra states appended in this order: + +1. map biases from `strapdown-geonav`, gravity first, then magnetic, one state per loaded map + (indices 15 and 16 when both maps are loaded); +2. the barometric bias, last, when it is carried. + +The barometric bias goes after the map biases because geonav numbers its map-bias states from +15; putting the barometer at 15 would collide with a gravity bias. Its index is therefore not +a constant, and `UkfConfig::baro_bias_index` and `EkfConfig::baro_bias_index` compute it. +Without maps, `cl --filter ekf` and `cl --filter ukf` carry 16 states. With both maps and the +barometric bias, they carry 18, the barometric bias at index 17. + +The library EKF can also run with `use_biases = false`, which makes it a nine-state filter +with no IMU-bias states; the simulator does not use that form. + +### RBPF (`pf`) + +The Rao-Blackwellized particle filter is structured differently inside. Its particles sample +the horizontal position error only; one Kalman filter shared by all particles holds the +altitude error, velocity error, navigation-frame tilt, the barometer-aiding loop's two states, +and for each map a time-varying part $V$ and a constant part $c$ of the map bias. It carries +**no IMU-bias states**. [Rao-Blackwellized Particle Filter](../filters/rbpf.md) gives the full +partition. + +It reports the same layout as the Kalman filters, `[9 navigation, b_a(3), b_g(3)]`, followed +by one total map bias $V + c$ per map. The six IMU-bias rows, and their variances, are zero. +It has no barometric bias state in the sense of the Kalman filters: its barometer error lives +inside the aiding loop, and its output leaves the `baro_bias` column empty. + +## The extra bias states + +| State | Units | Carried by | Default | +| --- | --- | --- | --- | +| Barometric bias | m | ESKF, EKF, UKF | **On** in `strapdown-sim cl` and `[closed_loop]`; off in the library's `EskfConfig`, `EkfConfig`, `UkfConfig` | +| Gravity anomaly map bias | mGal | EKF, UKF, RBPF, when a gravity map is loaded (`--geo`, experimental) | -- | +| Magnetic anomaly map bias | nT | EKF, UKF, RBPF, when a magnetic map is loaded (`--geo`, experimental) | -- | + +**Barometric bias.** A barometer measures height through pressure, and its reference pressure +drifts with the weather. A filter that treats the reading as unbiased pushes that drift into +altitude. The bias state is a random walk that starts at zero with variance +`sim::INITIAL_BARO_BIAS_VARIANCE_M2` $= (8.3\ \text{m})^2 = 68.89$ m² and grows at +`sim::BARO_BIAS_PROCESS_NOISE_M2_PER_S` $= (8.3\ \text{m})^2$ per hour, both derived from about +one hectopascal of reference drift per hour. The barometer measurement model reads it through +the index the filter declares (`AidingConfig::baro_bias_index`), which `strapdown-sim` sets +from the filter configuration. A library caller who builds a 16-state filter by hand must set +that index too, or nothing observes the state. + +**Map biases** are experimental and documented with [Geophysical +Navigation](../geonav/overview.md). Their layout is described by `strapdown-geonav`'s +`GeoBiasLayout`; `sim::ExtraStateLayout` is its counterpart on the `strapdown-core` side, which +the simulator uses to label the output columns. + +## How the state reaches the output + +Every mode writes one `NavigationResult` row per IMU record. The state appears as follows +([Output Format](./output-format.md) lists every column): + +| Columns | Content | Units | +| --- | --- | --- | +| `latitude`, `longitude` | Estimate | **degrees** | +| `altitude`, `velocity_north`, `velocity_east`, `velocity_vertical` | Estimate | m, m/s | +| `roll`, `pitch`, `yaw` | Estimate | rad | +| `acc_bias_x/y/z`, `gyro_bias_x/y/z` | IMU bias estimates; zero for `dr` and `pf` | m/s², rad/s | +| `latitude_cov`, `longitude_cov` | Variance | **rad²** | +| `altitude_cov` | Variance | m² | +| `latitude_longitude_cov`, `latitude_altitude_cov`, `longitude_altitude_cov` | The off-diagonal terms of the position block | rad², rad·m, rad·m | +| `velocity_n_cov`, `velocity_e_cov`, `velocity_v_cov`, `roll_cov`, `pitch_cov`, `yaw_cov`, `acc_bias_*_cov`, `gyro_bias_*_cov` | Variances (the covariance diagonal) | the state's units, squared | +| `gravity_bias`, `magnetic_bias`, `baro_bias` and their `_cov` | Extra bias states and their variances | mGal, nT, m | + +Points to watch when reading these columns: + +- **Latitude and longitude are degrees, their variances radians squared.** Convert the + position error to radians before comparing it with the variance, not the variance to degrees. + With $P_{LL}$ = `latitude_cov` and $P_{\lambda\lambda}$ = `longitude_cov`, the north and + east standard deviations in metres are approximately $\sigma_N = \sqrt{P_{LL}} (R_N + h)$ + and $\sigma_E = \sqrt{P_{\lambda\lambda}} (R_E + h) \cos L$, with the radii of + [The Navigation Model](./concepts.md#the-earth-model). +- **Only the position block is written in full.** The other states carry their variances + only. The three position off-diagonals are there so that a position NEES, + $\mathbf e^\top \mathbf P^{-1} \mathbf e$, can be computed from the output. +- **The ESKF's attitude variances are of its body-frame attitude error** $\delta\boldsymbol\theta$. + They equal Euler-angle variances only at level attitude. +- **An empty cell means "not carried", not zero.** The three extra-bias pairs are optional: + a run without a gravity map leaves `gravity_bias` empty, and a run that estimated a bias of + exactly zero writes `0`. The columns are always present, so every result file has the same + 40 columns. +- **Dead reckoning has no covariance.** `dr` runs no filter, so it writes `NaN` in all + eighteen covariance columns, zeros in the IMU-bias columns, and leaves the extra-bias + columns empty. + +On the [Quick Start](../quick-start.md)'s ESKF run, for example, the first row has +`baro_bias_cov` = 68.89, the initial variance above, and `latitude_cov` = 2.467 × 10⁻¹² rad²: +the default 10 m initial horizontal standard deviation +(`sim::DEFAULT_INITIAL_POSITION_UNCERTAINTY_M`), converted to radians and squared. diff --git a/book/src/user-guide/synthetic.md b/book/src/user-guide/synthetic.md new file mode 100644 index 00000000..97c6eba2 --- /dev/null +++ b/book/src/user-guide/synthetic.md @@ -0,0 +1,237 @@ +# Synthetic Trajectories + +`strapdown-sim syn` generates a trajectory from an initial kinematic state and writes it as CSV: +either noisy sensor records that every other mode can read, or the exact truth those records were +drawn from. Since no sample dataset ships with the repository, `syn` is the reproducible input +that every example in this book runs on: + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +``` + +```console +[INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +[INFO] - Sensor records written to synthetic.csv +``` + +## The motion model + +The vehicle moves at a **constant navigation-frame velocity** while the body turns at a +**constant body-frame angular rate**: zero linear and angular acceleration. The angular rate turns +the body, not the path -- with `--velocity-north-mps 10 --angular-velocity-z-dps 1` the vehicle +spins slowly while still travelling due north. Every trajectory is therefore a straight line (a +rhumb line over the ellipsoid) at constant altitude rate. + +Each epoch's ideal IMU output is computed by *inverse* mechanization: the specific force and +angular rate that keep the navigation-frame velocity constant, including the Earth-rate, +transport-rate and Coriolis terms, so that mechanizing the perfect IMU reproduces the truth. The +truth itself is propagated through the library's own mechanization (Groves §5.4-5.5) from those +perfect samples. Noise is then added to produce the sensor records. + +## Flags + +### Output and timing + +| Flag | Default | Meaning | +|---|---|---| +| `-o`, `--output ` | required | output CSV path; its parent directory is created | +| `--duration-s ` | `300` | trajectory length, seconds | +| `--sample-rate-hz ` | `10` | sample rate of every channel, Hz | +| `--seed ` | `42` | seeds every noise draw | +| `--no-noise` | off | write the truth instead of sensor records; see [below](#truth-with---no-noise) | +| `--enu` | off | emit the trajectory in ENU rather than NED | + +`syn` takes no `-i`. Its output path is always a file and is used exactly as given, whatever its +extension -- `-o x.parquet` writes CSV into a file named `x.parquet` -- so give it a `.csv` name. +Unlike the simulation subcommands, it never treats the path as a directory. + +### Initial state + +| Flag | Default | Meaning | +|---|---|---| +| `--latitude-deg` | `0` | WGS84 latitude, degrees | +| `--longitude-deg` | `0` | WGS84 longitude, degrees | +| `--altitude-m` | `0` | height above the ellipsoid, metres | +| `--velocity-north-mps` | `0` | north velocity, m/s | +| `--velocity-east-mps` | `0` | east velocity, m/s | +| `--velocity-down-mps` | `0` | vertical velocity, m/s: positive **down** by default, positive **up** with `--enu` (the flag keeps its NED name) | +| `--roll-deg`, `--pitch-deg`, `--yaw-deg` | `0` | initial attitude, degrees | +| `--angular-velocity-x-dps`, `-y-dps`, `-z-dps` | `0` | constant body rates about x, y and z, degrees per second | + +With every default the vehicle sits still at latitude 0, longitude 0, altitude 0. Pass negative +values with `=`: `--longitude-deg=-75`. + +### Sensor noise + +| Flag | Default | Meaning | +|---|---|---| +| `--imu-grade ` | `consumer` | IMU noise and bias levels; see [IMU grades](#imu-grades) | +| `--gnss-horizontal-noise-m ` | `2.5` | one-sigma white noise on the GNSS horizontal position, metres | +| `--gnss-vertical-noise-m ` | `5` | one-sigma white noise on the GNSS altitude, metres | +| `--baro-noise-std-pa ` | `50` | one-sigma white noise on the `pressure` column, pascals | +| `--mag-noise-std-ut ` | `0.5` | one-sigma white noise on each magnetometer axis, microtesla | +| `--mag-hard-iron-std-ut ` | `0` | standard deviation of a hard-iron offset drawn once per run and held, microtesla per axis | + +Hard iron is off by default on purpose: a constant body-frame field biases the computed heading in +a way no filter here can observe, so with it on the yaw error measures the offset rather than the +filter. + +## IMU grades + +Each grade sets four numbers, from the table in `IMUQuality` (`core/src/lib.rs`, after Groves +Table 4.1). The **bias** is drawn once per run per axis from a zero-mean normal with the bias +instability as its standard deviation, and held constant. The **white noise** is drawn every +sample, with the random-walk coefficient scaled to the sample rate as +$\sigma = \text{RW}\sqrt{f_s/3600}$. + +| `--imu-grade` | gyro bias instability (deg/h) | gyro angle random walk (deg/√h) | accel bias instability (m/s²) | accel velocity random walk (m/s/√h) | +|---|---:|---:|---:|---:| +| `consumer` | 100 | 1 | 0.1 | 0.1 | +| `industrial` | 50 | 0.1 | 0.05 | 0.03 | +| `tactical` | 1 | 0.01 | 0.001 | 0.01 | +| `navigation` | 0.01 | 0.005 | 0.0001 | 0.005 | +| `strategic` | 0.0001 | 0.0005 | 0.00001 | 0.0001 | + +Running [`dr`](./dead-reckoning.md) on the same trajectory at different grades shows what each +grade means for unaided drift. + +## What it writes + +### Sensor records (the default) + +Without `--no-noise`, the output is a `TestDataRecord` CSV: the same 31 columns, in the same +order, as a Sensor Logger export, readable by `dr`, `cl` and `pf` without conversion. Column +definitions are on [Input Data Format](./data-format.md). What `syn` puts in them: + +| Columns | Content | +|---|---| +| `time` | RFC 3339 UTC timestamps starting at `2025-01-01T00:00:00Z`, one per sample | +| `acc_*`, `gyro_*` | perfect IMU plus the grade's constant bias and white noise, body frame | +| `latitude`, `longitude`, `altitude` | truth plus white GNSS noise, **on every row** | +| `speed`, `bearing` | the truth's ground speed and track, **without noise** | +| `horizontalAccuracy`, `speedAccuracy`, `bearingAccuracy` | the `--gnss-horizontal-noise-m` value | +| `verticalAccuracy` | the `--gnss-vertical-noise-m` value | +| `relativeAltitude` | the *noisy* GNSS altitude minus the initial altitude | +| `pressure` | standard-atmosphere pressure at the true altitude plus `--baro-noise-std-pa` noise, Pa | +| `mag_*` | the World Magnetic Model field at the true position and the record's date, rotated into the body frame, plus noise and any hard iron, µT | +| `grav_*` | local gravity rotated into the body frame, m/s² | +| `qw`, `qx`, `qy`, `qz`, `roll`, `pitch`, `yaw` | the true attitude, as a quaternion and as Euler angles in radians | + +**There is no truth in this file.** It contains only what a sensor would report. + +Three properties of these records matter when you use them: + +- **GNSS is on every row**, so with the default `--sched passthrough` a filter receives GNSS at + the full sample rate -- 10 Hz by default. For a realistic 1 Hz receiver, run the filter with + `--sched fixed --interval-s 1`, as the accuracy suite's `syn_cruise_1hz` scenario does. +- **The barometric channel is the GNSS altitude.** `relativeAltitude` is derived from the noisy + GNSS altitude column, not from `pressure`, so the barometer measurement the filters build from + it carries the same noise sample as the GNSS fix, and `--baro-noise-std-pa` changes only the + `pressure` column, which no estimator reads. +- **The velocity aiding is exact.** `speed` and `bearing` carry no noise, while + `speedAccuracy` advertises the horizontal position noise as their one-sigma. + +### Truth with `--no-noise` + +With `--no-noise`, `syn` writes the truth instead, as a `NavigationResult` CSV -- the same 40 +columns every simulation mode writes, described on [Output Format](./output-format.md). Position, +velocity and attitude are exact; bias and covariance columns are zero and the six optional bias +columns are empty. + +The truth depends only on the kinematic flags (initial state, rates, duration, sample rate, +frame). It does not depend on `--seed`, `--imu-grade` or any noise flag, because it is propagated +from the perfect IMU. So the truth for a noisy run is obtained by repeating the command with +`--no-noise` added: + +```bash +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 +strapdown-sim syn -o synthetic_truth.csv --duration-s 600 --seed 42 --no-noise +``` + +Both files have one row per sample with identical timestamps, which is also how the output of +`dr`, `cl` and `pf` on `synthetic.csv` is laid out, so a result can be scored against the truth +row by row. + +## A moving trajectory + +The stationary default is enough to check that a filter holds still. For anything else, give the +vehicle a velocity. This is the trajectory the accuracy suite's synthetic scenarios use -- a +50 m/s cruise north-east at 50 Hz -- written for 120 s: + +```bash +strapdown-sim syn -o cruise.csv --duration-s 120 --sample-rate-hz 50 \ + --latitude-deg 40 --longitude-deg=-75 --altitude-m 200 \ + --velocity-north-mps 40 --velocity-east-mps 30 --yaw-deg 36.86989764584402 \ + --gnss-horizontal-noise-m 3 --gnss-vertical-noise-m 5 --baro-noise-std-pa 30 --seed 42 +``` + +The yaw of 36.87 degrees points the body along the velocity vector, so the vehicle travels +nose-first. + +## Frames + +`syn` writes NED by default: at rest the accelerometer reads about $-9.8$ m/s² on the down axis, +which is what `dr`, `cl` and `pf` expect without `--enu`. `syn --enu` writes ENU records, which +pair with `--enu` on the simulation subcommands. Mixing the two is refused before propagation; +see [Input Data Format](./data-format.md#frame-convention). + +## Reproducibility + +The same flags and seed produce a byte-identical file: two runs of +`syn -o s.csv --duration-s 60 --seed 42` compare equal with `cmp`. Three things make that hold: + +- every random draw comes from a generator seeded by `--seed`; +- the start time is fixed at `2025-01-01T00:00:00Z` rather than taken from the clock, which also + fixes the date the magnetic model is evaluated at; +- the magnetometer noise and the hard-iron offset draw from their own streams, so changing + `--mag-noise-std-ut` changes only the three `mag_*` columns and leaves the IMU, GNSS and + barometer noise untouched. + +Combined with the exact truth from `--no-noise`, that makes a synthetic trajectory the one input +on which an error can be attributed to the estimator rather than to an unknown reference. + +## The `[synthetic]` configuration section + +A configuration file with `mode = "synthetic"` runs `syn` from a `[synthetic]` section instead of +from flags. The keys are the flags' names with two differences: the IMU grade is `imu_quality`, +and the initial state sits in its own `[synthetic.initial_state]` table. + +```toml +mode = "synthetic" + +[synthetic] +output = "cfg_out/syn_cruise.csv" # required +duration_s = 300.0 # required +sample_rate_hz = 50.0 # default 10 +imu_quality = "consumer" # consumer | industrial | tactical | navigation | strategic +seed = 42 +no_noise = false +gnss_horizontal_noise_m = 3.0 # default 2.5 +gnss_vertical_noise_m = 5.0 # default 5 +baro_noise_std_pa = 30.0 # default 50 +mag_noise_std_ut = 0.5 +mag_hard_iron_std_ut = 0.0 + +[synthetic.initial_state] +latitude_deg = 40.0 # these three are required +longitude_deg = -75.0 # once the table is present +altitude_m = 200.0 +velocity_north_mps = 40.0 # the rest default to zero +velocity_east_mps = 30.0 +velocity_down_mps = 0.0 +yaw_deg = 36.86989764584402 +is_enu = false +``` + +```console +$ strapdown-sim --config syn.toml +[INFO] - Mode: Synthetic +[INFO] - Generated 15000 synthetic records (300.0 s at 50 Hz) +[INFO] - Sensor records written to cfg_out/syn_cruise.csv +``` + +`output` and `duration_s` have no defaults: a `[synthetic]` section without them is refused with +``missing field `duration_s` ``. Leave out `[synthetic.initial_state]` entirely and the vehicle +starts at rest at 0, 0, 0; include it and `latitude_deg`, `longitude_deg` and `altitude_m` must +all be given. The other top-level keys of a configuration file (`input`, `[closed_loop]` and so +on) are ignored in synthetic mode. diff --git a/book/theme/head.hbs b/book/theme/head.hbs new file mode 100644 index 00000000..ca1609b0 --- /dev/null +++ b/book/theme/head.hbs @@ -0,0 +1,13 @@ + + diff --git a/core/examples/aiding.rs b/core/examples/aiding.rs new file mode 100644 index 00000000..e9e43be3 --- /dev/null +++ b/core/examples/aiding.rs @@ -0,0 +1,236 @@ +//! Measurement models beyond GNSS, and innovation gating. +//! +//! Run it with: +//! +//! ```bash +//! cargo run -p strapdown-core --example aiding +//! ``` +//! +//! Three short scenes, each on a fresh [`InsEngine`] (the default 15-state ESKF): +//! +//! 1. **A parked vehicle.** A [`StationaryDetector`] decides when the IMU is still, and +//! while it is, zero-velocity (ZUPT) and zero-angular-rate (ZARU) pseudo-measurements are +//! applied, with a barometric altitude once a second. The gyro carries a bias the filter +//! is not told about; ZARU is what lets it find it. +//! 2. **A magnetometer heading.** A body-frame field is synthesised for a known heading and +//! turned back into a yaw measurement, with WMM declination applied. +//! 3. **A spoofed fix.** A chi-squared innovation gate rejects a GNSS fix 200 m off, and the +//! default [`GateRecovery`] forces a fix through after five consecutive rejections. +//! +//! None of the pseudo-measurements here are applied by `strapdown-sim`: the CLI's event +//! stream carries GNSS position and velocity, barometric altitude and magnetometer yaw. ZUPT, +//! ZARU and the stationary detector are library building blocks, called as below. + +use std::error::Error; + +use nalgebra::{Rotation3, Vector3}; + +use strapdown::earth::{METERS_TO_DEGREES, earth_rate_lla}; +use strapdown::engine::{GnssFix, InsEngine}; +use strapdown::gating::{GateRecovery, InnovationGate, chi_squared_quantile}; +use strapdown::kalman::InitialState; +use strapdown::measurements::{ + BAROMETRIC_ALTITUDE_NOISE_M, MagnetometerYawMeasurement, MeasurementModel, + RelativeAltitudeMeasurement, ZaruMeasurement, ZuptMeasurement, +}; +use strapdown::stationary::{StationaryConfig, StationaryDetector}; +use strapdown::{IMUData, StrapdownError}; + +const LATITUDE_DEG: f64 = 39.95; +const LONGITUDE_DEG: f64 = -75.16; +const ALTITUDE_M: f64 = 12.0; +/// IMU sample interval, seconds (100 Hz, what `StationaryConfig::default()` is tuned for). +const DT: f64 = 0.01; + +fn main() -> Result<(), Box> { + parked()?; + magnetometer_heading(); + spoofed_fix()?; + Ok(()) +} + +/// Scene 1: ZUPT, ZARU and barometric altitude on a parked vehicle. +fn parked() -> Result<(), StrapdownError> { + let mut engine = InsEngine::builder() + .with_initial_state(InitialState::new( + LATITUDE_DEG, + LONGITUDE_DEG, + ALTITUDE_M, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + true, // degrees + None, // NED, the default + )) + .build()?; + + // Level, heading north, NED: the accelerometer reads the reaction to gravity (-g on the + // down axis) and the gyro reads the Earth's rotation plus a bias the filter has not been + // given. Level and north-pointing means the body and navigation axes coincide, so the + // Earth rate resolves into the body frame unchanged. + let gyro_bias = Vector3::new(0.0, 0.0, 0.002); // rad/s, about z + let imu = IMUData { + accel: Vector3::new(0.0, 0.0, -9.81), + gyro: earth_rate_lla(&LATITUDE_DEG) + gyro_bias, + }; + + // ANCHOR: stationary + let mut detector = StationaryDetector::new(StationaryConfig::default()); + let mut pseudo_measurements = 0; + for step in 1..=3000 { + engine.predict_rates(&imu, DT)?; + + // The detector needs a full window, then a dwell, before it latches: 1.5 s at the + // defaults. Pseudo-measurements are applied at 10 Hz once it has. + if detector.push(&imu) && step % 10 == 0 { + engine.update(&ZuptMeasurement::default())?; + // ZARU reads the raw gyro: on a stationary platform that is bias plus Earth rate, + // and the model predicts the Earth rate itself. + engine.update(&ZaruMeasurement::from_gyro([ + imu.gyro.x, imu.gyro.y, imu.gyro.z, + ]))?; + pseudo_measurements += 1; + } + + // A barometer that has not moved since the reference epoch. + if step % 100 == 0 { + engine.update(&RelativeAltitudeMeasurement { + relative_altitude: 0.0, + reference_altitude: ALTITUDE_M, + noise_std: BAROMETRIC_ALTITUDE_NOISE_M, + bias_index: None, // no barometric-bias state in this filter + })?; + } + } + // ANCHOR_END: stationary + + let solution = engine.nav_solution(); + println!( + "parked for {:.0} s, {pseudo_measurements} ZUPT/ZARU pairs", + solution.elapsed_s + ); + println!( + " gyro bias estimate z: {:.5} rad/s (injected {:.5})", + solution.gyro_bias[2], gyro_bias.z + ); + println!( + " speed: {:.4} m/s, altitude {:.2} m", + solution.velocity_north.hypot(solution.velocity_east), + solution.altitude + ); + Ok(()) +} + +/// Scene 2: a tilt-compensated magnetometer heading with WMM declination. +fn magnetometer_heading() { + // ANCHOR: magnetometer + let mut magnetometer = MagnetometerYawMeasurement::default(); // NED, 0.05 rad noise + magnetometer.apply_declination = true; + magnetometer.year = 2025; + magnetometer.day_of_year = 1; + let declination = magnetometer.get_declination(LATITUDE_DEG, LONGITUDE_DEG, ALTITUDE_M); + + // A field whose horizontal part points to magnetic north, `declination` east of true + // north, seen from a level vehicle heading 30 degrees true. + let true_heading = 30.0_f64.to_radians(); + let field_ned = Vector3::new(20.0 * declination.cos(), 20.0 * declination.sin(), 45.0); // uT + let field_body = Rotation3::from_euler_angles(0.0, 0.0, true_heading).inverse() * field_ned; + magnetometer.mag_x = field_body.x; + magnetometer.mag_y = field_body.y; + magnetometer.mag_z = field_body.z; + + // Roll and pitch come from the state; latitude, longitude and altitude feed the WMM. + let state = nalgebra::DVector::from_vec(vec![ + LATITUDE_DEG.to_radians(), + LONGITUDE_DEG.to_radians(), + ALTITUDE_M, + 0.0, + 0.0, + 0.0, + 0.0, // roll + 0.0, // pitch + 0.0, // yaw: not used to form the measurement + ]); + let measured = magnetometer.get_measurement(&state); + // ANCHOR_END: magnetometer + + if let Ok(measured) = measured { + println!( + "magnetometer: declination {:.2} deg, true heading {:.2} deg, measured {:.2} deg", + declination.to_degrees(), + true_heading.to_degrees(), + measured[0].to_degrees() + ); + } +} + +/// Scene 3: a chi-squared gate, a spoofed fix, and the recovery that bounds a rejection run. +fn spoofed_fix() -> Result<(), StrapdownError> { + const SPEED_MPS: f64 = 10.0; + let mut engine = InsEngine::builder() + .with_initial_state(InitialState::new( + LATITUDE_DEG, + LONGITUDE_DEG, + ALTITUDE_M, + SPEED_MPS, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + true, + None, + )) + .build()?; + + // ANCHOR: gate + // Reject what the filter's own model says should happen less than 0.1% of the time. The + // threshold depends on the measurement's dimension, so one confidence serves a 1-D + // barometer and a 3-D position fix alike. + let gate = InnovationGate::chi_squared(0.999)?; + engine.set_innovation_gate(Some(gate)); + // Already the default; written out to show the knobs: double the covariance in the + // observed directions on every rejection, force an update after five in a row. + engine.set_gate_recovery(GateRecovery::new(2.0, Some(5))?); + // ANCHOR_END: gate + + for dof in [1, 3, 5] { + println!( + "chi-squared 0.999 threshold, {dof} dof: {:.2}", + chi_squared_quantile(0.999, dof) + ); + } + + let level = IMUData { + accel: Vector3::new(0.0, 0.0, -9.81), + gyro: Vector3::zeros(), + }; + for second in 1..=70 { + for _ in 0..100 { + engine.predict_rates(&level, DT)?; + } + let north_m = SPEED_MPS * f64::from(second); + // From t = 61 s on, the receiver reports a position 200 m east of the truth. + let spoof_m = if second > 60 { 200.0 } else { 0.0 }; + let fix = GnssFix::position( + LATITUDE_DEG + north_m * METERS_TO_DEGREES, + LONGITUDE_DEG + spoof_m * METERS_TO_DEGREES / LATITUDE_DEG.to_radians().cos(), + ALTITUDE_M, + 3.0, + 5.0, + ); + // ANCHOR: outcome + let outcome = engine.update_gnss(&fix)?; + if second > 58 { + println!( + "t = {second:2} s NIS {:>10.1} accepted {:<5} forced {}", + outcome.nis, outcome.accepted, outcome.forced + ); + } + // ANCHOR_END: outcome + } + Ok(()) +} diff --git a/core/examples/basic_ins.rs b/core/examples/basic_ins.rs index 56cb5952..11996993 100644 --- a/core/examples/basic_ins.rs +++ b/core/examples/basic_ins.rs @@ -56,9 +56,10 @@ fn main() -> Result<(), StrapdownError> { 0.0, // pitch 0.0, // yaw: heading north true, // the angles above are in degrees - None, // no initial bias estimate + None, // frame: `None` is the crate default, NED ); + // ANCHOR: build // NED, no antenna lever arm. `InsEngineConfig::default()` is NED already; naming it here // is documentation rather than necessity. let mut engine = InsEngine::builder() @@ -69,6 +70,7 @@ fn main() -> Result<(), StrapdownError> { }) .with_initial_state(initial_state) .build()?; + // ANCHOR_END: build println!("start: {}", engine.nav_solution()); @@ -86,14 +88,17 @@ fn main() -> Result<(), StrapdownError> { // // In ENU the same reading is +9.81 on the up axis, which is what the Sensor Logger // exports in `core/tests/test_data.csv` look like. + // ANCHOR: imu let stationary_imu = IMUData { accel: Vector3::new(0.0, 0.0, -9.81), gyro: Vector3::zeros(), }; + // ANCHOR_END: imu let steps = (DURATION_S / DT) as usize; let gnss_every = (GNSS_INTERVAL_S / DT) as usize; + // ANCHOR: loop for step in 1..=steps { engine.predict(&ImuSample::from_rates(&stationary_imu, DT))?; @@ -126,6 +131,7 @@ fn main() -> Result<(), StrapdownError> { } let solution = engine.nav_solution(); + // ANCHOR_END: loop println!("after {DURATION_S:.0} s: {solution}"); let expected_north_m = SPEED_MPS * DURATION_S; diff --git a/core/examples/eskf.rs b/core/examples/eskf.rs new file mode 100644 index 00000000..fbad1e1f --- /dev/null +++ b/core/examples/eskf.rs @@ -0,0 +1,162 @@ +//! The default filter end to end: build an ESKF, run it through a GNSS outage, score it. +//! +//! Run it with: +//! +//! ```bash +//! cargo run -p strapdown-core --example eskf +//! ``` +//! +//! This is the library route to what `strapdown-sim cl` does, minus the file I/O: +//! +//! 1. generate a five-minute synthetic drive and its exact truth trajectory; +//! 2. build the 15-state error-state Kalman filter with `initialize_eskf`, plus the +//! barometric-bias state `strapdown-sim` turns on by default; +//! 3. turn the sensor log into an event stream with a 60 s GNSS outage in the middle; +//! 4. run the closed loop and score the result against truth with `metrics::evaluate`. +//! +//! It prints the filter's own horizontal uncertainty beside its actual error at the end of the +//! outage, and the accelerometer and gyroscope bias estimates it finishes with. + +use std::error::Error; + +use rand::SeedableRng; +use rand::rngs::StdRng; + +use strapdown::earth::{haversine_distance, principal_radii}; +use strapdown::messages::{AidingConfig, GnssFaultModel, MeasurementScheduler, build_event_stream}; +use strapdown::metrics::{MetricOptions, evaluate, truth_from_trajectory}; +use strapdown::sim::{ + EskfConfig, NavigationResult, SyntheticConfig, SyntheticInitialState, generate_synthetic, + initialize_eskf, run_closed_loop, +}; +use strapdown::{IMUQuality, NavigationFilter}; + +/// GNSS is available for this long, then denied for `OUTAGE_S`, then available again. +const AIDED_S: f64 = 120.0; +/// Length of the GNSS outage, seconds. +const OUTAGE_S: f64 = 60.0; + +fn main() -> Result<(), Box> { + let mut synthetic = SyntheticConfig::default(); + synthetic.initial_state = SyntheticInitialState { + latitude_deg: 39.95, + longitude_deg: -75.16, + altitude_m: 50.0, + velocity_north_mps: 15.0, + velocity_east_mps: 0.0, + velocity_down_mps: 0.0, + roll_deg: 0.0, + pitch_deg: 0.0, + yaw_deg: 0.0, + angular_velocity_x_dps: 0.0, + angular_velocity_y_dps: 0.0, + angular_velocity_z_dps: 0.0, + is_enu: false, + }; + synthetic.duration_s = 300.0; + synthetic.sample_rate_hz = 10.0; + synthetic.imu_quality = IMUQuality::Consumer; + synthetic.seed = 7; + let mut rng = StdRng::seed_from_u64(synthetic.seed); + let (truth, records) = generate_synthetic(&synthetic, &mut rng)?; + + // ANCHOR: build + // `EskfConfig` is `#[non_exhaustive]`: start from the default (NED, 15 error states, bias + // priors from the consumer IMU grade) and assign what differs. + let mut config = EskfConfig::default(); + config.imu_quality = IMUQuality::Consumer; + config.estimate_baro_bias = true; // a 16th error state, as `strapdown-sim cl` uses + let baro_bias_index = config.baro_bias_index(); // where that state landed: Some(15) + let mut eskf = initialize_eskf(&records[0], config)?; + // ANCHOR_END: build + + // ANCHOR: stream + // GNSS on for 120 s, off for 60 s, on again. A duty cycle runs `start_phase_s` of ON, + // then `off_s` OFF and `on_s` ON repeating -- so with a zero phase it starts OFF. The + // barometer and magnetometer keep their own 1 Hz schedules, so the outage removes + // position and velocity aiding only. + let mut aiding = AidingConfig::default(); + aiding.scheduler = MeasurementScheduler::DutyCycle { + on_s: synthetic.duration_s, // longer than what is left: one outage, not a cycle + off_s: OUTAGE_S, + start_phase_s: AIDED_S, + }; + aiding.fault = GnssFaultModel::None; + // The barometer model has to be told which state is its bias; the filter carries it but + // does not observe it unless the measurement names it. + aiding.baro_bias_index = baro_bias_index; + let stream = build_event_stream(&records, &aiding, false)?; // false = NED + // ANCHOR_END: stream + + // ANCHOR: run + // No health or wall-clock limits: `None, None`. `strapdown-sim` passes its defaults here. + let results = run_closed_loop(&mut eskf, stream, None, None)?; + // ANCHOR_END: run + + // ANCHOR: score + let metrics = evaluate( + &results, + &truth_from_trajectory(&truth), + MetricOptions::default(), + )?; + println!("scored {} samples against truth", metrics.sample_count); + if let (Some(rmse), Some(max)) = (metrics.horizontal_rmse_m, metrics.horizontal_max_m) { + println!("horizontal error: RMSE {rmse:.2} m, max {max:.2} m"); + } + // ANCHOR_END: score + + // The filter's claim against its error, at the last sample of the outage. + // The last sample before GNSS returns: the fix at exactly `AIDED_S + OUTAGE_S` is applied + // in that row, so the row before it is the one that has coasted the whole outage. + let outage_end = AIDED_S + OUTAGE_S; + let start = truth[0].timestamp; + let at_outage_end = |row: &&NavigationResult| { + (row.timestamp - start).num_milliseconds() as f64 / 1000.0 >= outage_end - 0.15 + }; + if let (Some(estimate), Some(actual)) = ( + results.iter().find(at_outage_end), + truth.iter().find(at_outage_end), + ) { + print_outage_end(estimate, actual); + } + + // ANCHOR: biases + // Fifteen navigation and IMU-bias states, then the barometric bias: [lat, lon, alt, + // v_n, v_e, v_v, roll, pitch, yaw, b_a (3), b_g (3), baro bias]. + let state = eskf.get_estimate(); + println!( + "final accelerometer bias estimate: [{:.4}, {:.4}, {:.4}] m/s^2", + state[9], state[10], state[11] + ); + println!( + "final gyroscope bias estimate: [{:.5}, {:.5}, {:.5}] rad/s", + state[12], state[13], state[14] + ); + if let Some(index) = eskf.baro_bias_index() { + println!("final barometric bias estimate: {:.3} m", state[index]); + } + // ANCHOR_END: biases + + Ok(()) +} + +/// Print the horizontal error at one epoch beside the filter's own 1-sigma. +fn print_outage_end(estimate: &NavigationResult, truth: &NavigationResult) { + let error_m = haversine_distance( + estimate.latitude.to_radians(), + estimate.longitude.to_radians(), + truth.latitude.to_radians(), + truth.longitude.to_radians(), + ); + // The covariance columns stay in the filter's units: rad^2 for latitude and longitude. + // Back to metres through the WGS84 radii of curvature at the estimate. + let (meridian_m, transverse_m, _) = principal_radii(&estimate.latitude, &estimate.altitude); + let north_sigma_m = estimate.latitude_cov.sqrt() * (meridian_m + estimate.altitude); + let east_sigma_m = estimate.longitude_cov.sqrt() + * (transverse_m + estimate.altitude) + * estimate.latitude.to_radians().cos(); + println!( + "end of outage: error {error_m:.2} m, filter 1-sigma {north_sigma_m:.2} m north, \ + {east_sigma_m:.2} m east" + ); +} diff --git a/core/examples/gnss_outage.rs b/core/examples/gnss_outage.rs index 72afe26c..32714dee 100644 --- a/core/examples/gnss_outage.rs +++ b/core/examples/gnss_outage.rs @@ -82,10 +82,12 @@ fn main() -> Result<(), StrapdownError> { // -9.81 on the down axis in NED. See `basic_ins.rs` for why the sign is what it is. The // bias is added on the body x-axis (forward), where it looks exactly like the vehicle // speeding up. + // ANCHOR: biased_imu let imu = IMUData { accel: Vector3::new(ACCEL_BIAS_MPS2, 0.0, -9.81), gyro: Vector3::zeros(), }; + // ANCHOR_END: biased_imu let total_s = AIDED_BEFORE_S + OUTAGE_DURATION_S + AIDED_AFTER_S; let steps = (total_s / DT) as usize; @@ -122,6 +124,7 @@ fn main() -> Result<(), StrapdownError> { pitch_at_outage_start_deg = solution.pitch; } + // ANCHOR: outage if !in_outage && step % gnss_every == 0 { let fix = GnssFix::position( truth_latitude_deg, @@ -133,6 +136,7 @@ fn main() -> Result<(), StrapdownError> { .with_velocity([SPEED_MPS, 0.0, 0.0], GNSS_VELOCITY_STD_MPS); engine.update_gnss(&fix)?; } + // ANCHOR_END: outage if step % gnss_every == 0 { let solution = engine.nav_solution(); diff --git a/core/examples/kalman_filters.rs b/core/examples/kalman_filters.rs new file mode 100644 index 00000000..ea2cb1f2 --- /dev/null +++ b/core/examples/kalman_filters.rs @@ -0,0 +1,202 @@ +//! The three Kalman-family filters, built two ways and driven through one trait. +//! +//! Run it with: +//! +//! ```bash +//! cargo run -p strapdown-core --example kalman_filters +//! ``` +//! +//! [`strapdown::NavigationFilter`] is object-safe, so the error-state, extended and unscented +//! filters can sit in one `Vec>` and be driven by one loop. This +//! example builds each of them twice: +//! +//! 1. through the `sim::initialize_*` helpers, which is what `strapdown-sim cl` does; and +//! 2. through the filters' own constructors, with an initial covariance derived from an IMU +//! grade by `IMUQuality::auto_covariance` and the crate's default process-noise density. +//! +//! All six then run over the same two-minute synthetic drive with a GNSS position fix once a +//! second, and the example prints each filter's final horizontal error against the exact +//! trajectory and the last +//! [`UpdateOutcome`](strapdown::UpdateOutcome) it returned. The point is the shape of the API, +//! not a ranking: one short run of one trajectory says nothing general about which filter is +//! more accurate. + +use std::error::Error; + +use nalgebra::{DMatrix, DVector, Vector3}; +use rand::SeedableRng; +use rand::rngs::StdRng; + +use strapdown::earth::haversine_distance; +use strapdown::kalman::{ErrorStateKalmanFilter, ExtendedKalmanFilter, UnscentedKalmanFilter}; +use strapdown::measurements::GPSPositionMeasurement; +use strapdown::sim::{ + DEFAULT_PROCESS_NOISE_DENSITY, DEFAULT_UKF_ALPHA, EkfConfig, EskfConfig, NavigationResult, + SyntheticConfig, SyntheticInitialState, TestDataRecord, UkfConfig, generate_synthetic, + initialize_ekf, initialize_eskf, initialize_ukf, +}; +use strapdown::{IMUData, IMUQuality, ImuSample, InitialUncertainty, NavigationFilter}; + +/// How often a GNSS fix is applied, in records. The synthetic log is 10 Hz. +const GNSS_EVERY: usize = 10; + +/// A two-minute synthetic drive due north at 15 m/s, NED, consumer-grade IMU: the exact +/// trajectory and the noisy sensor log derived from it. +fn synthetic_drive() -> Result<(Vec, Vec), Box> { + // ANCHOR: synthetic + let mut config = SyntheticConfig::default(); + config.initial_state = SyntheticInitialState { + latitude_deg: 39.95, + longitude_deg: -75.16, + altitude_m: 50.0, + velocity_north_mps: 15.0, + velocity_east_mps: 0.0, + velocity_down_mps: 0.0, + roll_deg: 0.0, + pitch_deg: 0.0, + yaw_deg: 0.0, + angular_velocity_x_dps: 0.0, + angular_velocity_y_dps: 0.0, + angular_velocity_z_dps: 0.0, + is_enu: false, + }; + config.duration_s = 120.0; + config.sample_rate_hz = 10.0; + config.imu_quality = IMUQuality::Consumer; + config.seed = 42; + + // Returns the exact trajectory and the noisy sensor log derived from it. + let mut rng = StdRng::seed_from_u64(config.seed); + let (truth, records) = generate_synthetic(&config, &mut rng)?; + // ANCHOR_END: synthetic + Ok((truth, records)) +} + +fn main() -> Result<(), Box> { + let (truth, records) = synthetic_drive()?; + let first = &records[0]; + + // ANCHOR: helpers + // The route `strapdown-sim cl` takes: seed everything from the first record. The configs + // are `#[non_exhaustive]`, so start from `default()` and assign; the defaults are NED. + let eskf = initialize_eskf(first, EskfConfig::default())?; + let ekf = initialize_ekf(first, EkfConfig::default())?; + let ukf = initialize_ukf(first, UkfConfig::default())?; + // ANCHOR_END: helpers + + // ANCHOR: initial_covariance + // The same seed state the helpers use: radians, NED (`false`). + let initial_state = first.initial_state(false); + + // P0 from an IMU grade and how well the first fix placed the vehicle. The fifteen entries + // come back in each state's own units: rad^2 for latitude and longitude, m^2 for + // altitude, (m/s)^2, rad^2, (m/s^2)^2 and (rad/s)^2 -- never one literal for all of them. + let uncertainty = InitialUncertainty::new( + first.horizontal_accuracy, // horizontal, m (1-sigma) + first.vertical_accuracy, // vertical, m (1-sigma) + 0.5, // velocity, m/s (1-sigma) + ); + let initial_covariance = + IMUQuality::Consumer.auto_covariance(uncertainty, first.latitude, first.altitude)?; + + // Q as a spectral density: a variance per second on each state. Each filter forms + // Q_k = q * dt itself, so the same matrix is right at 1 Hz and at 100 Hz. + let process_noise_density = + DMatrix::from_diagonal(&DVector::from_row_slice(&DEFAULT_PROCESS_NOISE_DENSITY)); + // ANCHOR_END: initial_covariance + + // ANCHOR: constructors + let no_bias = [0.0; 6]; + // ANCHOR: eskf_new + let eskf_direct = ErrorStateKalmanFilter::new( + &initial_state, + &no_bias, + initial_covariance.to_vec(), + process_noise_density.clone(), + ); + // ANCHOR_END: eskf_new + // ANCHOR: ekf_new + let ekf_direct = ExtendedKalmanFilter::new( + &initial_state, + &no_bias, + initial_covariance.to_vec(), + process_noise_density.clone(), + true, // carry the six IMU-bias states: 15 states, not 9 + ); + // ANCHOR_END: ekf_new + // ANCHOR: ukf_new + let ukf_direct = UnscentedKalmanFilter::new( + &initial_state, + &no_bias, + None, // no extra states beyond the fifteen + initial_covariance.to_vec(), + process_noise_density, + DEFAULT_UKF_ALPHA, // 0.1 + 2.0, // beta: optimal for a Gaussian prior + 0.0, // kappa + ); + // ANCHOR_END: ukf_new + // ANCHOR_END: constructors + + // ANCHOR: trait_objects + let mut filters: Vec<(&str, Box)> = vec![ + ("ESKF, initialize_eskf", Box::new(eskf)), + ("EKF, initialize_ekf", Box::new(ekf)), + ("UKF, initialize_ukf", Box::new(ukf)), + ("ESKF, constructor", Box::new(eskf_direct)), + ("EKF, constructor", Box::new(ekf_direct)), + ("UKF, constructor", Box::new(ukf_direct)), + ]; + // ANCHOR_END: trait_objects + + let final_truth = &truth[truth.len() - 1]; + for (name, filter) in &mut filters { + // ANCHOR: loop + let mut outcome = None; + for (index, pair) in records.windows(2).enumerate() { + let (previous, record) = (&pair[0], &pair[1]); + let dt = (record.time - previous.time).num_milliseconds() as f64 / 1000.0; + + // Rates in, increments mechanized: `from_rates` integrates over `dt`. + let imu = IMUData { + accel: Vector3::new(record.acc_x, record.acc_y, record.acc_z), + gyro: Vector3::new(record.gyro_x, record.gyro_y, record.gyro_z), + }; + filter.predict(&ImuSample::from_rates(&imu, dt), dt)?; + + if (index + 1) % GNSS_EVERY == 0 { + let fix = GPSPositionMeasurement { + latitude: record.latitude, // degrees + longitude: record.longitude, + altitude: record.altitude, + horizontal_noise_std: record.horizontal_accuracy, // metres, 1-sigma + vertical_noise_std: record.vertical_accuracy, + }; + // `Ok` carries the NIS and whether the correction was applied. + outcome = Some(filter.update(&fix)?); + } + } + // ANCHOR_END: loop + + // The estimate is in the filter's native units: radians for latitude and longitude. + let estimate = filter.get_estimate(); + let error_m = haversine_distance( + estimate[0], + estimate[1], + final_truth.latitude.to_radians(), + final_truth.longitude.to_radians(), + ); + if let Some(outcome) = outcome { + println!( + "{name}: {} states, {error_m:.3} m from truth at the end, last update NIS {:.3} \ + ({} dof, accepted: {})", + estimate.len(), + outcome.nis, + outcome.dof, + outcome.accepted + ); + } + } + + Ok(()) +} diff --git a/core/examples/lever_arm.rs b/core/examples/lever_arm.rs new file mode 100644 index 00000000..8706b82c --- /dev/null +++ b/core/examples/lever_arm.rs @@ -0,0 +1,215 @@ +//! From raw IMU samples to an aided solution: calibration, coarse alignment, a P0 from the +//! IMU grade, and an antenna lever arm. +//! +//! Run it with: +//! +//! ```bash +//! cargo run -p strapdown-core --example lever_arm +//! ``` +//! +//! A tactical-grade IMU sits still for 30 s, tilted and pointing 60 degrees east of north, +//! with a GNSS antenna 1.5 m ahead of it and 0.8 m above it. The example +//! +//! 1. removes a known accelerometer bias and scale-factor error with [`ImuCalibration`]; +//! 2. levels and gyrocompasses from the stationary window, and shows the same window being +//! refused for a consumer-grade gyro; +//! 3. builds two engines seeded from that alignment with a P0 from +//! [`IMUQuality::auto_covariance`], one told about the lever arm and one not, and feeds both +//! the same antenna-referred fixes; +//! 4. shows a malformed fix being reported as a *recoverable* error and skipped. +//! +//! The scenario is synthetic and noise-free so that the numbers can be checked by hand. + +use std::error::Error; + +use nalgebra::{Rotation3, Vector3}; + +use strapdown::alignment::{ + GyrocompassConfig, attitude_from_level_and_heading, average_imu, coarse_leveling, + gyrocompassing, +}; +use strapdown::calibration::{ImuCalibration, SensorCalibration}; +use strapdown::earth::{self, METERS_TO_DEGREES, haversine_distance}; +use strapdown::engine::{GnssFix, InsEngine}; +use strapdown::kalman::InitialState; +use strapdown::stationary::{StationaryConfig, StationaryDetector}; +use strapdown::{IMUData, IMUQuality, ImuSample, InitialUncertainty, StrapdownError}; + +const LATITUDE_DEG: f64 = 39.95; +const LONGITUDE_DEG: f64 = -75.16; +const ALTITUDE_M: f64 = 12.0; +/// IMU sample interval, seconds. +const DT: f64 = 0.01; +/// Antenna phase centre relative to the IMU, body frame (x forward, y right, z down), metres. +const LEVER_ARM_M: [f64; 3] = [1.5, 0.0, -0.8]; + +fn main() -> Result<(), Box> { + // Truth: tilted 2 degrees right-wing-down and 1 degree nose-down, heading 060. + let truth_attitude = Rotation3::from_euler_angles( + 2.0_f64.to_radians(), + -1.0_f64.to_radians(), + 60.0_f64.to_radians(), + ); + // What a perfect stationary IMU reads in NED: the reaction to gravity, and Earth rate. + let perfect = IMUData { + accel: truth_attitude.inverse() + * Vector3::new(0.0, 0.0, -earth::gravity(&LATITUDE_DEG, &ALTITUDE_M)), + gyro: truth_attitude.inverse() * earth::earth_rate_lla(&LATITUDE_DEG), + }; + + // ANCHOR: calibration + // The accelerometer's forward error model, as a calibration report would quote it: + // 0.05 m/s^2 of bias on x and a 1% scale-factor error on y. The gyro is taken as perfect. + let accelerometer = SensorCalibration::new( + [0.05, 0.0, 0.0], // bias, m/s^2 + [0.0, 0.01, 0.0], // scale-factor error, fractional + [[0.0; 3]; 3], // misalignment (off-diagonal only) + )?; + let calibration = ImuCalibration::new(accelerometer, SensorCalibration::identity()); + // ANCHOR_END: calibration + + // What the sensor actually reports: b + (I + M) f. + let raw = IMUData { + accel: Vector3::new( + perfect.accel.x + 0.05, + perfect.accel.y * 1.01, + perfect.accel.z, + ), + gyro: perfect.gyro, + }; + + // ANCHOR: alignment + // Keep only the samples the detector vouches for, corrected before anything uses them. + let mut detector = StationaryDetector::new(StationaryConfig::default()); + let mut window = Vec::new(); + for _ in 0..3000 { + let corrected = calibration.correct_rates(&raw); + if detector.push(&corrected) { + window.push(corrected); + } + } + let averaged = average_imu(&window)?; + let level = coarse_leveling(&averaged.accel)?; + + // A consumer gyro's bias is several times Earth rate, so it cannot gyrocompass, and the + // estimator says so instead of returning a heading. + if let Err(refusal) = gyrocompassing( + &averaged, + LATITUDE_DEG, + IMUQuality::Consumer, + &GyrocompassConfig::default(), + ) { + println!("consumer grade: {refusal}"); + } + let heading = gyrocompassing( + &averaged, + LATITUDE_DEG, + IMUQuality::Tactical, + &GyrocompassConfig::default(), + )?; + let attitude = attitude_from_level_and_heading(level, heading.heading_radians); + // ANCHOR_END: alignment + + let (roll, pitch, yaw) = attitude.euler_angles(); + println!( + "aligned from {} stationary samples: roll {:.3}, pitch {:.3}, heading {:.3} deg \ + (claimed 1-sigma {:.1} deg)", + window.len(), + roll.to_degrees(), + pitch.to_degrees(), + yaw.to_degrees(), + heading.uncertainty_radians.to_degrees() + ); + + // ANCHOR: engine + let initial_state = InitialState::new( + LATITUDE_DEG.to_radians(), + LONGITUDE_DEG.to_radians(), + ALTITUDE_M, + 0.0, + 0.0, + 0.0, + roll, + pitch, + yaw, + false, // everything above is in radians + None, // NED + ); + // P0 from the grade and the first fix, then widened on yaw by the heading uncertainty the + // gyrocompass reported -- `auto_covariance` treats heading like roll and pitch. + let mut initial_covariance = IMUQuality::Tactical.auto_covariance( + InitialUncertainty::new(3.0, 5.0, 0.1), + LATITUDE_DEG, + ALTITUDE_M, + )?; + initial_covariance[8] += heading.uncertainty_radians.powi(2); + + let mut engine = InsEngine::builder() + .with_initial_state(initial_state.clone()) + .with_initial_covariance(initial_covariance.to_vec()) + .with_lever_arm(LEVER_ARM_M) + .build()?; + // ANCHOR_END: engine + let mut uncompensated = InsEngine::builder() + .with_initial_state(initial_state) + .with_initial_covariance(initial_covariance.to_vec()) + .build()?; + + // Where the antenna really is: the IMU position plus the lever arm rotated into NED. + let offset = truth_attitude * Vector3::from(LEVER_ARM_M); + let antenna = GnssFix::position( + LATITUDE_DEG + offset.x * METERS_TO_DEGREES, + LONGITUDE_DEG + offset.y * METERS_TO_DEGREES / LATITUDE_DEG.to_radians().cos(), + ALTITUDE_M - offset.z, // altitude is positive up; the NED offset is positive down + 3.0, + 5.0, + ); + + // ANCHOR: run + let sample = ImuSample::from_rates(&raw, DT); + for step in 1..=6000 { + let corrected = calibration.correct(&sample); + engine.predict(&corrected)?; + uncompensated.predict(&corrected)?; + if step % 100 == 0 { + engine.update_gnss(&antenna)?; + uncompensated.update_gnss(&antenna)?; + } + } + // ANCHOR_END: run + + for (name, solution) in [ + ("with lever arm ", engine.nav_solution()), + ("without lever arm", uncompensated.nav_solution()), + ] { + let horizontal_m = haversine_distance( + solution.latitude.to_radians(), + solution.longitude.to_radians(), + LATITUDE_DEG.to_radians(), + LONGITUDE_DEG.to_radians(), + ); + println!( + "{name}: IMU position off by {horizontal_m:.2} m horizontally, {:.2} m vertically", + solution.altitude - ALTITUDE_M + ); + } + + // ANCHOR: recoverable + // A fix with a NaN in it is reported, not applied. `is_recoverable` says whether the + // filter is still usable -- skip the measurement and carry on -- or the run must stop. + let broken = GnssFix::position(f64::NAN, LONGITUDE_DEG, ALTITUDE_M, 3.0, 5.0); + match engine.update_gnss(&broken) { + Ok(outcome) => println!("applied, NIS {:.2}", outcome.nis), + Err(error) if error.is_recoverable() => println!("skipped: {error}"), + Err(error) => return Err(error.into()), + } + // ANCHOR_END: recoverable + + // The offset can change on a running engine, and is validated like the builder's. + let refused: Result<(), StrapdownError> = engine.set_lever_arm([150.0, 0.0, 0.0]); + if let Err(error) = refused { + println!("set_lever_arm: {error}"); + } + + Ok(()) +} diff --git a/core/examples/score_run.rs b/core/examples/score_run.rs new file mode 100644 index 00000000..725359b1 --- /dev/null +++ b/core/examples/score_run.rs @@ -0,0 +1,68 @@ +//! Score a `strapdown-sim` output CSV against a synthetic truth CSV. +//! +//! Run it with: +//! +//! ```bash +//! cargo run -p strapdown-core --example score_run -- output.csv truth.csv +//! ``` +//! +//! `output.csv` is anything `strapdown-sim` writes (`dr`, `cl` or `pf`); `truth.csv` is the +//! exact trajectory `strapdown-sim syn --no-noise` writes for the same flags and seed. Both are +//! `NavigationResult` CSVs, so both load the same way. +//! +//! The scoring is [`strapdown::metrics::evaluate`], the same reduction the accuracy baseline in +//! `core/tests/perf_baseline.rs` gates on. Estimates are matched to truth rows by timestamp, so +//! the two files have to come from the same `syn` epoch; `syn` always starts at +//! 2025-01-01T00:00:00Z, which is what makes that hold. + +use std::error::Error; + +use strapdown::metrics::{MetricOptions, evaluate, truth_from_trajectory}; +use strapdown::sim::NavigationResult; + +/// Print one optional metric, or say it could not be computed for this run. +fn line(label: &str, value: Option, unit: &str) { + let text = match value { + Some(value) => format!("{label:<34} {value:>12.3} {unit}"), + None => format!("{label:<34} {:>12} {unit}", "n/a"), + }; + println!("{}", text.trim_end()); +} + +fn main() -> Result<(), Box> { + let arguments: Vec = std::env::args().collect(); + let [_, estimate_path, truth_path] = arguments.as_slice() else { + return Err("usage: score_run ".into()); + }; + + // ANCHOR: score + let estimates = NavigationResult::from_csv(estimate_path)?; + let truth = truth_from_trajectory(&NavigationResult::from_csv(truth_path)?); + + // Skip the first 10 aligned samples (one second at 10 Hz) as initialisation transient. + let mut options = MetricOptions::default(); + options.warmup_samples = 10; + let metrics = evaluate(&estimates, &truth, options)?; + // ANCHOR_END: score + + println!("{:<34} {:>12}", "aligned samples", metrics.sample_count); + line("horizontal RMSE", metrics.horizontal_rmse_m, "m"); + line("horizontal CEP50", metrics.horizontal_cep50_m, "m"); + line("horizontal CEP95", metrics.horizontal_cep95_m, "m"); + line("horizontal max", metrics.horizontal_max_m, "m"); + line("vertical RMSE", metrics.vertical_rmse_m, "m"); + line( + "horizontal velocity RMSE", + metrics.velocity_horizontal_rmse_mps, + "m/s", + ); + line("yaw RMSE", metrics.yaw_rmse_deg, "deg"); + line("position NEES (consistent at 3)", metrics.nees_position, ""); + line( + "3-sigma horizontal containment", + metrics.containment_3sigma_horizontal, + "", + ); + + Ok(()) +} From a4cdbbedd0bd16118786a481e27e429afa5ef18a Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:02:52 -0400 Subject: [PATCH 06/26] Write netCDF timestamps with their fractional seconds NavigationResult::to_netcdf and TestDataRecord::to_netcdf stored each row's time as timestamp(), whole Unix seconds, and the readers rebuilt it with from_timestamp(t, 0). Every row of a 10 Hz run within one second came back with the same time, so a netCDF file could no longer order or align its own rows. Both writers now store the time as fractional Unix seconds built from whole microseconds, and both readers round back to the nearest microsecond. An f64 holds about 15.9 significant digits and a present-day Unix time in microseconds needs 16, so microseconds are the finest resolution that survives; rounding rather than truncating keeps the round trip exact. A non-finite stored time is now an error rather than a cast to zero. HDF5 (RFC 3339 strings) and MCAP (serde) already kept the fraction. New tests hold all three formats to an exact round trip of sub-second timestamps, and the old netCDF test's note about whole seconds is gone. The output-format page described the truncation and recommended HDF5 or MCAP around it; it now describes the fix. Co-Authored-By: Claude Opus 5.5 --- book/src/user-guide/output-format.md | 8 +- core/src/sim.rs | 155 ++++++++++++++++++++++++--- 2 files changed, 143 insertions(+), 20 deletions(-) diff --git a/book/src/user-guide/output-format.md b/book/src/user-guide/output-format.md index e76e73f2..1cd26751 100644 --- a/book/src/user-guide/output-format.md +++ b/book/src/user-guide/output-format.md @@ -91,13 +91,13 @@ feature of `strapdown-core` that is off by default: | Method | Reader | Feature | Layout | |---|---|---|---| | `NavigationResult::to_hdf5` | `from_hdf5` | `hdf5` | group `navigation_results`, one 1-D dataset per column; `timestamp` as RFC 3339 strings | -| `NavigationResult::to_netcdf` | `from_netcdf` | `netcdf` | dimension `time`, one variable per column; `timestamp` as Unix seconds | +| `NavigationResult::to_netcdf` | `from_netcdf` | `netcdf` | dimension `time`, one variable per column; `timestamp` as fractional Unix seconds | | `NavigationResult::to_mcap` | `from_mcap` | `mcap` | channel `navigation_results`, one MessagePack-encoded record per message, logged at the row's timestamp | All three carry the same 40 columns. HDF5 and NetCDF have no optional type, so an absent bias -column is written as `NaN` and read back as absent. The NetCDF `timestamp` is stored as whole -seconds, so rows less than a second apart share a value there; use HDF5 or MCAP when sub-second -times matter. +column is written as `NaN` and read back as absent. The NetCDF `timestamp` is a double holding +Unix seconds with their fraction, which keeps microsecond resolution; `from_netcdf` rounds it back +to the microsecond, so a 10 Hz run's rows keep distinct times through the round trip. Enable a writer on the dependency, from git until 1.0 is published: diff --git a/core/src/sim.rs b/core/src/sim.rs index a398fabd..2dddc961 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -51,8 +51,8 @@ use std::path::Path; use std::time::{Duration as StdDuration, Instant}; use anyhow::Result; -// `bail!` survives in exactly two places, both `#[cfg(feature = "netcdf")]`: the empty-record -// guards in `to_netcdf`. The v1.0 freeze moved the compute layer -- the two runners and the two +// `bail!` survives only behind `#[cfg(feature = "netcdf")]`: the empty-record guards in +// `to_netcdf` and the timestamp check in `datetime_from_unix_seconds`. The v1.0 freeze moved the compute layer -- the two runners and the two // monitors -- onto `StrapdownError`, leaving anyhow where `core/src/error.rs` says it belongs, // at file I/O. So the import has to carry the same gate as its only users, or a // `--no-default-features` build fails on `unused_imports` under `-D warnings`. @@ -1009,7 +1009,7 @@ impl TestDataRecord { } // Prepare all data arrays first - let times: Vec = records.iter().map(|r| r.time.timestamp() as f64).collect(); + let times: Vec = records.iter().map(|r| unix_seconds(&r.time)).collect(); let bearing_accuracy: Vec = records.iter().map(|r| r.bearing_accuracy).collect(); let speed_accuracy: Vec = records.iter().map(|r| r.speed_accuracy).collect(); let vertical_accuracy: Vec = records.iter().map(|r| r.vertical_accuracy).collect(); @@ -1144,9 +1144,7 @@ impl TestDataRecord { // Build records let mut records = Vec::with_capacity(n); for i in 0..n { - let time = DateTime::from_timestamp(times[i] as i64, 0) - .ok_or_else(|| anyhow::anyhow!("Invalid timestamp"))? - .with_timezone(&Utc); + let time = datetime_from_unix_seconds(times[i])?; records.push(Self { time, @@ -1456,6 +1454,32 @@ const fn none_if_nan(value: f64) -> Option { if value.is_nan() { None } else { Some(value) } } +/// A timestamp as fractional Unix seconds, the form the netCDF writers store it in. +/// +/// Built from whole microseconds rather than `timestamp()`, which is whole seconds: the +/// writers used that, so every row of a 10 Hz run within one second was written with the same +/// time and could no longer be told apart or ordered on reading. An `f64` holds about 15.9 +/// significant digits and a present-day Unix time in microseconds needs 16, so microseconds +/// are the finest unit that survives; [`datetime_from_unix_seconds`] rounds back to them. +#[cfg(feature = "netcdf")] +fn unix_seconds(timestamp: &DateTime) -> f64 { + timestamp.timestamp_micros() as f64 / 1e6 +} + +/// The inverse of [`unix_seconds`], to the nearest microsecond. +/// +/// Rounding rather than truncating matters: `micros / 1e6 * 1e6` is not always exactly +/// `micros` in `f64`, and truncating a value a hair below it would move the timestamp back by +/// a microsecond on the round trip. +#[cfg(feature = "netcdf")] +fn datetime_from_unix_seconds(seconds: f64) -> Result> { + if !seconds.is_finite() { + bail!("invalid timestamp: {seconds} is not a finite number of seconds"); + } + DateTime::from_timestamp_micros((seconds * 1e6).round() as i64) + .ok_or_else(|| anyhow::anyhow!("invalid timestamp: {seconds} s is out of range")) +} + /// Generic result struct for navigation simulations. /// /// This structure contains a single row of position, velocity, and attitude vectors @@ -2019,10 +2043,7 @@ impl NavigationResult { } // Prepare all data arrays - let timestamps: Vec = records - .iter() - .map(|r| r.timestamp.timestamp() as f64) - .collect(); + let timestamps: Vec = records.iter().map(|r| unix_seconds(&r.timestamp)).collect(); let latitude: Vec = records.iter().map(|r| r.latitude).collect(); let longitude: Vec = records.iter().map(|r| r.longitude).collect(); let altitude: Vec = records.iter().map(|r| r.altitude).collect(); @@ -2219,9 +2240,7 @@ impl NavigationResult { // Build records let mut records = Vec::with_capacity(n); for i in 0..n { - let timestamp = DateTime::from_timestamp(timestamps[i] as i64, 0) - .ok_or_else(|| anyhow::anyhow!("Invalid timestamp"))? - .with_timezone(&Utc); + let timestamp = datetime_from_unix_seconds(timestamps[i])?; records.push(Self { latitude_longitude_cov: latitude_longitude_cov[i], @@ -10250,9 +10269,8 @@ mod tests { /// link. The hdf5 side has had round-trip, NaN and missing-column tests all along; this /// is the netCDF half of that. /// - /// Note the whole-second timestamps. `to_netcdf` stores time as `timestamp()`, an integer - /// number of seconds, so sub-second precision does not survive and a test using it would - /// fail for a reason that has nothing to do with netCDF. + /// Sub-second timestamps have their own test, + /// `test_netcdf_keeps_sub_second_timestamps_distinct`. #[cfg(feature = "netcdf")] #[test] fn test_test_data_record_netcdf_roundtrip() { @@ -10352,6 +10370,111 @@ mod tests { "a rejected write must not leave a partial file behind" ); } + /// Timestamps a tenth of a second apart, at microsecond resolution, for the round-trip + /// tests below: the rows a 10 Hz run writes. + #[cfg(any(feature = "netcdf", feature = "hdf5", feature = "mcap"))] + fn sub_second_timestamps() -> Vec> { + let start = DateTime::parse_from_rfc3339("2026-09-24T12:00:00Z") + .unwrap() + .with_timezone(&Utc); + (0..12) + .map(|i| start + Duration::microseconds(100_000 * i + 123_456 * (i % 3))) + .collect() + } + + /// Navigation results stamped with [`sub_second_timestamps`]. + #[cfg(any(feature = "netcdf", feature = "hdf5", feature = "mcap"))] + fn sub_second_navigation_results() -> Vec { + sub_second_timestamps() + .into_iter() + .enumerate() + .map(|(i, timestamp)| { + let mut result = NavigationResult::new(); + result.timestamp = timestamp; + result.latitude = 40.0 + i as f64 * 1e-5; + result + }) + .collect() + } + + /// `to_netcdf` wrote `timestamp()`, whole seconds, so the ten rows a 10 Hz run writes in + /// each second all came back with the same time. Both writers now store fractional seconds + /// and both readers round them back to the microsecond. + #[cfg(feature = "netcdf")] + #[test] + fn test_netcdf_keeps_sub_second_timestamps_distinct() { + use tempfile::tempdir; + + let dir = tempdir().unwrap(); + let expected = sub_second_timestamps(); + + let nav_path = dir.path().join("nav_sub_second.nc"); + NavigationResult::to_netcdf(&sub_second_navigation_results(), &nav_path).unwrap(); + let nav_read: Vec> = NavigationResult::from_netcdf(&nav_path) + .unwrap() + .into_iter() + .map(|result| result.timestamp) + .collect(); + assert_eq!( + nav_read, expected, + "NavigationResult timestamps must round-trip exactly" + ); + + let records: Vec = expected + .iter() + .map(|time| TestDataRecord { + time: *time, + ..Default::default() + }) + .collect(); + let record_path = dir.path().join("records_sub_second.nc"); + TestDataRecord::to_netcdf(&records, &record_path).unwrap(); + let record_read: Vec> = TestDataRecord::from_netcdf(&record_path) + .unwrap() + .into_iter() + .map(|record| record.time) + .collect(); + assert_eq!( + record_read, expected, + "TestDataRecord timestamps must round-trip exactly" + ); + } + + /// The HDF5 writers store RFC 3339 strings, which carry the fraction; held here so the + /// formats stay consistent with each other rather than only with themselves. + #[cfg(feature = "hdf5")] + #[test] + fn test_hdf5_keeps_sub_second_timestamps_distinct() { + use tempfile::tempdir; + + let dir = tempdir().unwrap(); + let path = dir.path().join("nav_sub_second.h5"); + NavigationResult::to_hdf5(&sub_second_navigation_results(), &path).unwrap(); + let read: Vec> = NavigationResult::from_hdf5(&path) + .unwrap() + .into_iter() + .map(|result| result.timestamp) + .collect(); + assert_eq!(read, sub_second_timestamps()); + } + + /// The MCAP writer serializes the whole record, timestamp included, with its fraction. + #[cfg(feature = "mcap")] + #[test] + fn test_mcap_keeps_sub_second_timestamps_distinct() { + use tempfile::tempdir; + + let dir = tempdir().unwrap(); + let path = dir.path().join("nav_sub_second.mcap"); + NavigationResult::to_mcap(&sub_second_navigation_results(), &path).unwrap(); + let read: Vec> = NavigationResult::from_mcap(&path) + .unwrap() + .into_iter() + .map(|result| result.timestamp) + .collect(); + assert_eq!(read, sub_second_timestamps()); + } + #[cfg(feature = "mcap")] #[test] fn test_navigation_result_mcap_roundtrip() { From 757a247e71727de86f86370d31325ca696632666 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:03:09 -0400 Subject: [PATCH 07/26] Correct core docs that contradicted the code - ClosedLoopConfig::innovation_gate showed the YAML form `innovation_gate: { chi_squared: { confidence: 0.999 } }`. InnovationGate is externally tagged, and serde_yaml 0.9 reads that as a tag, `!chi_squared`; the map form fails to parse. The doc now gives the TOML and YAML spellings, and a doctest parses both and checks the map form is refused. - ClosedLoopConfig::filter called the default "the 15-state ESKF", but estimate_baro_bias is on by default, so the ESKF strapdown-sim runs carries sixteen error states. estimate_baro_bias said it was not available on the geophysical path; that path carries it for the UKF and EKF, after the map biases. - The crate docs said the health checks bound altitude to [-11,000 m, 30,000 m]. That is the range the mechanization is valid over; the default health band is +/-1e8 m and enforces nothing like it. Both are now stated, in lib.rs and CLAUDE.md. - principal_radii swapped the names: r_n is the meridian radius (Groves' R_N) and r_e the transverse radius (R_E). The formulas were right. - gravity said it excluded centrifugal effects. Somigliana normal gravity is the magnitude of gravitation plus centrifugal acceleration, so it includes them. gravitation's doc, which leaned on the old claim, now says it adds the centrifugal term on top and so counts it twice; nothing in the mechanization calls it. - The ESKF doc labelled the position error (m); inject_error_state adds it to latitude and longitude in radians and altitude in metres. - perf_baseline.rs said run_closed_loop labels rows one step early. #367 fixed that, as the file's own module docs say. The comments now give the history and why the 50 Hz rate stays. No baseline value changes. Co-Authored-By: Claude Opus 5.5 --- CLAUDE.md | 6 ++-- core/src/earth.rs | 28 ++++++++++++------- core/src/kalman.rs | 2 +- core/src/lib.rs | 13 ++++++--- core/src/sim.rs | 55 +++++++++++++++++++++++++++++++++---- core/tests/perf_baseline.rs | 16 ++++++----- 6 files changed, 91 insertions(+), 29 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 977c4304..eb887732 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -229,8 +229,10 @@ The Free Core implementation must achieve the following capabilities: - Users control via `is_enu` boolean flags and sign conventions; `StrapdownState::to_ned` and `to_enu` convert an existing state between the two - Vertical velocity: positive down in NED, positive up in ENU - - `altitude` is height above the ellipsoid -- positive up -- in **both** frames, valid over - [-11,000m, 30,000m]. It is not a "down" coordinate in NED + - `altitude` is height above the ellipsoid -- positive up -- in **both** frames. The + mechanization is valid over [-11,000m, 30,000m], but nothing enforces that by default: the + health check's default altitude band is +/-1e8 m (`DEFAULT_HEALTH_ALT_{MIN,MAX}_M`), so + narrow `HealthLimits::alt_m` to make it a gate. It is not a "down" coordinate in NED - `TestDataRecord` carries no frame tag, so every entry point that loads one takes the frame from its caller: `sim::dead_reckoning(records, is_enu)` and `is_enu` on `UkfConfig`/`EkfConfig`/`EskfConfig`, all defaulting to NED. On the CLI that is `--enu` diff --git a/core/src/earth.rs b/core/src/earth.rs index cef8ffe3..3c25d3b9 100644 --- a/core/src/earth.rs +++ b/core/src/earth.rs @@ -432,10 +432,14 @@ pub fn haversine_distance(lat1_rad: f64, lon1_rad: f64, lat2_rad: f64, lon2_rad: /// - `altitude` - The WGS84 altitude in meters /// /// # Returns -/// A tuple of the principal radii of curvature (`r_n`, `r_e`, `r_p`) in meters where `r_n` is the radius -/// of curvature in the prime vertical (alternatively as _N_ or `R_N`), `r_e` is the radius of curvature -/// in the meridian (alternatively _M_ or `R_M`), and `r_p` is the radius of curvature in the local -/// normal direction. +/// A tuple (`r_n`, `r_e`, `r_p`) in meters, following Groves' naming: +/// - `r_n` is the **meridian** radius of curvature, Groves' $R_N$ and often written +/// _M_ elsewhere: $a(1 - e^2) / (1 - e^2 \sin^2 L)^{3/2}$. It relates north velocity to +/// latitude rate. +/// - `r_e` is the **transverse** (prime-vertical) radius of curvature, Groves' $R_E$ +/// and often written _N_ elsewhere: $a / \sqrt{1 - e^2 \sin^2 L}$. It relates +/// east velocity to longitude rate. +/// - `r_p` is `r_e * cos(latitude) + altitude`. /// /// # Example /// ```rust @@ -461,9 +465,10 @@ pub fn principal_radii(latitude: &f64, altitude: &f64) -> (f64, f64, f64) { /// the Earth's gravity as a function of the latitude and altitude. The gravity model is used to /// calculate the gravitational force scalar in the local-level frame. Free-air correction is applied. /// -/// *Note:* This function returns only the gravity scalar and does not include centrifugal effects -/// nor does it assuming a vector form or make any assumptions about the direction of the gravitional -/// force! +/// *Note:* Somigliana's formula gives **normal gravity**: the magnitude of the gravity vector on +/// the ellipsoid, which is gravitation *plus* the centrifugal acceleration of the Earth's +/// rotation. So the centrifugal effect is already included in this scalar. What it does not give +/// is a direction: it is a magnitude only, and makes no assumption about which way it points. /// /// # Arguments /// - `latitude` - The WGS84 latitude in degrees @@ -491,9 +496,12 @@ pub fn gravity(latitude: &f64, altitude: &f64) -> f64 { /// method](https://en.wikipedia.org/wiki/Theoretical_gravity#Somigliana_equation), which models /// the Earth's gravity as a function of the latitude and altitude. The gravity model is used to /// calculate the gravitational force vector in the local-level frame. This is then combined -/// with the rotational effects of the Earth to calculate the effective gravity vector. This -/// differs from the gravity scalar in that it includes the centrifugal effects of the Earth's -/// rotation. +/// with the rotational effects of the Earth to calculate the effective gravity vector: the +/// centrifugal acceleration is added as a vector to [`gravity`]'s vertical scalar. +/// +/// Because [`gravity`] is Somigliana *normal* gravity, which already includes the centrifugal +/// magnitude, the vertical component here counts it twice -- about 0.034 m/s^2 at the equator, +/// which the `gravitation` unit test pins. Nothing in the mechanization calls this function. /// /// *Note:* This function uses the ENU convention, thus gravity acts along the negative Z-axis /// (downward) in the local-level frame. diff --git a/core/src/kalman.rs b/core/src/kalman.rs index 69686667..36fe0716 100644 --- a/core/src/kalman.rs +++ b/core/src/kalman.rs @@ -1724,7 +1724,7 @@ impl NavigationFilter for ExtendedKalmanFilter { /// /// ## Error State (15 components, always small): /// ```text -/// δx = [δp_n, δp_e, δp_d, // position error (m) +/// δx = [δlat, δlon, δalt, // position error (rad, rad, m) /// δv_n, δv_e, δv_d, // velocity error (m/s) /// δθ_x, δθ_y, δθ_z, // attitude error (small angles, rad) /// δb_ax, δb_ay, δb_az, // accelerometer bias error (m/s²) diff --git a/core/src/lib.rs b/core/src/lib.rs index c4e4aedc..ab01f52f 100644 --- a/core/src/lib.rs +++ b/core/src/lib.rs @@ -98,12 +98,17 @@ //! //! This mechanization and coordinate frame is only valid for positions relatively close to the Earth's surface (within 30 km above mean sea level). //! Above that it is more common to use the Earth-Centered Earth-Fixed (ECEF) frame for navigation. Additionally, the deepest ocean trenches -//! are approximately 11 km below mean sea level. Thus, this mechanization is not valid for positions deeper than that. [`sim::health`] -//! implements general sanity checks to ensure that the position states remain within valid bounds, given a specific coordinate frame: +//! are approximately 11 km below mean sea level. Thus, this mechanization is not valid for positions deeper than that, and the +//! range it is meant for is altitude in [-11,000 m, 30,000 m]. `altitude` is height above the ellipsoid, positive up, +//! irrespective of `is_enu`; it is not a "down" coordinate in NED. +//! +//! [`sim::health`] implements run-level sanity checks on the position states. Its defaults +//! ([`sim::HealthLimits::default`]) are bounds on what is representable, not on where the mechanization is valid: //! - Latitude: [-90 deg, 90 deg] //! - Longitude: [-180 deg, 180 deg] -//! - Altitude: [-11,000 m, 30,000 m] in both frames. `altitude` is height above the ellipsoid, -//! positive up, irrespective of `is_enu`; it is not a "down" coordinate in NED. +//! - Altitude: [-1e8 m, 1e8 m] -- deliberately far wider than the valid range above, so a diverging vertical +//! channel is caught by the finiteness and covariance checks. Narrow [`sim::HealthLimits::alt_m`] (or pass +//! `--health-alt-min-m`/`--health-alt-max-m` to `strapdown-sim`) to enforce the valid range. //! //! ### Strapdown equations in the Local-Level Frame //! diff --git a/core/src/sim.rs b/core/src/sim.rs index 2dddc961..8e0f390c 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -5190,7 +5190,8 @@ pub enum ParticleFilterType { #[serde(default)] #[non_exhaustive] pub struct ClosedLoopConfig { - /// Filter type; defaults to the 15-state ESKF. + /// Filter type; defaults to the error-state Kalman filter (ESKF): 15 error states, plus a + /// barometric bias state unless [`Self::estimate_baro_bias`] is `false`. pub filter: FilterType, /// Sigma-point spread for the unscented transform. /// @@ -5265,11 +5266,53 @@ pub struct ClosedLoopConfig { /// scenario, and that is a decision to make against the ground-truth validation /// suite rather than as a side effect of adding the capability. /// - /// Deserializes from either form: + /// [`InnovationGate`] is an externally tagged enum, so the variant name is the key in TOML + /// and JSON and a **tag** in YAML -- `serde_yaml` reads `!chi_squared`, not a + /// `chi_squared:` key: + /// + /// ```toml + /// [closed_loop] + /// innovation_gate = { chi_squared = { confidence = 0.999 } } + /// # or: innovation_gate = { fixed = { threshold = 25.0 } } + /// ``` + /// /// ```yaml - /// innovation_gate: { chi_squared: { confidence: 0.999 } } - /// innovation_gate: { fixed: { threshold: 25.0 } } + /// closed_loop: + /// innovation_gate: !chi_squared { confidence: 0.999 } + /// # or: innovation_gate: !fixed { threshold: 25.0 } + /// ``` + /// + /// Both spellings, parsed: + /// /// ``` + /// use strapdown::gating::InnovationGate; + /// use strapdown::sim::ClosedLoopConfig; + /// + /// let from_yaml: ClosedLoopConfig = + /// serde_yaml::from_str("innovation_gate: !chi_squared { confidence: 0.999 }")?; + /// let from_toml: ClosedLoopConfig = + /// toml::from_str("innovation_gate = { fixed = { threshold = 25.0 } }")?; + /// + /// assert_eq!( + /// from_yaml.innovation_gate, + /// Some(InnovationGate::ChiSquared { confidence: 0.999 }) + /// ); + /// assert_eq!( + /// from_toml.innovation_gate, + /// Some(InnovationGate::Fixed { threshold: 25.0 }) + /// ); + /// + /// // The map form is not the YAML spelling of this enum. + /// assert!(serde_yaml::from_str::( + /// "innovation_gate: { chi_squared: { confidence: 0.999 } }" + /// ) + /// .is_err()); + /// # Ok::<(), Box>(()) + /// ``` + /// + /// Deserialization does not range-check the values; `strapdown-sim` refuses an + /// out-of-range confidence or threshold before a run starts, as it does for the + /// `--gate-confidence` flag. #[serde(default)] pub innovation_gate: Option, /// How the filter recovers from a measurement the gate rejected. @@ -5310,7 +5353,9 @@ pub struct ClosedLoopConfig { /// `UkfConfig`/`EkfConfig`/`EskfConfig` defaults stay `false` -- flipping those would hand /// a direct caller a sixteenth state that nothing reads. /// - /// Not available on the geophysical path, whose extra states are map biases. + /// Honoured on the geophysical path too, by the UKF and EKF that path runs: the map-bias + /// states are appended after the fifteen navigation and IMU-bias states and the barometric + /// bias follows them, last. pub estimate_baro_bias: bool, } diff --git a/core/tests/perf_baseline.rs b/core/tests/perf_baseline.rs index c0fbeb93..a9a2e0ff 100644 --- a/core/tests/perf_baseline.rs +++ b/core/tests/perf_baseline.rs @@ -484,9 +484,9 @@ fn scenarios() -> Vec { }); } - // The headline v1.0 case. Errors here reach tens of metres, so the one-step labelling - // offset described in the module docs is a small share of the number rather than most of - // it -- which is what makes this row a usable regression detector on real data. + // The headline v1.0 case. Errors here reach tens of metres, well above the receiver noise + // and the self-scoring floor the module docs describe, which is what makes this row a + // usable regression detector on real data. out.push(Scenario { id: "real_outage_60s__eskf".to_string(), description: "test_data.csv, 120 s of GNSS then a 60 s outage, repeating".to_string(), @@ -619,10 +619,12 @@ fn baseline_path() -> PathBuf { /// The synthetic trajectory every `syn_*` scenario runs over. /// -/// 50 Hz, not the 10 Hz default, on purpose: `run_closed_loop` labels each row with the -/// timestamp of the event *before* the one it has already applied, so a solution carries one -/// propagation step of along-track offset. At 50 m/s that is 1 m at 50 Hz and 5 m at 10 Hz, -/// and the point of these rows is to resolve filter error of a few metres. +/// 50 Hz, not the 10 Hz default. The rate was chosen while `run_closed_loop` labelled each row +/// with the timestamp of the event *before* the one it had already applied, so a solution +/// carried one propagation step of along-track offset -- 1 m at 50 m/s and 50 Hz, 5 m at 10 Hz, +/// against filter errors of a few metres. #367 fixed that labelling: a row stamped `t_k` now +/// holds every event at or before `t_k`. The rate stays because the baseline was blessed at it, +/// and changing it would move every `syn_*` number for a reason unrelated to any filter. fn synthetic_config(duration_s: f64) -> SyntheticConfig { // Not a `const fn` any more: `SyntheticConfig` is `#[non_exhaustive]` as of the v1.0 API // freeze, so it cannot be built from a struct literal outside its own crate, and From 5ab051fa4bd1766d68247a80609ec371570c46ea Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:05:33 -0400 Subject: [PATCH 08/26] Make strapdown-sim's flags, outputs and exit codes do what they say Each of these was accepted and then ignored, or failed in a way that hid it: - `config` with stdin at end of file looped forever on "Configuration path cannot be empty": read_user_input read `read_line`'s Ok(0) as an empty answer, and every prompt without a default re-asks. End of input (and a read error) now ends the wizard with a message on stderr and status 1. - `pf --horizontal-process-noise-std-m 0.5,0.5`, the form its help text shows, failed with "2 values required but 1 was provided": clap counts arguments before splitting on the delimiter. `num_args = 1..=2` takes `0.5,0.5` and `0.5 0.5`; the existing length check still refuses one value. - A config file's `[closed_loop]` gate was never range-checked. Deserialization bypasses InnovationGate::chi_squared and GateRecovery::new, so confidence = 1.5 or rejection_inflation = 0.5 ran with exit 0 where the same flags were refused. Both paths now go through one validator, validated_gating, and a config run refuses a bad gate before reading any input. forced_update_after = 0 still means "never force", as --gate-force-after 0 does, since TOML has no null. - --plot and --parallel are global flags but only run_from_config read them. dr, cl (with and without --geo) and pf now honour both, through one batch runner shared with the config path; dr results are plotted too, on both paths. syn, config and ol refuse them, and --plot is refused by a build without the plotting feature. - A cl batch logged a filter failure and exited 0, and dr/pf did the same for files with no usable records while aborting the batch on a filter failure. Every subcommand now runs the rest of the batch and exits non-zero naming how many files failed, as the config path did. - dr accepted the execution and health limit flags and applied none. It now runs the new strapdown::sim::dead_reckoning_with_limits, which checks the wall-clock budgets and the latitude/longitude/altitude/speed bounds after every step; dead_reckoning itself is unchanged and stays unbounded. Dead reckoning has no covariance and no measurement update, and pf computes no NIS, so each limit flag's help now says which modes apply it rather than inventing a value. - `syn -o x.parquet` wrote CSV under a Parquet name, and `dr/cl/pf -o out.h5` made a directory called out.h5 full of CSV. An output path whose extension names another data format (h5, hdf5, nc, netcdf, mcap, parquet, json, yaml, yml, toml, txt) is now refused with a pointer at the library writers, unless it is an existing directory; `syn` requires a .csv file. Extension-less paths are still directories. - The `Using aiding config` log line was printed before baro_bias_index was derived, so every cl log reported None. It is logged after the derivation. - CreateConfigArgs was dead code. The wizard offered Open-Loop, which is not implemented; it now offers Synthetic instead, which needs only an output file and a seed to produce a runnable config. - --no-estimate-baro-bias's help began "Estimate a barometric altitude bias", the opposite of the flag. The cl help, --filter help and top-level help called the default filter the 15-state ESKF; with the barometric bias on by default it carries sixteen error states, and now says so. The bare URL in the module docs is wrapped so rustdoc accepts it. The user guide described several of these as current behaviour (the parquet directory, inert dr limits, plot/parallel config-only, unchecked file gates, the comma form failing, the wizard's Open-Loop); those pages, the FAQ, the installation page and CLAUDE.md now describe the fixed behaviour. Co-Authored-By: Claude Opus 5.5 --- CLAUDE.md | 15 +- book/src/faq.md | 8 +- book/src/installation/installation.md | 8 +- book/src/user-guide/closed-loop.md | 4 +- book/src/user-guide/configuration.md | 8 +- book/src/user-guide/dead-reckoning.md | 10 +- book/src/user-guide/overview.md | 2 +- book/src/user-guide/particle-filter.md | 6 +- book/src/user-guide/simulations.md | 68 +- book/src/user-guide/synthetic.md | 6 +- core/src/sim.rs | 115 ++- sim/src/common.rs | 237 ++++- sim/src/main.rs | 1215 +++++++++++++++--------- 13 files changed, 1142 insertions(+), 560 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index eb887732..279aa9fd 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -43,10 +43,13 @@ Command-line tool for running INS simulations with GNSS degradation: - GNSS fault simulation: dropouts, reduced update rates, measurement corruption, bias injection - Input: CSV files with IMU and GNSS measurements (Sensor Logger format) - Output: **the CLI writes CSV only.** Every path through `strapdown-sim` ends in - `NavigationResult::to_csv`, and `OUTPUT_FILE_EXTENSIONS` in `sim/src/common.rs` is `["csv"]`, - so any other extension is rejected rather than filled with CSV. `to_hdf5`, `to_netcdf` and - `to_mcap` exist as **library** writers on `NavigationResult`, reachable from Rust but not - from this binary. There is no Parquet writer at all + `NavigationResult::to_csv`. In `sim/src/common.rs`, a `-o` ending in `.csv` + (`OUTPUT_FILE_EXTENSIONS`) names a file; one whose extension names another data format + (`REFUSED_OUTPUT_EXTENSIONS`: h5, hdf5, nc, netcdf, mcap, parquet, json, yaml, yml, toml, txt) + is refused unless it is an existing directory; anything else is a directory of CSV results. + `syn` must be given a `.csv` file. `to_hdf5`, `to_netcdf` and `to_mcap` exist as **library** + writers on `NavigationResult`, reachable from Rust but not from this binary. There is no + Parquet writer at all - Configuration: TOML/YAML/JSON scenario files or command-line arguments - Built-in logging: Use `--log-level` and `--log-file` flags (see `book/src/user-guide/logging.md`) @@ -186,8 +189,8 @@ The Free Core implementation must achieve the following capabilities: 5. **Output Formats**: - CSV, HDF5, NetCDF and MCAP export for analysis in Python/MATLAB/R. **These are `NavigationResult` methods, not CLI output modes**: `strapdown-sim` writes CSV and - rejects every other extension, so reaching the other three means calling `to_hdf5`, - `to_netcdf` or `to_mcap` from Rust + refuses an output path naming another data format, so reaching the other three means + calling `to_hdf5`, `to_netcdf` or `to_mcap` from Rust - Parquet is deliberately **not** supported at either layer: `sim/src/common.rs` refuses `.parquet` rather than naming a file Parquet and filling it with CSV - Navigation solution time series with position, velocity, attitude estimates diff --git a/book/src/faq.md b/book/src/faq.md index 3cb169d2..0c162b76 100644 --- a/book/src/faq.md +++ b/book/src/faq.md @@ -74,10 +74,10 @@ See [Input Data Format](./user-guide/data-format.md). ### What does it write? CSV. Every path through `strapdown-sim` ends in `NavigationResult::to_csv`. An `-o` value ending -in `.csv` is a file; any other value is treated as a directory to write `.csv` -into, so `-o results.parquet` creates a *directory* called `results.parquet`. HDF5, NetCDF and -MCAP writers exist as library methods on `NavigationResult` behind cargo features, not as CLI -options. See [Output Format](./user-guide/output-format.md). +in `.csv` is a file; one ending in another data format's extension (`.h5`, `.nc`, `.mcap`, +`.parquet` and so on) is refused; any other value is a directory to write `.csv` +into. HDF5, NetCDF and MCAP writers exist as library methods on `NavigationResult` behind cargo +features, not as CLI options. See [Output Format](./user-guide/output-format.md). ### My run stops with `InvalidConfiguration { field: "is_enu", ... }` diff --git a/book/src/installation/installation.md b/book/src/installation/installation.md index 13d61bde..ccf0983d 100644 --- a/book/src/installation/installation.md +++ b/book/src/installation/installation.md @@ -93,15 +93,15 @@ Library](../user-guide/library.md) continues from here. | `full` | no | All four of the above | as for `hdf5` | The default build has none of them and compiles with Rust alone. These writers are library -methods only. `strapdown-sim` writes CSV whatever features the library was built with: an -`-o` path that does not end in `.csv` is treated as a *directory*, and the results are written -into it as `.csv` files (`-o out.h5` produces `out.h5/.csv`). +methods only. `strapdown-sim` writes CSV whatever features the library was built with, and +refuses an `-o` path whose extension names another format (`-o out.h5` is an error, not an HDF5 +file); a path with no extension is a directory to write `.csv` files into. ### `strapdown-sim` | Feature | Default | What it adds | Build needs | | --- | --- | --- | --- | -| `plotting` | **yes** | Rendering for the global `--plot` flag, a performance plot against the GNSS track. Without the feature the flag is still accepted, but it logs an error and draws nothing | C compiler (bundled FreeType); libfontconfig at **run time** | +| `plotting` | **yes** | Rendering for the global `--plot` flag, a performance plot against the GNSS track. Without the feature `--plot` is refused, and a config file's `generate_plot = true` logs an error and draws nothing | C compiler (bundled FreeType); libfontconfig at **run time** | | `geonav` | no | Gravity and magnetic anomaly map aiding: `--geo`, `--gravity-*`, `--magnetic-*` and `--geo-interval-s` on `cl` and `pf`. Without the feature these flags do not exist | C/C++ compiler, cmake ≥ 3.26 | `plotting` loads libfontconfig when it first draws text rather than linking it at build time. diff --git a/book/src/user-guide/closed-loop.md b/book/src/user-guide/closed-loop.md index e46e0618..31bf9528 100644 --- a/book/src/user-guide/closed-loop.md +++ b/book/src/user-guide/closed-loop.md @@ -31,8 +31,8 @@ specific to `cl`. |---|---|---| | `--filter ` | `eskf` | `eskf`, `ukf`, `ekf` | -- **`eskf`** -- the 15-state error-state Kalman filter with a multiplicative attitude error. The - default. See [ESKF](../filters/eskf.md). +- **`eskf`** -- the error-state Kalman filter with a multiplicative attitude error: 15 error + states, plus a barometric bias state unless `--no-estimate-baro-bias`. The default. See [ESKF](../filters/eskf.md). - **`ekf`** -- the extended Kalman filter. See [EKF](../filters/ekf.md). - **`ukf`** -- the unscented Kalman filter. See [UKF](../filters/ukf.md). diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index b3ff92ba..1ccfa8f2 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -388,10 +388,10 @@ Consulted only when a gate is installed. Either field may be omitted and keeps i | `rejection_inflation` | `2.0` | each rejection multiplies the filter's uncertainty by this factor in the directions the rejected measurement observed; `1.0` disables it | | `forced_update_after` | `5` | after this many consecutive rejections, apply the next measurement regardless; `0` disables it (YAML may also write `null`) | -The command-line flags `--gate-confidence`, `--gate-inflation` and `--gate-force-after` are -checked before a run starts -- a confidence outside (0, 1) or an inflation below 1 is refused. The -same values in a file are **not** checked: `confidence = 1.5` or `rejection_inflation = 0.5` runs -without an error. Keep file values inside the ranges above. +These values are checked before a run starts, exactly as the command-line flags +`--gate-confidence`, `--gate-inflation` and `--gate-force-after` are: a confidence outside (0, 1), +a fixed threshold that is not positive, an inflation below 1, or a `forced_update_after` of 1 is +refused with an error naming `[closed_loop]`. ## `[particle_filter]` diff --git a/book/src/user-guide/dead-reckoning.md b/book/src/user-guide/dead-reckoning.md index 57d64207..42c004ad 100644 --- a/book/src/user-guide/dead-reckoning.md +++ b/book/src/user-guide/dead-reckoning.md @@ -47,9 +47,13 @@ Three practical consequences of having no filter: and fails on it. Blanking a single `acc_x` cell in a synthetic file stops the run with `Error: NonFinite { what: "propagated attitude matrix" }`, where `cl` on the same file logs a tolerated 0.2 s gap and finishes. -- **The limit flags are inert.** `dr` accepts `--max-wall-clock-*`, `--health-*` and - `--nis-pos-*` and applies none of them, so a run that drifts a long way is written out in full - instead of being cut off. +- **The position and speed limits apply.** `dr` checks the wall-clock budgets + (`--max-wall-clock-*`, `--max-no-progress-s`) and the latitude, longitude, altitude and speed + bounds (`--health-lat-*`, `--health-lon-*`, `--health-alt-*`, `--health-speed-mps-max`) after + every step, as the filters do, and a run that leaves them stops with an error and writes no + file. The default speed bound is 500 m/s, which a long unaided MEMS arc can pass; raise it + when the drift itself is what you want to see. `--health-cov-diag-max` and `--nis-pos-*` do + not apply: there is no covariance and no measurement update. - **The frame check still runs.** Declaring the wrong frame is the one mistake that would make dead reckoning fail at 2 g rather than drift, so it is refused before propagation exactly as in the other modes; see [Input Data Format](./data-format.md#frame-convention). diff --git a/book/src/user-guide/overview.md b/book/src/user-guide/overview.md index 82f1df8f..ca798100 100644 --- a/book/src/user-guide/overview.md +++ b/book/src/user-guide/overview.md @@ -48,7 +48,7 @@ A few options are shared: - **Global:** `--config ` runs a whole scenario from a TOML, YAML or JSON file instead of subcommand flags; `--log-level` and `--log-file` control logging ([Logging](./logging.md)); `--parallel` processes several input files at once; `--plot` draws a performance plot - against the GNSS track. + against the GNSS track. Both work with `dr`, `cl`, `pf` and `--config`. - **Every simulation mode:** `-i` takes a CSV file or a directory of them, `-o` a CSV file or a directory; `--enu` declares ENU input; health limits (`--health-*`) and execution limits (`--max-wall-clock-*`, `--max-no-progress-s`) stop a run that diverges or hangs. diff --git a/book/src/user-guide/particle-filter.md b/book/src/user-guide/particle-filter.md index 66bd272d..4c4fe1ea 100644 --- a/book/src/user-guide/particle-filter.md +++ b/book/src/user-guide/particle-filter.md @@ -82,9 +82,9 @@ use. Pass `0 0` to run the paper's model -- on the 600 s synthetic trajectory ab until the health monitor stops the run (`Error: OutOfRange { what: "speed", ... max: 500.0 }`), while the default `1 1` and `0.5 0.5` both finish. -`--horizontal-process-noise-std-m` takes exactly two values, **separated by a space**: -`--horizontal-process-noise-std-m 0.5 0.5`. The `north,east` comma form that the flag's help text -shows is rejected by the argument parser with `2 values required ... but 1 was provided`. +`--horizontal-process-noise-std-m` takes exactly two values, north then east, separated by a +comma or a space: `--horizontal-process-noise-std-m 0.5,0.5` and +`--horizontal-process-noise-std-m 0.5 0.5` are the same. A single value is refused. ### Barometer loop diff --git a/book/src/user-guide/simulations.md b/book/src/user-guide/simulations.md index b27a9ad2..d5586b20 100644 --- a/book/src/user-guide/simulations.md +++ b/book/src/user-guide/simulations.md @@ -52,18 +52,19 @@ These are accepted before or after the subcommand. | `-c`, `--config ` | -- | Run entirely from a TOML, YAML or JSON file. The format is chosen by extension (`.toml`, `.yaml`/`.yml`, `.json`). | | `--log-level ` | `info` | `off`, `error`, `warn`, `info`, `debug` or `trace`. An unrecognized value prints a warning and falls back to `info`. See [Logging](./logging.md). | | `--log-file ` | stderr | Append log lines to this file instead of stderr. Parent directories are created. | -| `--parallel` | off | Process the files of a directory input concurrently. **Only honoured with `--config`**; see below. | -| `--plot` | off | Write a PNG performance plot beside each result. **Only honoured with `--config`**; see below. | +| `--parallel` | off | Process the files of a directory input concurrently. Honoured by `dr`, `cl`, `pf` and `--config`. | +| `--plot` | off | Write a PNG performance plot beside each result. Honoured by `dr`, `cl`, `pf` and `--config`. | -`--parallel` and `--plot` are read only on the configuration-file path, where they force -`parallel = true` and `generate_plot = true` respectively. A subcommand run accepts both flags -without error and does nothing with them: `cl -i synthetic.csv -o plotdir/eskf.csv --plot` writes -`eskf.csv` and no PNG. To get a plot or parallel processing, use a configuration file. +`--parallel` and `--plot` work the same way on a subcommand as on the configuration-file path, +where they force `parallel = true` and `generate_plot = true`. `--parallel` matters only for a +directory input; a single file runs as before. `syn`, `config` and `ol` have nothing to plot or +parallelize, and refuse either flag with an error rather than ignoring it. -On the configuration path the plot is drawn for closed-loop and particle-filter runs only (a -dead-reckoning config with `generate_plot = true` writes no PNG). It lands beside the CSV with the -extension replaced by `.png`, and needs the `plotting` feature, which is on by default; a failure -to draw it is logged and does not fail the run. +The plot is drawn for every dead-reckoning, closed-loop and particle-filter result. It lands +beside the CSV with the extension replaced by `.png`, and needs the `plotting` feature, which is on +by default. Without the feature, `--plot` is refused before the run starts; a configuration file's +`generate_plot = true` is logged as an error and the run goes on without the plot. A failure to +draw a plot is logged and does not fail the run. ## Input and output paths @@ -81,9 +82,10 @@ The rules, all from `sim/src/common.rs`: - **Whether `--output` is a file or a directory is decided by its extension.** A path ending in `.csv` (any case) names a file; any other path names a directory. An *existing* directory always counts as a directory, even one named `results.csv`. -- **Output is CSV only.** There is no flag that selects another format, and no other extension - makes the output a file: `-o results.parquet` creates a *directory* called `results.parquet` - and writes `.csv` inside it. HDF5, NetCDF and MCAP exist only as library methods; +- **Output is CSV only.** There is no flag that selects another format. A path whose extension + names another data format -- `.h5`, `.hdf5`, `.nc`, `.netcdf`, `.mcap`, `.parquet`, `.json`, + `.yaml`, `.yml`, `.toml` or `.txt` -- is refused, unless it is an existing directory, rather + than made into a directory of CSV files. HDF5, NetCDF and MCAP exist only as library methods; see [Output Format](./output-format.md). - **Missing directories are created**: the output directory itself, or the parent of an output file. @@ -103,10 +105,12 @@ $ strapdown-sim dr -i batch/a.csv -o batch/a.csv Error: "Refusing to write results to 'batch/a.csv': that is the input file. Pass a different --output path." ``` -**Batches.** When the input is a directory, a file that yields no usable records is logged, -skipped and counted, and the rest of the batch runs. A file whose *run* fails is also logged and -skipped on `cl` and on any `--config` run; `dr` and `pf` stop at the first run that fails. A run -that fails writes no output file for that input. +**Batches.** When the input is a directory, a file that fails -- because it yields no usable +records, or because its run fails -- is logged and counted, and the rest of the batch still runs. +If any file failed, the command then exits with status 1 and +`Error: "N of M file(s) failed to process"`, on every subcommand and on a `--config` run alike. A +run that fails writes no output file for that input. With a single input file, its own error is +reported. **Negative numbers** need the `=` form, because clap otherwise reads a leading `-` as a flag: `--longitude-deg=-75`, not `--longitude-deg -75`. @@ -162,11 +166,15 @@ Error: OutOfRange { what: "speed", value: 49.999902224914834, min: 0.0, max: 10. (`cruise.csv` is a 50 m/s synthetic trajectory; see [Synthetic Trajectories](./synthetic.md#a-moving-trajectory).) -**`dr` and `ol` accept all of the limit flags above and apply none of them.** Dead reckoning -never consults the execution or health monitor, so an unaided run that drifts far away is -reported in full rather than cut off. **`pf` applies every limit except the NIS pair**: its loop -checks the state and covariance bounds but passes no NIS to the monitor, so `--nis-pos-max` and -`--nis-pos-consec-fail` have no effect on it. +Not every limit can apply to every mode, and each flag's `--help` says which modes apply it: + +- **`dr` applies the execution limits and the position and speed bounds**, checked after every + propagation step. It carries no covariance and makes no measurement update, so + `--health-cov-diag-max` and the NIS pair do not apply to it. An unaided MEMS arc can pass the + default 500 m/s speed bound on a long recording; raise `--health-speed-mps-max` for those. +- **`pf` applies every limit except the NIS pair**: its loop checks the state and covariance + bounds but computes no NIS, so `--nis-pos-max` and `--nis-pos-consec-fail` have no effect on it. +- **`ol` runs nothing**, so none of them applies. ## Seeds @@ -185,10 +193,11 @@ command on the same input produces the same output. ## `config`: the configuration wizard `strapdown-sim config` takes no arguments. It asks a series of questions on the terminal -- file -name and directory, input and output paths, mode, seed, frame, parallel processing, log level and +name and directory, mode, input and output paths, seed, frame, parallel processing, log level and file, the filter (for closed loop), a GNSS scheduler and fault model, and optional geophysical aiding -- and writes a complete configuration file in the format its extension names. Answer `q` -at any prompt to quit. +at any prompt to quit. The wizard needs a terminal: if standard input closes before it has its +answers (`strapdown-sim config < /dev/null`), it says so on stderr and exits with status 1. The file it writes spells out every section it covers with default values filled in, which makes it a reasonable starting point to edit by hand: @@ -202,11 +211,12 @@ You can now run the simulation with: strapdown-sim --config wiz/template.toml ``` -Two limits: the wizard offers dead reckoning, open loop, closed loop and particle filter but not -synthetic generation, and it offers open loop although that mode is not implemented (a config -with `mode = "open-loop"` exits with status 1 and `Error: "Open-loop mode is not yet fully -implemented"`). When geophysical aiding is enabled on top of the ESKF, the wizard switches the -filter to the UKF, because the ESKF has no geophysical implementation. +The wizard offers dead reckoning, closed loop, particle filter and synthetic generation. It does +not offer open loop, which is not implemented. For synthetic generation it asks only for the +output file (which must end in `.csv`), the seed and the logging, and writes a `[synthetic]` +section at the `syn` defaults for you to edit. When geophysical aiding is enabled on top of the +ESKF, the wizard switches the filter to the UKF, because the ESKF has no geophysical +implementation. ## `ol`: not implemented diff --git a/book/src/user-guide/synthetic.md b/book/src/user-guide/synthetic.md index 97c6eba2..cdd0b3f0 100644 --- a/book/src/user-guide/synthetic.md +++ b/book/src/user-guide/synthetic.md @@ -41,9 +41,9 @@ perfect samples. Noise is then added to produce the sensor records. | `--no-noise` | off | write the truth instead of sensor records; see [below](#truth-with---no-noise) | | `--enu` | off | emit the trajectory in ENU rather than NED | -`syn` takes no `-i`. Its output path is always a file and is used exactly as given, whatever its -extension -- `-o x.parquet` writes CSV into a file named `x.parquet` -- so give it a `.csv` name. -Unlike the simulation subcommands, it never treats the path as a directory. +`syn` takes no `-i`. Its output path is always one file, and must end in `.csv`: any other +extension (`-o x.parquet`) or none is refused rather than given CSV under its name. Unlike the +simulation subcommands, it never treats the path as a directory. ### Initial state diff --git a/core/src/sim.rs b/core/src/sim.rs index 8e0f390c..9e706129 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -3029,7 +3029,12 @@ pub fn check_declared_frame( Ok(()) } -/// Run dead reckoning or "open-loop" simulation using test data. +/// Run a dead-reckoning simulation over test data. +/// +/// Dead reckoning propagates the IMU from the first record's position, velocity and attitude +/// with no aiding at all. It is not the "open loop" (feed-forward) mode that `strapdown-sim ol` +/// reserves, which would estimate errors from GNSS without feeding them back; that mode is not +/// implemented. /// /// This function processes a sequence of sensor records through a `StrapdownState`, using /// the "forward" method to propagate the state based on IMU measurements. It initializes @@ -3041,8 +3046,10 @@ pub fn check_declared_frame( /// only valid at lower latitude (e.g. < 60 degrees) and at low altitudes (e.g. < 1000m). With /// that, remember that dead reckoning is subject to drift and errors accumulate over time relative /// to the quality of the IMU data. Poor quality IMU data (e.g. MEMS grade IMUs) will lead to -/// significant drift very quickly which may cause this function to produce unrealistic results, -/// hang, or crash. +/// significant drift very quickly which may cause this function to produce unrealistic results. +/// +/// No health or execution limit is applied, so an arc that drifts far out of any physical +/// bound is returned in full; [`dead_reckoning_with_limits`] is the bounded form. /// /// # Arguments /// * `records` - Vector of test data records containing IMU measurements and other sensor data @@ -3062,14 +3069,41 @@ pub fn dead_reckoning( records: &[TestDataRecord], is_enu: bool, ) -> Result, StrapdownError> { - if records.is_empty() { + dead_reckoning_with_limits(records, is_enu, None, None) +} + +/// [`dead_reckoning`], bounded by the run-level limits the filters are held to. +/// +/// After every propagation step the state is checked against `health_limits` -- finiteness, +/// the latitude, longitude and altitude bands, and the speed bound -- and the wall-clock and +/// no-progress budgets in `execution_limits` are checked before it. `None` skips that check. +/// +/// Two of the health limits cannot apply and are ignored: dead reckoning has no covariance, so +/// [`HealthLimits::cov_diag_max`] has nothing to test, and makes no measurement update, so +/// there is no NIS for [`HealthLimits::nis_pos_max`]. +/// +/// # Errors +/// As [`dead_reckoning`]; also [`StrapdownError::OutOfRange`] or +/// [`StrapdownError::NonFinite`] when the state leaves `health_limits`, and +/// [`StrapdownError::Timeout`] when a budget in `execution_limits` is exceeded. +pub fn dead_reckoning_with_limits( + records: &[TestDataRecord], + is_enu: bool, + health_limits: Option<&HealthLimits>, + execution_limits: Option<&ExecutionLimits>, +) -> Result, StrapdownError> { + let (Some(first_record), Some(last_record)) = (records.first(), records.last()) else { return Ok(Vec::new()); - } + }; check_declared_frame(records, is_enu)?; + let mut health_monitor = health_limits.map(|limits| HealthMonitor::new(limits.clone())); + let sim_duration_s = (last_record.time - first_record.time).as_seconds_f64(); + let mut execution_monitor = + execution_limits.map(|limits| ExecutionMonitor::new(limits, sim_duration_s)); + // A covariance-free check: the monitor's diagonal sweep has nothing to visit. + let no_covariance = DMatrix::::zeros(0, 0); // Initialize the result vector let mut results = Vec::with_capacity(records.len()); - // Initialize the StrapdownState with the first record - let first_record = &records[0]; // Attitude comes from the record's quaternion, not its Euler angles -- see // `TestDataRecord::attitude` for why the two are not interchangeable and what feeding // the raw angles here used to cost. @@ -3087,10 +3121,12 @@ pub fn dead_reckoning( }; // Store the initial state and metadata results.push(NavigationResult::from((&first_record.time, &state))); - let mut previous_time = records[0].time; + let mut previous_time = first_record.time; // Process each subsequent record for record in records.iter().skip(1) { - // Try to calculate time difference from timestamps, default to 1 second if parsing fails + if let Some(monitor) = execution_monitor.as_ref() { + monitor.check("dead reckoning")?; + } let current_time = record.time; let dt = (current_time - previous_time).as_seconds_f64(); // Create IMU data from the record @@ -3099,6 +3135,20 @@ pub fn dead_reckoning( gyro: Vector3::new(record.gyro_x, record.gyro_y, record.gyro_z), }; mechanize(&mut state, &ImuSample::from_rates(&imu_data, dt))?; + if let Some(monitor) = health_monitor.as_mut() { + let position_and_velocity = [ + state.latitude, + state.longitude, + state.altitude, + state.velocity_north, + state.velocity_east, + state.velocity_vertical, + ]; + monitor.check(&position_and_velocity, &no_covariance, None)?; + } + if let Some(monitor) = execution_monitor.as_mut() { + monitor.mark_progress(); + } results.push(NavigationResult::from((¤t_time, &state))); previous_time = record.time; } @@ -4709,6 +4759,7 @@ pub mod health { /// this crate. Narrow this to the scenario's real speed range to make it an /// effective gate; unaided `dead_reckoning` never calls [`HealthMonitor`], so a run /// that deliberately drifts past this bound (see #299) is unaffected. + /// `dead_reckoning_with_limits`, which `strapdown-sim dr` runs, does apply it. #[serde(default = "default_health_speed_mps_max")] pub speed_mps_max: f64, /// Largest variance allowed on the covariance diagonal before the run is failed @@ -7371,6 +7422,52 @@ mod tests { ); } + /// `dead_reckoning_with_limits` with no limits is `dead_reckoning`, row for row. + #[test] + fn test_dead_reckoning_with_no_limits_matches_dead_reckoning() { + let records = stationary_synthetic_records(false, 10.0); + let plain = dead_reckoning(&records, false).unwrap(); + let bounded = dead_reckoning_with_limits( + &records, + false, + Some(&HealthLimits::default()), + Some(&ExecutionLimits::default()), + ) + .unwrap(); + assert_eq!(plain.len(), bounded.len()); + for (left, right) in plain.iter().zip(&bounded) { + assert_eq!(left.timestamp, right.timestamp); + assert_eq!(left.latitude, right.latitude); + assert_eq!(left.altitude, right.altitude); + } + } + + /// The health bounds reach dead reckoning: `strapdown-sim dr` accepted them and applied + /// none. An altitude band the stationary trajectory sits outside of fails the run, as it + /// would a filter's. + #[test] + fn test_dead_reckoning_with_limits_applies_the_health_bounds() { + let records = stationary_synthetic_records(false, 10.0); + let altitude = records[0].altitude; + let limits = { + let mut built = HealthLimits::default(); + built.alt_m = (altitude + 100.0, altitude + 200.0); + built + }; + let error = dead_reckoning_with_limits(&records, false, Some(&limits), None) + .expect_err("an altitude outside the band must fail the run"); + assert!( + matches!( + error, + StrapdownError::OutOfRange { + what: "altitude", + .. + } + ), + "expected an altitude OutOfRange, got {error:?}" + ); + } + #[test] fn test_dead_reckoning_rejects_a_frame_the_records_contradict() { // Both directions, because the guard has to be a discriminator rather than a diff --git a/sim/src/common.rs b/sim/src/common.rs index 8640c8a4..5b16b3d5 100644 --- a/sim/src/common.rs +++ b/sim/src/common.rs @@ -155,6 +155,88 @@ pub(crate) fn output_names_a_file(output: &Path) -> bool { }) } +/// Extensions of data formats `strapdown-sim` does not write, refused on an output path. +/// +/// Without this a name such as `-o results.h5` was taken as a *directory* -- only `.csv` marks +/// a file, see [`OUTPUT_FILE_EXTENSIONS`] -- so the run made a directory called `results.h5` +/// and filled it with CSV, and `syn -o x.parquet` wrote CSV under a Parquet name. Either way +/// the user asked for a format and silently got another one. +const REFUSED_OUTPUT_EXTENSIONS: [&str; 11] = [ + "h5", "hdf5", "nc", "netcdf", "mcap", "parquet", "json", "yaml", "yml", "toml", "txt", +]; + +/// The error for an output path that names a format other than CSV. +fn csv_only_error(output: &Path, extension: &str) -> Box { + format!( + "Refusing output path '{}': strapdown-sim writes CSV only, so a `.{extension}` name \ + would hold CSV, not {extension}. Name a `.csv` file or a directory. HDF5, NetCDF and \ + MCAP are written by the library rather than the CLI: `NavigationResult::to_hdf5`, \ + `to_netcdf` and `to_mcap` in strapdown-core.", + output.display() + ) + .into() +} + +/// Refuse an output path whose extension names a data format the CLI does not write. +/// +/// An existing directory is left alone whatever it is called, as [`output_names_a_file`] +/// does, and so is any path with another or no extension, which is taken as a directory. +/// +/// # Errors +/// When the extension is one of [`REFUSED_OUTPUT_EXTENSIONS`]. +pub(crate) fn refuse_non_csv_output(output: &Path) -> Result<(), Box> { + if output.is_dir() { + return Ok(()); + } + match output.extension().and_then(|extension| extension.to_str()) { + Some(extension) + if REFUSED_OUTPUT_EXTENSIONS + .iter() + .any(|refused| extension.eq_ignore_ascii_case(refused)) => + { + Err(csv_only_error(output, extension)) + } + _ => Ok(()), + } +} + +/// Validate a `syn` output path, which must be a `.csv` file, and create its parent. +/// +/// `syn` writes exactly one file, so unlike the simulation subcommands it has no directory +/// form: anything but a `.csv` name is refused. +/// +/// # Errors +/// When the path does not end in `.csv`, or its parent directory cannot be created. +pub(crate) fn validate_synthetic_output_path(output: &Path) -> Result<(), Box> { + if output.is_dir() { + return Err(format!( + "Refusing output path '{}': it is a directory, and a synthetic trajectory is \ + written to one `.csv` file.", + output.display() + ) + .into()); + } + match output.extension().and_then(|extension| extension.to_str()) { + Some(extension) if extension.eq_ignore_ascii_case("csv") => {} + Some(extension) => return Err(csv_only_error(output, extension)), + None => { + return Err(format!( + "Refusing output path '{}': a synthetic trajectory is written to one CSV \ + file, so the path must end in `.csv`.", + output.display() + ) + .into()); + } + } + if let Some(parent) = output.parent() + && !parent.as_os_str().is_empty() + && !parent.exists() + { + std::fs::create_dir_all(parent)?; + } + Ok(()) +} + /// Create the directory that results for `output` will be written into. /// /// When `output` names a file (see [`output_names_a_file`]) this creates its *parent*. @@ -165,8 +247,10 @@ pub(crate) fn output_names_a_file(output: &Path) -> bool { /// * `output` - The user's `--output` value, either a result file or a directory /// /// # Errors -/// Returns an error if directory creation fails. +/// Returns an error if `output` names a data format other than CSV (see +/// [`refuse_non_csv_output`]), or if directory creation fails. pub(crate) fn validate_output_path(output: &Path) -> Result<(), Box> { + refuse_non_csv_output(output)?; let directory = if output_names_a_file(output) { output.parent() } else { @@ -352,34 +436,68 @@ pub(crate) fn load_records(input_file: &Path) -> Result, Box // User Input Utilities // ============================================================================ +/// One answer read from the user, before anything acts on it. +#[derive(Debug, PartialEq, Eq)] +pub(crate) enum UserInput { + /// A non-empty line, trimmed. + Answer(String), + /// An empty line: the user pressed Enter, which a prompt with a default takes as "use it". + Empty, + /// `q` or `Q`. + Quit, + /// Input is exhausted (end of file) or could not be read. No further answer will come. + Closed(String), +} + +/// Read and classify one line from `reader`. +/// +/// End of input is reported as [`UserInput::Closed`], not as an empty line. `read_line` +/// returns `Ok(0)` at end of file, and this used to treat that as the user pressing Enter: a +/// prompt with no default then re-asked forever, so `strapdown-sim config < /dev/null` printed +/// "Configuration path cannot be empty" in an endless loop. +pub(crate) fn read_answer(reader: &mut impl io::BufRead) -> UserInput { + let mut input = String::new(); + match reader.read_line(&mut input) { + Ok(0) => UserInput::Closed("end of input".to_string()), + Ok(_) => { + let input = input.trim(); + if input.eq_ignore_ascii_case("q") { + UserInput::Quit + } else if input.is_empty() { + UserInput::Empty + } else { + UserInput::Answer(input.to_string()) + } + } + Err(e) => UserInput::Closed(e.to_string()), + } +} + /// Read a line from stdin, trimming whitespace and checking for quit command. /// /// # Returns /// - `None` if user enters empty input or presses Enter /// - `Some(String)` with the trimmed input otherwise /// -/// Returns `None` when stdin cannot be read, which is the normal case when the tool runs -/// non-interactively — piped input, a closed stdin, or a batch invocation. That used to -/// panic, so a CLI that also supports batch mode aborted rather than falling back. -/// -/// # Panics -/// Exits the process if user enters 'q' or 'Q'. +/// Exits the process with status 0 if the user enters 'q' or 'Q', and with status 1, after +/// saying why on stderr, when stdin is at end of file or cannot be read. Every prompt re-asks +/// until it gets an answer it can use, so an exhausted stdin -- the wizard run with input +/// redirected from an empty or too-short file -- has to end the run rather than be read as +/// another empty answer. pub(crate) fn read_user_input() -> Option { - let mut input = String::new(); - if let Err(e) = io::stdin().read_line(&mut input) { - log::warn!("could not read from stdin: {e}"); - return None; - } - let input = input.trim(); - - if input.eq_ignore_ascii_case("q") { - std::process::exit(0); - } - - if input.is_empty() { - None - } else { - Some(input.to_string()) + let answer = read_answer(&mut io::stdin().lock()); + match answer { + UserInput::Answer(answer) => Some(answer), + UserInput::Empty => None, + UserInput::Quit => std::process::exit(0), + UserInput::Closed(reason) => { + eprintln!( + "\nError: the configuration wizard needs an interactive answer, but standard \ + input closed ({reason}). Run `strapdown-sim config` in a terminal, or write a \ + configuration file by hand." + ); + std::process::exit(1); + } } } @@ -482,6 +600,26 @@ mod tests { use std::fs::File; use tempfile::tempdir; + #[test] + fn test_read_answer_reports_end_of_input_rather_than_an_empty_line() { + // `read_line` returns `Ok(0)` at end of file. Reading that as an empty answer made + // every prompt without a default loop forever on a closed stdin. + let mut exhausted = io::Cursor::new(Vec::::new()); + assert!(matches!(read_answer(&mut exhausted), UserInput::Closed(_))); + } + + #[test] + fn test_read_answer_classifies_lines() { + let mut input = io::Cursor::new(b" results.toml \n\nQ\n".to_vec()); + assert_eq!( + read_answer(&mut input), + UserInput::Answer("results.toml".to_string()) + ); + assert_eq!(read_answer(&mut input), UserInput::Empty); + assert_eq!(read_answer(&mut input), UserInput::Quit); + assert!(matches!(read_answer(&mut input), UserInput::Closed(_))); + } + #[test] fn test_validate_input_path_file() { let dir = tempdir().unwrap(); @@ -796,6 +934,61 @@ mod tests { assert!(output_names_a_file(&dir.path().join("results.csv"))); } + #[test] + fn test_validate_output_path_refuses_non_csv_data_formats() { + // `-o out.h5` and `-o out.parquet` used to create a directory of that name and write + // CSV inside it. + let dir = tempdir().unwrap(); + for name in [ + "out.h5", + "out.parquet", + "out.NC", + "out.mcap", + "out.json", + "out.txt", + ] { + let output = dir.path().join(name); + let error = + validate_output_path(&output).expect_err("a non-CSV data format must be refused"); + assert!( + error.to_string().contains("writes CSV only"), + "unexpected error for {name}: {error}" + ); + assert!(!output.exists(), "{name} must not be created"); + } + } + + #[test] + fn test_validate_output_path_keeps_directories_and_csv_files() { + let dir = tempdir().unwrap(); + // An extension-less path is a directory to write into, and is created. + let fresh = dir.path().join("results"); + validate_output_path(&fresh).unwrap(); + assert!(fresh.is_dir()); + // An existing directory is used whatever it is called. + let existing = dir.path().join("archive.h5"); + std::fs::create_dir_all(&existing).unwrap(); + validate_output_path(&existing).unwrap(); + // A .csv file is still a file. + validate_output_path(&dir.path().join("nested").join("run.csv")).unwrap(); + } + + #[test] + fn test_validate_synthetic_output_path_requires_csv() { + // `syn -o x.parquet` used to write CSV under a Parquet name. + let dir = tempdir().unwrap(); + for name in ["x.parquet", "x.h5", "x", "x.dat"] { + assert!( + validate_synthetic_output_path(&dir.path().join(name)).is_err(), + "{name} must be refused" + ); + } + let output = dir.path().join("new").join("trajectory.csv"); + validate_synthetic_output_path(&output).unwrap(); + assert!(output.parent().unwrap().is_dir()); + assert!(!output.exists()); + } + #[test] fn test_output_names_a_file_prefers_an_existing_directory() { let dir = tempdir().unwrap(); diff --git a/sim/src/main.rs b/sim/src/main.rs index c6b0226a..15550d8b 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -5,8 +5,8 @@ //! - dr: dead reckoning. Propagates the IMU from the initial state with no aiding at all. //! //! - cl: closed loop. Corrects the INS with GNSS, barometric and magnetometer measurements -//! through the 15-state error-state Kalman filter (ESKF) by default, or the EKF or UKF with -//! --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). +//! through the error-state Kalman filter (ESKF) by default -- 15 error states, plus a +//! barometric bias state unless --no-estimate-baro-bias -- or the EKF or UKF with --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). //! //! - pf: the Rao-Blackwellized particle filter, with the same GNSS degradation options. //! @@ -21,7 +21,7 @@ //! 1. Loading all parameters from a configuration file (TOML/JSON/YAML) with --config //! 2. Specifying parameters via command-line flags //! -//! Results are written as CSV. User guide: https://jbrodovsky.github.io/strapdown-rs/ +//! Results are written as CSV. User guide: mod common; #[cfg(feature = "plotting")] @@ -31,14 +31,13 @@ use clap::{Args, Parser, Subcommand}; use common::{ get_csv_files, init_logger, load_records, prompt_config_name, prompt_config_path, prompt_f64_with_default, prompt_input_path, prompt_output_path, read_user_input, - resolve_output_path, validate_input_path, validate_output_path, + resolve_output_path, validate_input_path, validate_output_path, validate_synthetic_output_path, }; use log::{error, info}; use nalgebra::{Vector2, Vector3}; use rayon::prelude::*; use std::error::Error; use std::path::{Path, PathBuf}; -use std::sync::Mutex; use strapdown::messages::{Event, EventStream, MeasurementScheduler, build_event_stream}; use strapdown::rbpf::{RaoBlackwellizedParticleFilter, RbpfConfig}; @@ -65,13 +64,15 @@ use strapdown::sim::health::HealthMonitor; use strapdown::sim::run_closed_loop_with_geo; use strapdown::sim::{ ClosedLoopConfig, EkfConfig, EskfConfig, ExecutionLimits, ExecutionMonitor, ExtraStateLayout, - FaultArgs, FilterType, NavigationResult, ParticleFilterConfig, ParticleFilterType, - SchedulerArgs, SimulationConfig, SimulationMode, SyntheticConfig, TestDataRecord, UkfConfig, - build_fault, build_scheduler, check_declared_frame, dead_reckoning, generate_synthetic, - initialize_ekf, initialize_eskf, initialize_ukf, run_closed_loop, + FaultArgs, FilterType, NavigationResult, ParticleFilterType, SchedulerArgs, SimulationConfig, + SimulationMode, SyntheticConfig, TestDataRecord, UkfConfig, build_fault, build_scheduler, + check_declared_frame, dead_reckoning_with_limits, generate_synthetic, initialize_ekf, + initialize_eskf, initialize_ukf, run_closed_loop, }; #[cfg(feature = "geonav")] -use strapdown::sim::{DEFAULT_PROCESS_NOISE_DENSITY, GeoResolution, GeophysicalConfig}; +use strapdown::sim::{ + DEFAULT_PROCESS_NOISE_DENSITY, GeoResolution, GeophysicalConfig, ParticleFilterConfig, +}; /// The `--log-level` default. /// @@ -88,8 +89,8 @@ This program runs strapdown INS simulations on recorded or synthetic data: - dr: dead reckoning. Propagates the IMU from the initial state with no aiding at all. - cl: closed loop. Corrects the INS with GNSS, barometric and magnetometer measurements - through the 15-state error-state Kalman filter (ESKF) by default, or the EKF or UKF with - --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). + through the error-state Kalman filter (ESKF) by default -- 15 error states, plus a + barometric bias state unless --no-estimate-baro-bias -- or the EKF or UKF with --filter. GNSS can be withheld (outages, reduced rates) or corrupted (noise, drift, spoofing). - pf: the Rao-Blackwellized particle filter, with the same GNSS degradation options. @@ -154,7 +155,7 @@ enum Command { #[command( name = "cl", about = "Run simulation in closed-loop mode", - long_about = "Run INS simulation in a closed-loop (feedback) mode. In this mode, GNSS measurements are incorporated to correct for IMU drift and directly reset or update the navigation states. The filter is the 15-state error-state Kalman filter (ESKF) by default; --filter selects the extended (EKF) or unscented (UKF) Kalman filter instead. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." + long_about = "Run INS simulation in a closed-loop (feedback) mode. In this mode, GNSS measurements are incorporated to correct for IMU drift and directly reset or update the navigation states. The filter is the error-state Kalman filter (ESKF) by default: 15 error states, plus a barometric bias state unless --no-estimate-baro-bias; --filter selects the extended (EKF) or unscented (UKF) Kalman filter instead. Various GNSS degradation scenarios can be simulated, including jamming, reduced update rates, and spoofing." )] ClosedLoop(Box), @@ -184,7 +185,8 @@ With --no-noise: outputs 9-state kinematic truth in NavigationResult CSV format. /// Arguments for the `syn` (synthetic trajectory) command #[derive(Args, Clone, Debug)] struct SyntheticArgs { - /// Output CSV file path + /// Output CSV file path. Must end in `.csv`: `syn` writes one CSV file, and any other + /// extension is refused rather than given CSV under its name. #[arg(short, long)] output: PathBuf, @@ -304,60 +306,74 @@ struct SimArgs { /// Output CSV file path, or a directory to write results into /// A path ending in .csv is treated as a file: a single input writes straight to it, /// and multiple inputs write {`output_stem`}_{`input_stem}.csv` beside it. - /// Any other path is treated as a directory, and each input writes to its own file - /// name inside it. Writing results over an input file is refused. + /// The CLI writes CSV only, so a path naming another data format (.h5, .hdf5, .nc, + /// .netcdf, .mcap, .parquet, .json, .yaml, .yml, .toml, .txt) is refused unless it is an + /// existing directory. Any other path is treated as a directory, and each input writes to + /// its own file name inside it. Writing results over an input file is refused. #[arg(short, long, value_parser)] output: PathBuf, - /// Max wall-clock time as a ratio of simulated duration (<= 0 disables) + /// Max wall-clock time as a ratio of simulated duration (<= 0 disables). Applied by dr, + /// cl and pf; ol runs nothing #[arg(long, default_value_t = strapdown::sim::DEFAULT_MAX_WALL_CLOCK_RATIO)] max_wall_clock_ratio: f64, - /// Max wall-clock time per trajectory in seconds (<= 0 disables) + /// Max wall-clock time per trajectory in seconds (<= 0 disables). Applied by dr, cl and + /// pf; ol runs nothing #[arg(long, default_value_t = strapdown::sim::DEFAULT_MAX_WALL_CLOCK_S)] max_wall_clock_s: f64, - /// Max wall-clock time without progress in seconds (<= 0 disables) + /// Max wall-clock time without progress in seconds (<= 0 disables). Applied by dr, cl and + /// pf; ol runs nothing #[arg(long, default_value_t = strapdown::sim::DEFAULT_MAX_NO_PROGRESS_S)] max_no_progress_s: f64, - /// Minimum latitude the filter estimate may reach, in degrees, before the run is failed + /// Minimum latitude the estimate may reach, in degrees, before the run is failed. Applied + /// by dr, cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LAT_MIN_RAD.to_degrees())] health_lat_min_deg: f64, - /// Maximum latitude the filter estimate may reach, in degrees, before the run is failed + /// Maximum latitude the estimate may reach, in degrees, before the run is failed. Applied + /// by dr, cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LAT_MAX_RAD.to_degrees())] health_lat_max_deg: f64, - /// Minimum longitude the filter estimate may reach, in degrees, before the run is failed + /// Minimum longitude the estimate may reach, in degrees, before the run is failed. Applied + /// by dr, cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LON_MIN_RAD.to_degrees())] health_lon_min_deg: f64, - /// Maximum longitude the filter estimate may reach, in degrees, before the run is failed + /// Maximum longitude the estimate may reach, in degrees, before the run is failed. Applied + /// by dr, cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LON_MAX_RAD.to_degrees())] health_lon_max_deg: f64, - /// Minimum altitude in metres above the ellipsoid before the run is failed + /// Minimum altitude in metres above the ellipsoid before the run is failed. Applied by dr, + /// cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_ALT_MIN_M)] health_alt_min_m: f64, - /// Maximum altitude in metres above the ellipsoid before the run is failed + /// Maximum altitude in metres above the ellipsoid before the run is failed. Applied by dr, + /// cl and pf #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_ALT_MAX_M)] health_alt_max_m: f64, - /// Max velocity vector magnitude in m/s before the run is failed + /// Max velocity vector magnitude in m/s before the run is failed. Applied by dr, cl and pf; + /// an unaided dr arc on MEMS data can exceed the default, so raise it for long runs #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_SPEED_MPS_MAX)] health_speed_mps_max: f64, - /// Largest variance tolerated on the covariance diagonal before the run is failed + /// Largest variance tolerated on the covariance diagonal before the run is failed. Applied + /// by cl and pf; dr carries no covariance #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_COV_DIAG_MAX)] health_cov_diag_max: f64, - /// NIS above which a measurement update counts as an outlier + /// NIS above which a measurement update counts as an outlier. Applied by cl only: dr makes + /// no measurement update, and pf computes no NIS #[arg(long, default_value_t = strapdown::sim::DEFAULT_NIS_POS_MAX)] nis_pos_max: f64, - /// Consecutive NIS exceedances that fail the run + /// Consecutive NIS exceedances that fail the run. Applied by cl only, as --nis-pos-max #[arg(long, default_value_t = strapdown::sim::DEFAULT_NIS_POS_CONSEC_FAIL)] nis_pos_consec_fail: usize, @@ -490,7 +506,8 @@ struct ClosedLoopSimArgs { #[command(flatten)] sim: SimArgs, - /// Filter type to use for closed-loop navigation (default: the 15-state ESKF) + /// Filter type for closed-loop navigation. Default: the error-state Kalman filter (ESKF): + /// 15 error states, plus a barometric bias state unless --no-estimate-baro-bias #[arg(long, value_enum, default_value_t = FilterType::default())] filter: FilterType, @@ -518,7 +535,7 @@ struct ClosedLoopSimArgs { #[arg(long, default_value_t = 42)] seed: u64, - /// Estimate a barometric altitude bias as an extra filter state. + /// Do not estimate a barometric altitude bias; by default the filter carries one as an extra state. /// /// A barometer's reference pressure drifts and a filter that models the reading as /// unbiased pushes that drift into altitude. On the reference recording this takes @@ -633,13 +650,17 @@ struct ParticleFilterSimArgs { #[command(flatten)] geo_bias: GeophysicalBiasArgs, - /// Random walk on the sampled horizontal position error as `north,east` in m/sqrt(s). - /// Default: 1,1, as the recipes under `conf/` use. Canciani & Raquet's eq. 19 is `0,0`, - /// which diverges with GNSS-rate fixes on MEMS data. + /// Random walk on the sampled horizontal position error, north then east, in m/sqrt(s). + /// Written `0.5,0.5` or `0.5 0.5`. Default: 1,1, as the recipes under `conf/` use. Canciani + /// & Raquet's eq. 19 is `0,0`, which diverges with GNSS-rate fixes on MEMS data. + // + // `num_args = 1..=2`, not `2`: clap counts arguments before splitting on the delimiter, so + // with `2` the comma form arrived as one argument and was refused for want of a second. + // Exactly two values is enforced by `horizontal_process_noise_from` either way. #[arg( long, value_delimiter = ',', - num_args = 2, + num_args = 1..=2, default_values_t = [strapdown::sim::DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M; 2] )] horizontal_process_noise_std_m: Vec, @@ -784,19 +805,6 @@ fn horizontal_process_noise_from(values: &[f64]) -> Result, Box ExecutionLimits { ExecutionLimits { max_wall_clock_ratio: args.max_wall_clock_ratio, @@ -842,6 +850,149 @@ impl RunLimits { } } +/// Run `job` on every input file and fail if any of them failed. +/// +/// The one batch policy every mode shares, the configuration-file path and each subcommand +/// alike. A single input returns its own error unchanged. With several, a failing file is +/// logged and the rest still run -- one unusable recording must not abandon a batch -- and the +/// run then fails naming how many did not complete, so a script sees a non-zero exit. The +/// subcommands used to log a filter failure and exit 0, which only the configuration path did +/// not do. +/// +/// `parallel` runs the files on rayon's pool. It is what `--parallel` and `parallel = true` +/// mean, and it used to be read by the configuration path alone. +/// +/// # Errors +/// The single input's own error, or a count of the inputs that failed. +fn run_batch(csv_files: &[PathBuf], parallel: bool, job: F) -> Result<(), Box> +where + F: Fn(&Path) -> Result<(), Box> + Sync, +{ + if let [only] = csv_files { + return job(only); + } + + info!("Processing {} CSV files from directory", csv_files.len()); + let run_one = |input_file: &PathBuf| { + job(input_file).map_err(|e| { + error!("Error processing {}: {e}", input_file.display()); + (input_file.clone(), e.to_string()) + }) + }; + let failures: Vec<(PathBuf, String)> = if parallel { + info!("Running in parallel mode"); + csv_files + .par_iter() + .filter_map(|input_file| run_one(input_file).err()) + .collect() + } else { + csv_files + .iter() + .filter_map(|input_file| run_one(input_file).err()) + .collect() + }; + + if failures.is_empty() { + return Ok(()); + } + error!( + "{} of {} file(s) failed to process", + failures.len(), + csv_files.len() + ); + for (file, err) in &failures { + error!(" {}: {err}", file.display()); + } + Err(format!( + "{} of {} file(s) failed to process", + failures.len(), + csv_files.len() + ) + .into()) +} + +/// Write a performance plot beside `output_file` when `requested`. +/// +/// A failed plot is logged rather than returned: the run it describes has already completed +/// and been written, and discarding that over a drawing error would be the wrong trade. +#[cfg(feature = "plotting")] +fn plot_if_requested( + requested: bool, + results: &[NavigationResult], + records: &[TestDataRecord], + output_file: &Path, +) { + if !requested { + return; + } + let plot_path = output_file.with_extension("png"); + info!("Generating performance plot at {}", plot_path.display()); + match plotting::plot_performance(results, records, &plot_path) { + Ok(()) => info!("Performance plot generated successfully"), + Err(e) => error!("Failed to generate performance plot: {e}"), + } +} + +/// Without the `plotting` feature there is nothing to draw with. `--plot` is refused up front +/// (see [`refuse_inapplicable_global_flags`]); this is reached only by `generate_plot = true` +/// in a scenario file, which every `conf/` recipe sets, so it is logged rather than fatal. +#[cfg(not(feature = "plotting"))] +fn plot_if_requested( + requested: bool, + _results: &[NavigationResult], + _records: &[TestDataRecord], + _output_file: &Path, +) { + if requested { + error!( + "Plotting requested but 'plotting' feature not enabled. Rebuild with --features plotting" + ); + } +} + +/// Refuse a global `--plot` or `--parallel` that the chosen command cannot honour. +/// +/// Both flags are `global`, so clap accepts them after any subcommand. They used to be read on +/// the configuration-file path only, and every subcommand ignored them without a word. `dr`, +/// `cl` and `pf` honour both now; the commands below cannot, and say so rather than ignore them. +/// +/// # Errors +/// When `--plot` is given to a build without the `plotting` feature, or either flag is given to +/// `syn`, `config` or `ol`. +fn refuse_inapplicable_global_flags( + command: Option<&Command>, + plot: bool, + parallel: bool, +) -> Result<(), Box> { + if plot && !cfg!(feature = "plotting") { + return Err( + "--plot needs the `plotting` feature, which this build does not have: \ + rebuild with `--features plotting`" + .into(), + ); + } + let refusing = match command { + Some(Command::Synthetic(_)) => Some(( + "syn", + "it writes one trajectory file, with no navigation solution to plot", + )), + Some(Command::CreateConfig) => Some(( + "config", + "it writes a configuration file; set `parallel` and `generate_plot` in it instead", + )), + Some(Command::OpenLoop(_)) => Some(("ol", "it is not implemented and writes nothing")), + _ => None, + }; + if let Some((name, reason)) = refusing { + for (flag, given) in [("--plot", plot), ("--parallel", parallel)] { + if given { + return Err(format!("{flag} does not apply to `{name}`: {reason}").into()); + } + } + } + Ok(()) +} + /// Process a single CSV file with the given configuration fn process_file( input_file: &Path, @@ -857,14 +1008,15 @@ fn process_file( // Execute based on mode match config.mode { SimulationMode::DeadReckoning => { - info!("Running dead reckoning simulation"); - let results = dead_reckoning(&records, config.is_enu)?; - info!("Generated {} navigation results", results.len()); - let output_file = resolve_output_path(output, input_file, all_inputs)?; - NavigationResult::to_csv(&results, &output_file)?; - info!("Results written to {}", output_file.display()); - Ok(()) + run_dead_reckoning_file( + &records, + config.is_enu, + &config.health_limits, + &config.execution_limits, + &output_file, + config.generate_plot, + ) } SimulationMode::OpenLoop => { info!("Open-loop mode is not yet fully implemented"); @@ -891,7 +1043,7 @@ fn process_file( // Both now resolve to the same `GeoClosedLoopSettings` and the same runner. #[cfg(feature = "geonav")] if let Some(geo_config) = config.geophysical.as_ref() { - let settings = geo_settings_from_config(config, geo_config); + let settings = geo_settings_from_config(config, geo_config)?; settings.validate()?; let output_file = resolve_output_path(output, input_file, all_inputs)?; return run_geo_closed_loop_file(&settings, &records, input_file, &output_file); @@ -909,6 +1061,9 @@ fn process_file( } let filter_config = config.closed_loop.clone().unwrap_or_default(); + // The same validation the `--gate-*` flags get; a scenario file used to install a + // confidence of 1.5 or an inflation of 0.5 without a word. + let (innovation_gate, gate_recovery) = gating_from_config(&filter_config)?; // `ukf_alpha`/`beta`/`kappa` are read from the file rather than left at the // constructor's defaults. This path ignored all three, so a config file setting @@ -944,8 +1099,8 @@ fn process_file( FilterType::Ukf => { let mut ukf = initialize_ukf(&records[0].clone(), ukf_config)?; info!("Initialized UKF"); - ukf.set_innovation_gate(filter_config.innovation_gate); - ukf.set_gate_recovery(filter_config.gate_recovery); + ukf.set_innovation_gate(innovation_gate); + ukf.set_gate_recovery(gate_recovery); run_closed_loop( &mut ukf, event_stream, @@ -956,8 +1111,8 @@ fn process_file( FilterType::Ekf => { let mut ekf = initialize_ekf(&records[0].clone(), ekf_config)?; info!("Initialized EKF"); - ekf.set_innovation_gate(filter_config.innovation_gate); - ekf.set_gate_recovery(filter_config.gate_recovery); + ekf.set_innovation_gate(innovation_gate); + ekf.set_gate_recovery(gate_recovery); run_closed_loop( &mut ekf, event_stream, @@ -968,8 +1123,8 @@ fn process_file( FilterType::Eskf => { let mut eskf = initialize_eskf(&records[0].clone(), eskf_config)?; info!("Initialized ESKF"); - eskf.set_innovation_gate(filter_config.innovation_gate); - eskf.set_gate_recovery(filter_config.gate_recovery); + eskf.set_innovation_gate(innovation_gate); + eskf.set_gate_recovery(gate_recovery); run_closed_loop( &mut eskf, event_stream, @@ -984,31 +1139,7 @@ fn process_file( Ok(ref nav_results) => { NavigationResult::to_csv(nav_results, &output_file)?; info!("Results written to {}", output_file.display()); - - // Generate performance plot if requested - #[cfg(feature = "plotting")] - if config.generate_plot { - let plot_path = output_file.with_extension("png"); - info!("Generating performance plot at {}", plot_path.display()); - - match plotting::plot_performance(nav_results, &records, &plot_path) { - Ok(()) => { - info!("Performance plot generated successfully"); - } - Err(e) => { - error!("Failed to generate performance plot: {e}"); - // Don't fail the entire process if plotting fails - } - } - } - - #[cfg(not(feature = "plotting"))] - if config.generate_plot { - error!( - "Plotting requested but 'plotting' feature not enabled. Rebuild with --features plotting" - ); - } - + plot_if_requested(config.generate_plot, nav_results, &records, &output_file); Ok(()) } Err(e) => { @@ -1225,28 +1356,7 @@ fn process_file( let output_file = resolve_output_path(output, input_file, all_inputs)?; NavigationResult::to_csv(&results, &output_file)?; info!("Results written to {}", output_file.display()); - - #[cfg(feature = "plotting")] - if config.generate_plot { - let plot_path = output_file.with_extension("png"); - info!("Generating performance plot at {}", plot_path.display()); - - match plotting::plot_performance(&results, &records, &plot_path) { - Ok(()) => { - info!("Performance plot generated successfully"); - } - Err(e) => { - error!("Failed to generate performance plot: {e}"); - } - } - } - - #[cfg(not(feature = "plotting"))] - if config.generate_plot { - error!( - "Plotting requested but 'plotting' feature not enabled. Rebuild with --features plotting" - ); - } + plot_if_requested(config.generate_plot, &results, &records, &output_file); Ok(()) } @@ -1281,15 +1391,18 @@ fn run_from_config( // Synthetic mode has no input file — handle it separately before path validation if matches!(config.mode, SimulationMode::Synthetic) { + if cli_plot || cli_parallel { + return Err( + "--plot and --parallel do not apply to a synthetic configuration: it \ + writes one trajectory file, with no navigation solution to plot" + .into(), + ); + } let syn_config = config .synthetic .ok_or("mode is 'synthetic' but no [synthetic] section found in config file")?; let output = Path::new(&syn_config.output); - if let Some(parent) = output.parent() - && !parent.as_os_str().is_empty() - { - validate_output_path(parent)?; - } + validate_synthetic_output_path(output)?; let mut rng = StdRng::seed_from_u64(syn_config.seed); let (truth, sensors) = generate_synthetic(&syn_config, &mut rng)?; let n = truth.len(); @@ -1307,6 +1420,15 @@ fn run_from_config( return Ok(()); } + // Refused before any file is read, as the `--gate-*` flags are. + if matches!(config.mode, SimulationMode::ClosedLoop) { + let (innovation_gate, gate_recovery) = + gating_from_config(&config.closed_loop.clone().unwrap_or_default())?; + if let Some(gate) = innovation_gate { + info!("Innovation gating enabled: {gate:?}, recovery {gate_recovery:?}"); + } + } + info!("Input: {}", config.input); info!("Output: {}", config.output); info!("Parallel: {}", config.parallel); @@ -1324,68 +1446,10 @@ fn run_from_config( validate_input_path(input)?; validate_output_path(output)?; - // Get all CSV files to process let csv_files = get_csv_files(input)?; - let is_multiple = csv_files.len() > 1; - - if is_multiple { - info!("Processing {} CSV files from directory", csv_files.len()); - if config.parallel { - info!("Running in parallel mode"); - } - } - - // Process files either sequentially or in parallel - if config.parallel && is_multiple { - // Parallel processing - let errors = Mutex::new(Vec::new()); - - csv_files.par_iter().for_each(|input_file| { - match process_file(input_file, output, &csv_files, &config) { - Ok(()) => {} - Err(e) => { - error!("Error processing {}: {}", input_file.display(), e); - // Recover from a poisoned lock rather than panicking. This mutex - // guards the list of per-file failures; if another worker panicked - // while holding it, turning that into a second panic here loses the - // very error report this block exists to produce. - errors - .lock() - .unwrap_or_else(std::sync::PoisonError::into_inner) - .push((input_file.clone(), e.to_string())); - } - } - }); - - let errors = errors - .into_inner() - .unwrap_or_else(std::sync::PoisonError::into_inner); - if !errors.is_empty() { - error!("{} file(s) failed to process", errors.len()); - for (file, err) in &errors { - error!(" {}: {}", file.display(), err); - } - return Err(format!("{} file(s) failed to process", errors.len()).into()); - } - } else { - // Sequential processing - let mut failures = 0usize; - for input_file in &csv_files { - if let Err(e) = process_file(input_file, output, &csv_files, &config) { - if !is_multiple { - return Err(e); - } - failures += 1; - error!("Error processing {}: {}", input_file.display(), e); - } - } - if failures > 0 { - error!("{failures} file(s) failed to process"); - return Err(format!("{failures} file(s) failed to process").into()); - } - } - - Ok(()) + run_batch(&csv_files, config.parallel, |input_file| { + process_file(input_file, output, &csv_files, &config) + }) } /// The settings every closed-loop Kalman filter is built from, besides its first record and @@ -1469,19 +1533,17 @@ impl KalmanSettings { /// Execute a single closed-loop simulation run /// /// This is a helper function that extracts the common logic for running closed-loop simulations -/// with either UKF or EKF filters. It handles event stream creation, filter initialization, -/// simulation execution, and results writing. -#[allow(clippy::too_many_arguments)] +/// with any of the three Kalman filters. It handles event stream creation, filter +/// initialization and simulation execution; the caller writes the results. fn run_single_closed_loop_simulation( filter_type: FilterType, records: &[TestDataRecord], aiding: &strapdown::messages::AidingConfig, - output_file: &Path, limits: RunLimits, kalman: KalmanSettings, innovation_gate: Option, gate_recovery: GateRecovery, -) -> Result<(), Box> { +) -> Result, Box> { // Same full-window guard as the other entry points: the `initialize_*` helpers below see // only one record, which is not enough evidence in either direction (#296). check_declared_frame(records, kalman.is_enu)?; @@ -1503,6 +1565,9 @@ fn run_single_closed_loop_simulation( }; built }; + // Logged here, after the index is derived: the caller used to log the aiding config it + // passed in, so every `cl` log reported `baro_bias_index: None` whatever the filter used. + info!("Using aiding config: {aiding:?}"); // Build event stream from records and the aiding config let event_stream = build_event_stream(records, &aiding, kalman.is_enu)?; @@ -1551,29 +1616,17 @@ fn run_single_closed_loop_simulation( } }; - // Write results to CSV - match results { - Ok(ref nav_results) => { - NavigationResult::to_csv(nav_results, output_file)?; - info!("Results written to {}", output_file.display()); - Ok(()) - } - Err(e) => { - error!("Error running closed-loop simulation: {e}"); - Err(e.into()) - } - } + results.map_err(|e| { + error!("Error running closed-loop simulation: {e}"); + e.into() + }) } /// Execute synthetic trajectory generation fn run_synthetic(args: &SyntheticArgs) -> Result<(), Box> { use strapdown::sim::SyntheticInitialState; - if let Some(parent) = args.output.parent() - && !parent.as_os_str().is_empty() - { - validate_output_path(parent)?; - } + validate_synthetic_output_path(&args.output)?; let config = { let mut built = SyntheticConfig::default(); @@ -1627,58 +1680,54 @@ fn run_synthetic(args: &SyntheticArgs) -> Result<(), Box> { } /// Execute dead-reckoning simulation -fn run_dead_reckoning(args: &SimArgs) -> Result<(), Box> { +fn run_dead_reckoning(args: &SimArgs, parallel: bool, plot: bool) -> Result<(), Box> { validate_input_path(&args.input)?; validate_output_path(&args.output)?; info!("Running in dead reckoning mode"); - // Get all CSV files to process let csv_files = get_csv_files(&args.input)?; - let is_multiple = csv_files.len() > 1; - - if is_multiple { - info!("Processing {} CSV files from directory", csv_files.len()); - } - - // Process each CSV file - let mut failures = 0usize; - for input_file in &csv_files { + let limits = RunLimits::from_args(args); + run_batch(&csv_files, parallel, |input_file| { info!("Processing file: {}", input_file.display()); - - // One unusable file must not abandon the rest of a batch. Before #311 this loop - // could not fail here at all -- `from_csv` returned `Ok(vec![])` and the run wrote an - // empty output file -- so aborting would trade a silent wrong answer for a loud - // incomplete one. `run_from_config` already counts per-file failures and continues; - // this matches it. - let records = match load_records(input_file) { - Ok(records) => records, - Err(e) if is_multiple => { - error!("Skipping {}: {e}", input_file.display()); - failures += 1; - continue; - } - Err(e) => return Err(e), - }; - - // Run dead reckoning simulation - info!( - "Running dead reckoning simulation on {} records", - records.len() - ); - let results = dead_reckoning(&records, args.enu)?; - info!("Generated {} navigation results", results.len()); - - // Write results to CSV + let records = load_records(input_file)?; let output_file = resolve_output_path(&args.output, input_file, &csv_files)?; - NavigationResult::to_csv(&results, &output_file)?; - info!("Results written to {}", output_file.display()); - } - - if failures > 0 { - error!("{failures} file(s) skipped because they held no usable records"); - } + run_dead_reckoning_file( + &records, + args.enu, + &limits.health, + &limits.execution, + &output_file, + plot, + ) + }) +} +/// Dead-reckon one file's records and write the result. +/// +/// Shared by `dr` and a `mode = "dead-reckoning"` scenario file. Both apply the wall-clock +/// budgets and the position and speed health bounds; the covariance and NIS limits have +/// nothing to test without a filter. They used to apply none of them. +/// +/// # Errors +/// From [`dead_reckoning_with_limits`], or when the result cannot be written. +fn run_dead_reckoning_file( + records: &[TestDataRecord], + is_enu: bool, + health: &HealthLimits, + execution: &ExecutionLimits, + output_file: &Path, + plot: bool, +) -> Result<(), Box> { + info!( + "Running dead reckoning simulation on {} records", + records.len() + ); + let results = dead_reckoning_with_limits(records, is_enu, Some(health), Some(execution))?; + info!("Generated {} navigation results", results.len()); + NavigationResult::to_csv(&results, output_file)?; + info!("Results written to {}", output_file.display()); + plot_if_requested(plot, &results, records, output_file); Ok(()) } @@ -1709,6 +1758,40 @@ fn run_open_loop(args: &SimArgs) -> Result<(), Box> { Ok(()) } +/// Check an innovation gate and its recovery policy against the ranges their constructors +/// enforce, and return them rebuilt through those constructors. +/// +/// The one validator for both ways of configuring a gate. The command line built its gate +/// through [`InnovationGate::chi_squared`] and [`GateRecovery::new`] and so had their checks; +/// a scenario file's `[closed_loop]` section is deserialized, which bypasses both, so a +/// confidence of 1.5 or an inflation of 0.5 ran without a word. `source` names where the values +/// came from, for the error message. +/// +/// # Errors +/// [`StrapdownError::OutOfRange`](strapdown::StrapdownError::OutOfRange) or +/// [`StrapdownError::InvalidConfiguration`](strapdown::StrapdownError::InvalidConfiguration) +/// from the two constructors, prefixed with `source`. +fn validated_gating( + innovation_gate: Option, + gate_recovery: GateRecovery, + source: &str, +) -> Result<(Option, GateRecovery), Box> { + let in_source = |e: strapdown::StrapdownError| format!("{source}: {e}"); + let innovation_gate = innovation_gate + .map(|gate| match gate { + InnovationGate::ChiSquared { confidence } => InnovationGate::chi_squared(confidence), + InnovationGate::Fixed { threshold } => InnovationGate::fixed(threshold), + }) + .transpose() + .map_err(in_source)?; + let gate_recovery = GateRecovery::new( + gate_recovery.rejection_inflation, + gate_recovery.forced_update_after, + ) + .map_err(in_source)?; + Ok((innovation_gate, gate_recovery)) +} + /// Build the innovation gate and its recovery policy from the closed-loop CLI arguments. /// /// Shared by the plain and geophysical closed-loop paths so that `--gate-confidence` and its @@ -1721,19 +1804,18 @@ fn run_open_loop(args: &SimArgs) -> Result<(), Box> { /// impossible factor is a mistake worth naming whether or not this run gates. /// /// # Errors -/// [`StrapdownError::OutOfRange`](strapdown::StrapdownError::OutOfRange) or -/// [`StrapdownError::InvalidConfiguration`](strapdown::StrapdownError::InvalidConfiguration) -/// from the two constructors. +/// From [`validated_gating`]. fn gating_from_args( args: &ClosedLoopSimArgs, ) -> Result<(Option, GateRecovery), Box> { - let innovation_gate = args - .gate_confidence - .map(InnovationGate::chi_squared) - .transpose()?; - let gate_recovery = GateRecovery::new( - args.gate_inflation, - (args.gate_force_after > 0).then_some(args.gate_force_after), + let (innovation_gate, gate_recovery) = validated_gating( + args.gate_confidence + .map(|confidence| InnovationGate::ChiSquared { confidence }), + GateRecovery { + rejection_inflation: args.gate_inflation, + forced_update_after: (args.gate_force_after > 0).then_some(args.gate_force_after), + }, + "the --gate-* flags", )?; if let Some(gate) = innovation_gate { info!("Innovation gating enabled: {gate:?}, recovery {gate_recovery:?}"); @@ -1741,12 +1823,36 @@ fn gating_from_args( Ok((innovation_gate, gate_recovery)) } +/// The innovation gate and recovery policy a scenario file's `[closed_loop]` section names, +/// validated exactly as [`gating_from_args`] validates the command line's. +/// +/// `forced_update_after = 0` means "never force", as `--gate-force-after 0` does: TOML has no +/// null, so zero is the only way a TOML file can say it. +/// +/// # Errors +/// From [`validated_gating`]. +fn gating_from_config( + config: &ClosedLoopConfig, +) -> Result<(Option, GateRecovery), Box> { + let mut recovery = config.gate_recovery; + recovery.forced_update_after = recovery.forced_update_after.filter(|&after| after > 0); + validated_gating( + config.innovation_gate, + recovery, + "[closed_loop] innovation_gate/gate_recovery", + ) +} + /// Execute closed-loop simulation -fn run_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { +fn run_closed_loop_cli( + args: &ClosedLoopSimArgs, + parallel: bool, + plot: bool, +) -> Result<(), Box> { // Check if geophysical navigation is enabled #[cfg(feature = "geonav")] if args.geo.geo { - return run_geo_closed_loop_cli(args); + return run_geo_closed_loop_cli(args, parallel, plot); } validate_input_path(&args.sim.input)?; @@ -1763,88 +1869,37 @@ fn run_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { // be reported once, up front, not after the first file has already been written. let (innovation_gate, gate_recovery) = gating_from_args(args)?; - // Get all CSV files to process let csv_files = get_csv_files(&args.sim.input)?; - let is_multiple = csv_files.len() > 1; let limits = RunLimits::from_args(&args.sim); - if is_multiple { - info!("Processing {} CSV files from directory", csv_files.len()); - //println!("Processing {} CSV files from directory", csv_files.len()); - } + // The barometer and magnetometer schedules have no CLI flag; they take their 1 Hz + // default, overridable from a config file through serde. + let aiding = { + let mut built = strapdown::messages::AidingConfig::default(); + built.scheduler = build_scheduler(&args.scheduler); + built.fault = build_fault(&args.fault); + built.seed = args.seed; + built + }; - // Process each CSV file - let mut failures = 0usize; - for input_file in &csv_files { + run_batch(&csv_files, parallel, |input_file| { info!("Processing file: {}", input_file.display()); - - // One unusable file must not abandon the rest of a batch. Before #311 this loop - // could not fail here at all -- `from_csv` returned `Ok(vec![])` and the run wrote an - // empty output file -- so aborting would trade a silent wrong answer for a loud - // incomplete one. `run_from_config` already counts per-file failures and continues; - // this matches it. - let records = match load_records(input_file) { - Ok(records) => records, - Err(e) if is_multiple => { - error!("Skipping {}: {e}", input_file.display()); - failures += 1; - continue; - } - Err(e) => return Err(e), - }; - - // Build the aiding config from CLI args - let aiding = { - // The barometer and magnetometer schedules have no CLI flag; they take their - // 1 Hz default, overridable from a config file through serde. - let mut built = strapdown::messages::AidingConfig::default(); - built.scheduler = build_scheduler(&args.scheduler); - built.fault = build_fault(&args.fault); - built.seed = args.seed; - built - }; - - info!("Using aiding config: {aiding:?}"); + let records = load_records(input_file)?; let output_file = resolve_output_path(&args.sim.output, input_file, &csv_files)?; - - // Run simulation using the common helper function - match run_single_closed_loop_simulation( + let results = run_single_closed_loop_simulation( args.filter, &records, &aiding, - &output_file, limits.clone(), KalmanSettings::from_args(args), innovation_gate, gate_recovery, - ) { - Ok(()) => { - // Success - result logging is handled by the helper function - } - Err(e) => { - error!( - "Error running closed-loop simulation on {}: {}", - input_file.display(), - e - ); - if !is_multiple { - return Err(e); - } - // For multiple files, continue processing remaining files - error!( - "Error processing {}: {}. Continuing with remaining files...", - input_file.display(), - e - ); - } - } - } - - if failures > 0 { - error!("{failures} file(s) skipped because they held no usable records"); - } - - Ok(()) + )?; + NavigationResult::to_csv(&results, &output_file)?; + info!("Results written to {}", output_file.display()); + plot_if_requested(plot, &results, &records, &output_file); + Ok(()) + }) } // ============================================================================ @@ -2042,6 +2097,7 @@ impl GeoClosedLoopSettings { #[cfg(feature = "geonav")] fn geo_settings_from_args( args: &ClosedLoopSimArgs, + generate_plot: bool, ) -> Result> { let (innovation_gate, gate_recovery) = gating_from_args(args)?; let limits = RunLimits::from_args(&args.sim); @@ -2075,21 +2131,26 @@ fn geo_settings_from_args( execution: limits.execution, innovation_gate, gate_recovery, - generate_plot: false, + generate_plot, }) } /// Build the same settings from a scenario file. /// -/// Infallible, unlike [`geo_settings_from_args`]: a scenario file's gate is already a parsed -/// [`InnovationGate`], while the command line takes a confidence that has to be converted. -/// What a file can get wrong is caught by [`GeoClosedLoopSettings::validate`] instead. +/// The gate goes through [`gating_from_config`], the validator the command line's flags share: +/// a scenario file's gate is parsed but not range-checked by serde, and this used to install +/// whatever the file said. What else a file can get wrong is caught by +/// [`GeoClosedLoopSettings::validate`]. +/// +/// # Errors +/// From [`gating_from_config`]. #[cfg(feature = "geonav")] fn geo_settings_from_config( config: &SimulationConfig, geo: &GeophysicalConfig, -) -> GeoClosedLoopSettings { +) -> Result> { let filter_config = config.closed_loop.clone().unwrap_or_default(); + let (innovation_gate, gate_recovery) = gating_from_config(&filter_config)?; // Cleared here and derived by the runner from the filter it builds, as the unaided arm of // `process_file` derives it: the index depends on how many map biases precede the @@ -2097,7 +2158,7 @@ fn geo_settings_from_config( let mut aiding = config.aiding.clone(); aiding.baro_bias_index = None; - GeoClosedLoopSettings { + Ok(GeoClosedLoopSettings { filter: filter_config.filter, kalman: KalmanSettings::from_closed_loop(&filter_config, config.is_enu), gravity_resolution: geo.gravity_resolution, @@ -2116,10 +2177,10 @@ fn geo_settings_from_config( aiding, health: config.health_limits.clone(), execution: config.execution_limits.clone(), - innovation_gate: filter_config.innovation_gate, - gate_recovery: filter_config.gate_recovery, + innovation_gate, + gate_recovery, generate_plot: config.generate_plot, - } + }) } /// One map channel's bias prior, as configured and before its defaults are resolved. @@ -2442,60 +2503,24 @@ fn geo_bias_setup(gravity: Option, magnetic: Option) /// Resolves `--geo`'s flags into [`GeoClosedLoopSettings`] and hands each input file to /// [`run_geo_closed_loop_file`], which is the same entry point a `--config` run uses. #[cfg(feature = "geonav")] -fn run_geo_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { +fn run_geo_closed_loop_cli( + args: &ClosedLoopSimArgs, + parallel: bool, + plot: bool, +) -> Result<(), Box> { validate_input_path(&args.sim.input)?; validate_output_path(&args.sim.output)?; - let settings = geo_settings_from_args(args)?; + let settings = geo_settings_from_args(args, plot)?; settings.validate()?; - // Get all CSV files to process let csv_files = get_csv_files(&args.sim.input)?; - let is_multiple = csv_files.len() > 1; - - if is_multiple { - info!("Processing {} CSV files from directory", csv_files.len()); - } - - // Process each CSV file - let mut failures = 0usize; - for input_file in &csv_files { + run_batch(&csv_files, parallel, |input_file| { info!("Processing file: {}", input_file.display()); - - // One unusable file must not abandon the rest of a batch. Before #311 this loop - // could not fail here at all -- `from_csv` returned `Ok(vec![])` and the run wrote an - // empty output file -- so aborting would trade a silent wrong answer for a loud - // incomplete one. `run_from_config` already counts per-file failures and continues; - // this matches it. - let records = match load_records(input_file) { - Ok(records) => records, - Err(e) if is_multiple => { - error!("Skipping {}: {e}", input_file.display()); - failures += 1; - continue; - } - Err(e) => return Err(e), - }; - + let records = load_records(input_file)?; let output_file = resolve_output_path(&args.sim.output, input_file, &csv_files)?; - if let Err(e) = run_geo_closed_loop_file(&settings, &records, input_file, &output_file) { - error!( - "Error running geophysical navigation on {}: {}", - input_file.display(), - e - ); - if !is_multiple { - return Err(e); - } - failures += 1; - } - } - - if failures > 0 { - error!("{failures} file(s) skipped or failed"); - } - - Ok(()) + run_geo_closed_loop_file(&settings, &records, input_file, &output_file) + }) } /// Run one file's geophysical closed loop, from settings that say nothing about where they @@ -2715,24 +2740,8 @@ fn run_geo_closed_loop_file( // Plotting reached the non-geophysical config path and never this one, so a // `generate_plot = true` in a geophysical config used to be accepted and ignored -- - // and every conf/*.toml sets it. The CLI path leaves it false, matching what `--geo` - // did before. - #[cfg(feature = "plotting")] - if settings.generate_plot { - let plot_path = output_file.with_extension("png"); - info!("Generating performance plot at {}", plot_path.display()); - match plotting::plot_performance(&nav_results, records, &plot_path) { - Ok(()) => info!("Performance plot generated successfully"), - // A missing plot is not a reason to discard a completed run. - Err(e) => error!("Failed to generate performance plot: {e}"), - } - } - #[cfg(not(feature = "plotting"))] - if settings.generate_plot { - error!( - "Plotting requested but 'plotting' feature not enabled. Rebuild with --features plotting" - ); - } + // and every conf/*.toml sets it. + plot_if_requested(settings.generate_plot, &nav_results, records, output_file); Ok(()) } @@ -2885,38 +2894,22 @@ fn run_rbpf_event_loop( } /// Execute particle filter simulation -fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box> { +fn run_particle_filter( + args: &ParticleFilterSimArgs, + parallel: bool, + plot: bool, +) -> Result<(), Box> { refuse_removed_particle_filter_flags(args)?; validate_input_path(&args.sim.input)?; validate_output_path(&args.sim.output)?; let csv_files = get_csv_files(&args.sim.input)?; - let is_multiple = csv_files.len() > 1; let execution_limits = execution_limits_from_args(&args.sim); let health_limits = health_limits_from_args(&args.sim); - if is_multiple { - info!("Processing {} CSV files from directory", csv_files.len()); - } - - let mut failures = 0usize; - for input_file in &csv_files { + run_batch(&csv_files, parallel, |input_file| { info!("Processing file: {}", input_file.display()); - - // One unusable file must not abandon the rest of a batch. Before #311 this loop - // could not fail here at all -- `from_csv` returned `Ok(vec![])` and the run wrote an - // empty output file -- so aborting would trade a silent wrong answer for a loud - // incomplete one. `run_from_config` already counts per-file failures and continues; - // this matches it. - let records = match load_records(input_file) { - Ok(records) => records, - Err(e) if is_multiple => { - error!("Skipping {}: {e}", input_file.display()); - failures += 1; - continue; - } - Err(e) => return Err(e), - }; + let records = load_records(input_file)?; let aiding = { // The barometer and magnetometer schedules have no CLI flag; they take their @@ -3004,8 +2997,8 @@ fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box Result<(), Box 0 { - error!("{failures} file(s) skipped because they held no usable records"); - } - Ok(()) } /// Prompt for simulation mode with validation +/// +/// Open loop is not offered: it is not implemented, and a configuration naming it fails the +/// moment it is run. Synthetic generation is, since its configuration needs only an output +/// file and a seed to be runnable. fn prompt_simulation_mode() -> SimulationMode { loop { println!( "Please specify the simulation mode you would like:\n\ [1] - Dead Reckoning\n\ - [2] - Open-Loop (Feed-Forward)\n\ - [3] - Closed-Loop (Feedback)\n\ - [4] - Particle Filter\n\ + [2] - Closed-Loop (Feedback)\n\ + [3] - Particle Filter\n\ + [4] - Synthetic trajectory generation\n\ [q] - Quit\n" ); if let Some(input) = read_user_input() { match input.as_str() { "1" => return SimulationMode::DeadReckoning, - "2" => return SimulationMode::OpenLoop, - "3" => return SimulationMode::ClosedLoop, - "4" => return SimulationMode::ParticleFilter, - _ => println!("Error: Invalid selection. Please enter 1, 2, 3, or q.\n"), + "2" => return SimulationMode::ClosedLoop, + "3" => return SimulationMode::ParticleFilter, + "4" => return SimulationMode::Synthetic, + _ => println!("Error: Invalid selection. Please enter 1, 2, 3, 4, or q.\n"), } } } } +/// Prompt for the CSV file a synthetic trajectory is written to. +fn prompt_synthetic_output_path() -> String { + loop { + let output = prompt_output_path(); + if Path::new(&output) + .extension() + .and_then(|extension| extension.to_str()) + .is_some_and(|extension| extension.eq_ignore_ascii_case("csv")) + { + return output; + } + println!( + "Error: a synthetic trajectory is written to one CSV file; the path must end in .csv.\n" + ); + } +} + +/// The scenario file the wizard writes for synthetic generation. +/// +/// Every `[synthetic]` key but the output and the seed takes the `syn` subcommand's default, +/// which is a stationary 300 s trajectory; the wizard tells the user to edit the section. +fn synthetic_wizard_config( + output: String, + seed: u64, + logging: strapdown::sim::LoggingConfig, +) -> SimulationConfig { + let mut synthetic = SyntheticConfig::default(); + synthetic.output.clone_from(&output); + synthetic.seed = seed; + let mut built = SimulationConfig::default(); + built.mode = SimulationMode::Synthetic; + built.output = output; + built.seed = seed; + built.logging = logging; + built.synthetic = Some(synthetic); + built +} + +/// Write the wizard's configuration to `save_path/config_name` and say how to run it. +fn write_wizard_config( + config: &SimulationConfig, + save_path: &str, + config_name: &str, +) -> Result<(), Box> { + let config_output_path = Path::new(save_path).join(config_name); + if let Some(parent) = config_output_path.parent() + && !parent.as_os_str().is_empty() + && !parent.exists() + { + std::fs::create_dir_all(parent)?; + } + config.to_file(&config_output_path)?; + + println!( + "\n✓ Configuration file successfully created: {}", + config_output_path.display() + ); + println!("\nYou can now run the simulation with:"); + println!(" strapdown-sim --config {}", config_output_path.display()); + Ok(()) +} + /// Prompt for filter type with validation fn prompt_filter_type() -> FilterType { loop { @@ -3697,9 +3753,26 @@ fn create_config_file() -> Result<(), Box> { let save_path = prompt_config_path(); println!("\nCreating configuration file at: {save_path}/{config_name}\n"); + let mode = prompt_simulation_mode(); + + if matches!(mode, SimulationMode::Synthetic) { + let output_path = prompt_synthetic_output_path(); + let seed = prompt_seed(); + println!("\n--- Logging Configuration ---"); + let logging = strapdown::sim::LoggingConfig { + level: prompt_log_level(), + file: prompt_log_file(), + }; + println!( + "\nThe trajectory takes the `syn` defaults. Edit the [synthetic] section of the \ + generated file to set its initial state, duration, rate and noise." + ); + let config = synthetic_wizard_config(output_path, seed, logging); + return write_wizard_config(&config, &save_path, &config_name); + } + let input_path = prompt_input_path(); let output_path = prompt_output_path(); - let mode = prompt_simulation_mode(); let seed = prompt_seed(); let is_enu = prompt_frame(); let parallel = prompt_parallel(); @@ -3834,24 +3907,7 @@ fn create_config_file() -> Result<(), Box> { built }; - // validate output location exists and write to file using appropriate format based on file extension - let config_output_path = Path::new(&save_path).join(&config_name); - if let Some(parent) = config_output_path.parent() - && !parent.as_os_str().is_empty() - && !parent.exists() - { - std::fs::create_dir_all(parent)?; - } - config.to_file(&config_output_path)?; - - println!( - "\n✓ Configuration file successfully created: {}", - config_output_path.display() - ); - println!("\nYou can now run the simulation with:"); - println!(" strapdown-sim --config {}", config_output_path.display()); - - Ok(()) + write_wizard_config(&config, &save_path, &config_name) } fn main() -> Result<(), Box> { @@ -3882,12 +3938,14 @@ fn main() -> Result<(), Box> { // Initialize logger with resolved settings init_logger(log_level, log_file)?; + refuse_inapplicable_global_flags(None, cli.plot, cli.parallel)?; return run_from_config(config_path, cli.parallel, cli.plot); } // Initialize logger with CLI settings for command-line mode init_logger(&cli.log_level, cli.log_file.as_ref())?; + refuse_inapplicable_global_flags(cli.command.as_ref(), cli.plot, cli.parallel)?; // Otherwise, execute based on subcommand match cli.command { @@ -3896,11 +3954,11 @@ fn main() -> Result<(), Box> { "Running in Dead Reckoning mode with input: {}", args.input.display() ); - run_dead_reckoning(&args) + run_dead_reckoning(&args, cli.parallel, cli.plot) } Some(Command::OpenLoop(args)) => run_open_loop(&args), - Some(Command::ClosedLoop(args)) => run_closed_loop_cli(&args), - Some(Command::ParticleFilter(args)) => run_particle_filter(&args), + Some(Command::ClosedLoop(args)) => run_closed_loop_cli(&args, cli.parallel, cli.plot), + Some(Command::ParticleFilter(args)) => run_particle_filter(&args, cli.parallel, cli.plot), Some(Command::CreateConfig) => create_config_file(), Some(Command::Synthetic(args)) => run_synthetic(&args), None => { @@ -3933,16 +3991,6 @@ mod tests { config } - #[test] - fn test_create_config_args_structure() { - let args = CreateConfigArgs { - output: PathBuf::from("test_config.toml"), - mode: SimulationMode::ClosedLoop, - }; - assert_eq!(args.output, PathBuf::from("test_config.toml")); - assert!(matches!(args.mode, SimulationMode::ClosedLoop)); - } - #[test] fn test_simulation_mode_variants() { let modes = [ @@ -4085,7 +4133,7 @@ mod tests { let Some(Command::ClosedLoop(args)) = cli.command else { panic!("expected the `cl` subcommand"); }; - let from_cli = geo_settings_from_args(&args).expect("CLI settings must resolve"); + let from_cli = geo_settings_from_args(&args, false).expect("CLI settings must resolve"); let toml = r#" input = "in.csv" @@ -4129,7 +4177,8 @@ r_scale = 5.0 .geophysical .as_ref() .expect("the fixture declares a [geophysical] section"); - let from_config = geo_settings_from_config(&config, geo); + let from_config = + geo_settings_from_config(&config, geo).expect("the config gate must validate"); // `generate_plot` is the one field the two are meant to disagree on: a config file // asks for a plot and the command line has no equivalent flag on this path. @@ -4154,7 +4203,8 @@ r_scale = 5.0 .geophysical .as_ref() .expect("a [geophysical] section"); - let settings = geo_settings_from_config(&config, geo); + let settings = + geo_settings_from_config(&config, geo).expect("the config gate must validate"); let error = settings .validate() @@ -4201,7 +4251,8 @@ magnetic_bias_init_std = 32000.0 .geophysical .as_ref() .expect("the fixture declares a [geophysical] section"); - let settings = geo_settings_from_config(&config, geo); + let settings = + geo_settings_from_config(&config, geo).expect("the config gate must validate"); let unaided = KalmanSettings::from_closed_loop(&config.closed_loop.clone().unwrap_or_default(), true); assert_eq!( @@ -4377,7 +4428,8 @@ magnetic_bias_process_noise_std = 3.0 // The Kalman arm, reading the same section through its own settings, must resolve the // identical priors: the two families are compared against each other, so they must // be told the same thing about the sensor. - let settings = geo_settings_from_config(&config, geo); + let settings = + geo_settings_from_config(&config, geo).expect("the config gate must validate"); let kalman = geo_bias_setup( Some(settings.gravity_bias_prior()), Some(settings.magnetic_bias_prior()), @@ -4550,7 +4602,8 @@ magnetic_bias_process_noise_std = 3.0 .geophysical .as_ref() .expect("a [geophysical] section"); - let settings = geo_settings_from_config(&config, geo); + let settings = + geo_settings_from_config(&config, geo).expect("the config gate must validate"); assert!( matches!(settings.filter, FilterType::Eskf), @@ -4566,4 +4619,226 @@ magnetic_bias_process_noise_std = 3.0 "the error should say that omitting `filter` lands here, got: {message}" ); } + + /// The `pf` flags a `--horizontal-process-noise-std-m` value parses into. + fn horizontal_noise_from_cli(values: &[&str]) -> Vec { + let mut argv = vec![ + "strapdown-sim", + "pf", + "-i", + "in.csv", + "-o", + "out.csv", + "--horizontal-process-noise-std-m", + ]; + argv.extend_from_slice(values); + let cli = Cli::try_parse_from(argv).expect("the flags above must parse"); + let Some(Command::ParticleFilter(args)) = cli.command else { + panic!("expected the `pf` subcommand"); + }; + args.horizontal_process_noise_std_m + } + + /// The comma form the help text documents. `num_args = 2` counted it as one argument and + /// refused it with "2 values required but 1 was provided". + #[test] + fn pf_horizontal_process_noise_parses_the_comma_form() { + let values = horizontal_noise_from_cli(&["0.5,0.25"]); + assert_eq!(values, vec![0.5, 0.25]); + assert_eq!( + horizontal_process_noise_from(&values).unwrap(), + Vector2::new(0.5, 0.25) + ); + } + + #[test] + fn pf_horizontal_process_noise_parses_the_space_form() { + let values = horizontal_noise_from_cli(&["0.5", "0.25"]); + assert_eq!(values, vec![0.5, 0.25]); + assert_eq!( + horizontal_process_noise_from(&values).unwrap(), + Vector2::new(0.5, 0.25) + ); + } + + /// One value parses, and is refused where the pair is checked, rather than guessed at. + #[test] + fn pf_horizontal_process_noise_refuses_a_single_value() { + let values = horizontal_noise_from_cli(&["0.5"]); + assert!(horizontal_process_noise_from(&values).is_err()); + } + + /// A `[closed_loop]` section with the values the `--gate-*` flags refuse. + fn closed_loop_config_with_gating(gate: &str, recovery: &str) -> ClosedLoopConfig { + let mut built = ClosedLoopConfig::default(); + built.innovation_gate = Some(InnovationGate::ChiSquared { + confidence: gate.parse().unwrap(), + }); + built.gate_recovery = GateRecovery { + rejection_inflation: recovery.parse().unwrap(), + forced_update_after: Some(DEFAULT_FORCED_UPDATE_AFTER), + }; + built + } + + /// A scenario file's gate goes through the validator the flags use. A confidence of 1.5 + /// and an inflation of 0.5 used to run with exit 0. + #[test] + fn config_file_gating_is_validated_like_the_flags() { + let bad_confidence = closed_loop_config_with_gating("1.5", "2.0"); + let error = gating_from_config(&bad_confidence).expect_err("confidence 1.5 is refused"); + assert!( + error.to_string().contains("innovation gate confidence"), + "got: {error}" + ); + + let bad_inflation = closed_loop_config_with_gating("0.999", "0.5"); + let error = gating_from_config(&bad_inflation).expect_err("inflation 0.5 is refused"); + assert!( + error.to_string().contains("gate rejection inflation"), + "got: {error}" + ); + + let good = closed_loop_config_with_gating("0.999", "2.0"); + let (gate, recovery) = gating_from_config(&good).unwrap(); + assert_eq!(gate, good.innovation_gate); + assert_eq!(recovery, good.gate_recovery); + + // Zero is "never force" in a file, as `--gate-force-after 0` is on the command line. + let mut never_force = good; + never_force.gate_recovery.forced_update_after = Some(0); + let (_, recovery) = gating_from_config(&never_force).unwrap(); + assert_eq!(recovery.forced_update_after, None); + + // And the same values on the command line fail the same way. + let cli = Cli::try_parse_from([ + "strapdown-sim", + "cl", + "-i", + "in.csv", + "-o", + "out.csv", + "--gate-confidence", + "1.5", + ]) + .unwrap(); + let Some(Command::ClosedLoop(args)) = cli.command else { + panic!("expected the `cl` subcommand"); + }; + let error = gating_from_args(&args).expect_err("confidence 1.5 is refused"); + assert!( + error.to_string().contains("innovation gate confidence"), + "got: {error}" + ); + } + + /// The whole config path refuses a bad gate before it reads any input. + #[test] + fn run_from_config_refuses_an_out_of_range_gate_up_front() { + let dir = tempfile::tempdir().unwrap(); + let path = dir.path().join("gate.toml"); + std::fs::write( + &path, + "mode = \"closed-loop\"\ninput = \"does-not-exist.csv\"\noutput = \"out.csv\"\n\n\ + [closed_loop]\ninnovation_gate = { chi_squared = { confidence = 1.5 } }\n", + ) + .unwrap(); + let error = run_from_config(&path, false, false).expect_err("confidence 1.5 is refused"); + assert!( + error.to_string().contains("innovation gate confidence"), + "the gate must be refused before the missing input is noticed, got: {error}" + ); + } + + /// Several inputs: every file runs, and one failure makes the batch fail, in either mode. + #[test] + fn run_batch_runs_every_file_and_fails_if_any_failed() { + use std::sync::atomic::{AtomicUsize, Ordering}; + + let files: Vec = ["a.csv", "b.csv", "c.csv"] + .iter() + .map(PathBuf::from) + .collect(); + for parallel in [false, true] { + let ran = AtomicUsize::new(0); + let result = run_batch(&files, parallel, |file| { + ran.fetch_add(1, Ordering::SeqCst); + if file == Path::new("b.csv") { + Err("filter diverged".into()) + } else { + Ok(()) + } + }); + assert_eq!(ran.load(Ordering::SeqCst), 3, "parallel = {parallel}"); + let error = result.expect_err("one failed file must fail the batch"); + assert!(error.to_string().contains("1 of 3"), "got: {error}"); + + assert!(run_batch(&files, parallel, |_| Ok(())).is_ok()); + } + } + + /// One input returns its own error rather than a count. + #[test] + fn run_batch_returns_a_single_files_own_error() { + let files = vec![PathBuf::from("only.csv")]; + let error = run_batch(&files, true, |_| Err("the real reason".into())).unwrap_err(); + assert_eq!(error.to_string(), "the real reason"); + } + + /// `--plot` and `--parallel` are honoured by `dr`, `cl` and `pf`, and refused where they + /// cannot apply instead of being ignored. + #[test] + fn global_flags_are_refused_where_they_cannot_apply() { + for argv in [ + vec!["strapdown-sim", "--parallel", "syn", "-o", "x.csv"], + vec!["strapdown-sim", "--parallel", "config"], + vec!["strapdown-sim", "--parallel", "ol", "-i", "in", "-o", "out"], + ] { + let cli = Cli::try_parse_from(&argv).unwrap(); + assert!( + refuse_inapplicable_global_flags(cli.command.as_ref(), cli.plot, cli.parallel) + .is_err(), + "{argv:?} must be refused" + ); + } + for argv in [ + vec!["strapdown-sim", "--parallel", "dr", "-i", "in", "-o", "out"], + vec!["strapdown-sim", "--parallel", "cl", "-i", "in", "-o", "out"], + vec!["strapdown-sim", "--parallel", "pf", "-i", "in", "-o", "out"], + vec!["strapdown-sim", "syn", "-o", "x.csv"], + ] { + let cli = Cli::try_parse_from(&argv).unwrap(); + assert!( + refuse_inapplicable_global_flags(cli.command.as_ref(), cli.plot, cli.parallel) + .is_ok(), + "{argv:?} must be accepted" + ); + } + let cli = Cli::try_parse_from(["strapdown-sim", "dr", "-i", "in", "-o", "out", "--plot"]) + .unwrap(); + assert_eq!( + refuse_inapplicable_global_flags(cli.command.as_ref(), cli.plot, cli.parallel).is_ok(), + cfg!(feature = "plotting"), + "--plot is accepted exactly when there is a plotting backend" + ); + } + + /// The wizard's synthetic configuration is a runnable one: it writes, reads back as + /// synthetic, and names its output in the `[synthetic]` section the runner reads. + #[test] + fn the_wizard_writes_a_runnable_synthetic_config() { + let dir = tempfile::tempdir().unwrap(); + let config = synthetic_wizard_config( + "trajectory.csv".to_string(), + 7, + strapdown::sim::LoggingConfig::default(), + ); + write_wizard_config(&config, &dir.path().to_string_lossy(), "syn.toml").unwrap(); + + let read = SimulationConfig::from_file(dir.path().join("syn.toml")).unwrap(); + assert!(matches!(read.mode, SimulationMode::Synthetic)); + let synthetic = read.synthetic.expect("a [synthetic] section"); + assert_eq!(synthetic.output, "trajectory.csv"); + assert_eq!(synthetic.seed, 7); + } } From 550fd4d0042dab2234e4017f015f919cf9251cac Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:06:53 -0400 Subject: [PATCH 09/26] Skip a UKF update whose sigma points cannot all be predicted The geophysical measurement models return NaN from get_expected_measurement for a point off the loaded map, because that method is infallible by signature. The UKF maps every sigma point through it, so one off-map sigma point made z_hat, the innovation covariance and the gain NaN, the update wrote NaN into the state, and the run stopped with NonFinite { what: "filter state" } -- even while the estimate itself was still on the map. The EKF and the particle filter meet the same edge as a recoverable OutOfMapBounds and skip the fix with a warning. UnscentedKalmanFilter::update now checks the predicted sigma-point measurements before anything else and, if any is non-finite, returns the recoverable MeasurementUnavailable with the state and covariance untouched. The runners already skip recoverable errors with the same "Measurement rejected" warning they log for the EKF, and still fail a run after 100 in a row. A test drives a model that cannot predict north of the mean's latitude and checks the update is refused unchanged, and that a model every sigma point can predict still updates. The geonav pages documented the abort as a known limitation; they now describe the skip. Co-Authored-By: Claude Opus 5.5 --- book/src/geonav/maps.md | 8 +-- book/src/geonav/overview.md | 8 +-- core/src/kalman.rs | 102 ++++++++++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+), 7 deletions(-) diff --git a/book/src/geonav/maps.md b/book/src/geonav/maps.md index a1cd3f5f..679d148d 100644 --- a/book/src/geonav/maps.md +++ b/book/src/geonav/maps.md @@ -62,9 +62,11 @@ What happens when the estimate leaves the grid depends on the filter: - **EKF and particle filter:** `OutOfMapBounds` is classed as recoverable, so the measurement is skipped with a warning naming the map bounds, and the run continues unaided by the map. - **UKF:** the predicted measurement is evaluated at each sigma point through a method that - cannot return an error, and an off-map point yields `NaN` there. In the current release that - `NaN` reaches the state and the run stops with `NonFinite { what: "filter state" }`, even when - only a sigma point, not the estimate itself, is off the map. + cannot return an error, and an off-map point yields `NaN` there. The UKF checks for that before + it touches the state: if any sigma point has no finite prediction, the update returns the + recoverable `MeasurementUnavailable`, and the measurement is skipped with a warning as on the + other filters. This happens a little sooner than on the EKF, since a sigma point can leave the + map while the estimate itself is still on it. Either way, size the map with room to spare around the track. diff --git a/book/src/geonav/overview.md b/book/src/geonav/overview.md index ba4037d2..32695e29 100644 --- a/book/src/geonav/overview.md +++ b/book/src/geonav/overview.md @@ -178,10 +178,10 @@ strapdown-sim pf -i synthetic.csv -o geo_pf.csv \ - **Experimental API.** `strapdown-geonav` is 0.1 and may change in any release. - **No ESKF.** Only the UKF, the EKF and the particle filter accept geophysical measurements. -- **Leaving the map.** With the EKF and the particle filter, a measurement whose estimate is - off the map tile is skipped with a warning, so a map that is too small quietly stops aiding. - With the UKF, an off-map sigma point currently stops the run with - `NonFinite { what: "filter state" }`. Pad the map well beyond the track either way; see +- **Leaving the map.** A measurement whose estimate is off the map tile is skipped with a + warning, so a map that is too small quietly stops aiding. The UKF skips one as soon as any of + its sigma points is off the map. A run that skips more than 100 measurements in a row fails + with `FilterDiverged`. Pad the map well beyond the track; see [Maps and Measurement Models](./maps.md#reading-a-value-bilinear-interpolation). - **The sensor matters more than the filter.** The gravity observation is the magnitude of the record's `grav_x/y/z` columns and the magnetic observation the magnitude of `mag_x/y/z`. diff --git a/core/src/kalman.rs b/core/src/kalman.rs index 36fe0716..a9546584 100644 --- a/core/src/kalman.rs +++ b/core/src/kalman.rs @@ -905,6 +905,28 @@ impl NavigationFilter for UnscentedKalmanFilter { measurement_sigma_points.set_column(i, &sigma_point); z_hat += self.weights_mean[i] * sigma_point; } + // A model that cannot predict a sigma point says so with a non-finite value: + // `get_expected_measurement` is infallible by signature, so the geophysical models + // return NaN for a point off the edge of their map. One NaN column makes `z_hat`, `s` + // and the gain NaN, and the update used to write that into the state, ending the run + // with `NonFinite { what: "filter state" }`. The EKF meets the same edge as a + // recoverable `OutOfMapBounds` from its Jacobian and skips the fix; this is that + // outcome for the UKF -- a recoverable error, raised before anything is changed. + let unusable = measurement_sigma_points + .column_iter() + .filter(|column| !column.iter().all(|value| value.is_finite())) + .count(); + if unusable > 0 { + return Err(StrapdownError::MeasurementUnavailable { + model: "UKF sigma-point prediction", + reason: format!( + "{unusable} of {} sigma points have no finite predicted measurement \ + (for a map-based model, they lie off the loaded map); the update is \ + skipped and the state left unchanged", + measurement_sigma_points.ncols() + ), + }); + } let mut s = DMatrix::::zeros(measurement.get_dimension(), measurement.get_dimension()); for (i, sigma_point) in measurement_sigma_points.column_iter().enumerate() { let diff = sigma_point - &z_hat; @@ -3000,6 +3022,86 @@ mod tests { assert!(!ukf.mean_state.is_empty()); } + /// A measurement model that cannot predict for states north of `latitude_limit_rad`, as a + /// geophysical map cannot predict off its edge: it returns NaN there. + #[derive(Clone, Debug)] + struct EdgeOfMapMeasurement { + latitude_limit_rad: f64, + } + + impl MeasurementModel for EdgeOfMapMeasurement { + fn as_any(&self) -> &dyn std::any::Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn std::any::Any { + self + } + fn get_dimension(&self) -> usize { + 1 + } + fn get_measurement(&self, _state: &DVector) -> Result, StrapdownError> { + Ok(DVector::from_element(1, 0.0)) + } + fn get_noise(&self) -> DMatrix { + DMatrix::from_element(1, 1, 1.0) + } + fn get_expected_measurement(&self, state: &DVector) -> DVector { + let value = if state[0] > self.latitude_limit_rad { + f64::NAN + } else { + state[2] + }; + DVector::from_element(1, value) + } + fn get_jacobian(&self, state: &DVector) -> Result, StrapdownError> { + let mut jacobian = DMatrix::zeros(1, state.len()); + jacobian[(0, 2)] = 1.0; + Ok(jacobian) + } + } + + /// Sigma points off the edge of a map used to put NaN into the state and end the run; + /// the UKF now skips the measurement as the EKF does, with a recoverable error and the + /// state and covariance untouched. + #[test] + fn ukf_skips_a_measurement_some_sigma_points_cannot_predict() { + let mut ukf = UnscentedKalmanFilter::new( + &UKF_PARAMS, + &IMU_BIASES, + None, + COVARIANCE_DIAGONAL.to_vec(), + DMatrix::from_diagonal(&DVector::from_vec(PROCESS_NOISE_DIAGONAL.to_vec())), + ALPHA, + BETA, + KAPPA, + ); + let state_before = ukf.mean_state.clone(); + let covariance_before = ukf.covariance.clone(); + // On the mean's own latitude: the mean is on the map, the sigma points spread north + // of it are not. + let measurement = EdgeOfMapMeasurement { + latitude_limit_rad: state_before[0], + }; + + let error = ukf + .update(&measurement) + .expect_err("an unpredictable sigma point must not reach the state"); + assert!(error.is_recoverable(), "got {error:?}"); + assert!( + matches!(error, StrapdownError::MeasurementUnavailable { .. }), + "got {error:?}" + ); + assert_eq!(ukf.mean_state, state_before); + assert_eq!(ukf.covariance, covariance_before); + + // And a model every sigma point can predict still updates. + let on_map = EdgeOfMapMeasurement { + latitude_limit_rad: f64::INFINITY, + }; + ukf.update(&on_map).unwrap(); + assert!(ukf.mean_state.iter().all(|value| value.is_finite())); + } + #[test] fn ukf_with_additional_states() { // Test UKF construction with additional states beyond 15 From e52ee92636b7f4d619d1078683e672a0bf14038b Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:10:13 -0400 Subject: [PATCH 10/26] Seed the GNSS faults from a config file's top-level seed A scenario file's top-level `seed` reached only the particle filter. The fault models were seeded by `[aiding] seed`, which defaulted to 42 on its own, so two closed-loop files differing only in `seed` gave byte-identical output, while on the command line `--seed` seeds both. AidingConfig::seed is now Option. None means "the run's seed": SimulationConfig::resolved_aiding fills it from the top-level seed, and strapdown-sim's config path builds every event stream from that, so the top-level seed alone seeds the whole run, as --seed does. An explicit `[aiding] seed` still wins, which keeps the documented Monte Carlo split -- vary the particle filter's seed with the faults held fixed, or the reverse -- available. Anything that builds an event stream from a None takes DEFAULT_AIDING_SEED, 42, so defaults and every shipped config (which set both seeds to 42) give the same output as before. The wizard leaves `[aiding] seed` unset, so editing one key changes the run. This is a breaking change to a pub field; call sites that set the seed now write Some(seed). Tests hold the resolution order and that two top-level seeds give two fault realizations. The configuration, scenario and architecture pages described the old behaviour and now describe this one. Co-Authored-By: Claude Opus 5.5 --- book/src/development/architecture.md | 3 +- book/src/gnss/scenarios.md | 6 +-- book/src/user-guide/configuration.md | 26 +++++----- core/src/messages.rs | 26 ++++++---- core/src/sim.rs | 19 ++++++- core/tests/example_configs.rs | 3 +- core/tests/perf_baseline.rs | 12 ++--- core/tests/ukf_conditioning.rs | 2 +- geonav/src/lib.rs | 12 ++--- sim/src/main.rs | 74 +++++++++++++++++++++++++--- 10 files changed, 134 insertions(+), 49 deletions(-) diff --git a/book/src/development/architecture.md b/book/src/development/architecture.md index 832d2c08..3111999e 100644 --- a/book/src/development/architecture.md +++ b/book/src/development/architecture.md @@ -145,7 +145,8 @@ A run is a function of its inputs and its seeds. There are no wall-clock or thre inputs to the navigation solution: - the GNSS fault model draws from a `StdRng` seeded from `AidingConfig::seed` (`[aiding] seed` - in a config file, `--seed` on the command line); + in a config file, falling back to the top-level `seed` through + `SimulationConfig::resolved_aiding`; `--seed` on the command line; 42 when nothing sets it); - the RBPF seeds its own `StdRng` from its configuration's seed; - `generate_synthetic` takes the seeded RNG it draws from. diff --git a/book/src/gnss/scenarios.md b/book/src/gnss/scenarios.md index f25f927d..eeeaf7c1 100644 --- a/book/src/gnss/scenarios.md +++ b/book/src/gnss/scenarios.md @@ -67,9 +67,9 @@ Three behaviours of the parser are worth knowing: `core/tests/example_configs.rs` guards the shipped files against this; your own files are not guarded, so check section names against this page. -**Seeds.** In a configuration file the fault model is seeded by `[aiding] seed`, not by the -top-level `seed`: two files differing only in the top-level `seed` produce identical -closed-loop output. On the command line, `--seed` is what seeds the fault model. +**Seeds.** In a configuration file the fault model is seeded by `[aiding] seed` when it is +given, and otherwise by the top-level `seed`, so two files differing only in the top-level +`seed` produce two fault realizations. On the command line, `--seed` seeds the fault model. **`baro_bias_index`** tells the barometer measurement which state of the filter holds a barometric bias. `strapdown-sim` overwrites it from the filter it builds (and clears it for the diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index 1ccfa8f2..6b8be26e 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -115,7 +115,7 @@ restructured, which are refused by name; see [below](#particle_filter). | `mode` | **required** | what to run; see the table below | | `input` | `"input.csv"` | input CSV file or directory, as for `-i` | | `output` | `"output.csv"` | output CSV file or directory, as for `-o` | -| `seed` | `42` | seed of the **particle filter**; see [Seeds](#seeds) | +| `seed` | `42` | the run's seed: the particle filter's, and the fault models' unless `[aiding] seed` is set; see [Seeds](#seeds) | | `is_enu` | `false` | the input records are ENU rather than NED; as `--enu` | | `parallel` | `false` | process a directory's files concurrently | | `generate_plot` | `false` | write a PNG plot beside each closed-loop or particle-filter result | @@ -151,7 +151,7 @@ files keep working -- but `aiding` is the name to write. | `baro_scheduler` | `fixed_interval`, 1 s | when barometric altitude is delivered | | `magnetometer_scheduler` | `fixed_interval`, 1 s | when magnetometer heading is delivered | | `baro_noise_std_m` | `2.23606797749979` | barometric altitude one-sigma, metres | -| `seed` | `42` | seed of the fault models | +| `seed` | the top-level `seed` | seed of the fault models; overrides the top-level `seed` for them | | `max_imu_gap_s` | `5.0` | longest tolerated gap in the inertial stream, seconds; `0` or less disables the check | `max_imu_gap_s` bounds the IMU stream only. A record with a missing accelerometer or gyroscope @@ -495,18 +495,18 @@ apart from not passing the flag, and the file's level wins. See [Logging](./logg ## Seeds -A configuration file has up to three seeds, one per random process, all defaulting to `42`: +A configuration file has up to three seeds: -| Key | Seeds | -|---|---| -| `aiding.seed` | the GNSS fault models | -| `seed` (top level) | the particle filter's sampling; read by `particle-filter` mode only | -| `synthetic.seed` | the generated trajectory's noise | - -On the command line `pf --seed` sets both of the first two at once. In a file they are -independent, so a particle-filter Monte Carlo study can vary the filter's seed while holding the -GNSS corruption fixed, or the reverse. Changing the top-level `seed` of a closed-loop file has no -effect: vary `aiding.seed` instead. +| Key | Default | Seeds | +|---|---|---| +| `seed` (top level) | `42` | the particle filter's sampling, and the GNSS fault models unless `aiding.seed` is set | +| `aiding.seed` | the top-level `seed` | the GNSS fault models | +| `synthetic.seed` | `42` | the generated trajectory's noise | + +So the top-level `seed` alone seeds the whole run, as `--seed` does on the command line: two +closed-loop files that differ only in it give two different fault realizations. Set +`aiding.seed` as well to separate the two, so that a particle-filter Monte Carlo study can vary +the filter's seed while holding the GNSS corruption fixed, or the reverse. The same seeds with the same file and input produce identical output, which is what makes a reported error meaningful. diff --git a/core/src/messages.rs b/core/src/messages.rs index dc5ec27d..84f820fa 100644 --- a/core/src/messages.rs +++ b/core/src/messages.rs @@ -285,10 +285,8 @@ pub enum GnssFaultModel { }, } -/// Default seed value for reproducible simulations -const fn default_seed() -> u64 { - 42 -} +/// Seed of the fault models when [`AidingConfig::seed`] names none and nothing filled it in. +pub const DEFAULT_AIDING_SEED: u64 = 42; /// Default [`AidingConfig::max_imu_gap_s`]: five seconds without a usable inertial sample. /// @@ -424,12 +422,19 @@ pub struct AidingConfig { #[serde(default)] pub baro_bias_index: Option, - /// Random number generator seed for deterministic tests and reproducibility. + /// Seed of the GNSS fault models' random draws (AR(1) degradation, slow-bias random walk). /// /// Use the same seed to repeat scenarios exactly; change it to get a new /// realization of stochastic processes such as AR(1) degradation. - #[serde(default = "default_seed")] - pub seed: u64, + /// + /// `None` -- the default -- means "the run's seed": a scenario file's top-level `seed` + /// fills it (see [`crate::sim::SimulationConfig::resolved_aiding`]), as `strapdown-sim`'s + /// `--seed` does on the command line, and anything else that builds an event stream from + /// a `None` takes [`DEFAULT_AIDING_SEED`]. It was a plain `u64` defaulting to 42, so a + /// config file's top-level `seed` never reached the faults: two files differing only in + /// it gave byte-identical output. + #[serde(default, skip_serializing_if = "Option::is_none")] + pub seed: Option, /// How long the inertial stream may stop reporting before the run is refused, in seconds. /// @@ -464,7 +469,7 @@ impl Default for AidingConfig { magnetometer_scheduler: default_aiding_scheduler(), baro_noise_std_m: default_baro_noise_std_m(), baro_bias_index: None, - seed: default_seed(), + seed: None, max_imu_gap_s: default_max_imu_gap_s(), } } @@ -1337,7 +1342,7 @@ pub fn build_event_stream( .map(|r| ((r.time - start_time).num_milliseconds() as f64 / 1000.0, r)) .collect(); let mut events = Vec::with_capacity(records_with_elapsed.len() * 2); - let mut st = FaultState::new(cfg.seed); + let mut st = FaultState::new(cfg.seed.unwrap_or(DEFAULT_AIDING_SEED)); // Scheduler state, one clock per aided channel. Only `FixedInterval` needs any: // `PassThrough` emits unconditionally and `DutyCycle` derives its window from the elapsed @@ -2270,7 +2275,7 @@ mod tests { tau_pos_s: None, tau_vel_s: None, }, - seed: 500, + seed: Some(500), ..Default::default() }; @@ -2782,6 +2787,7 @@ mod serialization_tests { tau_pos_s: None, tau_vel_s: None, }, + seed: Some(7), ..Default::default() } } diff --git a/core/src/sim.rs b/core/src/sim.rs index 9e706129..799dd9e1 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -5783,7 +5783,12 @@ pub struct SimulationConfig { pub output: String, /// Simulation mode pub mode: SimulationMode, - /// Random number generator seed + /// The run's random seed (default 42). + /// + /// Seeds the particle filter's own sampling and, unless `[aiding] seed` is given, the GNSS + /// fault models -- see [`Self::resolved_aiding`] -- the way `strapdown-sim`'s `--seed` seeds + /// both. It used to reach only the particle filter, so two files differing only in this + /// key gave byte-identical closed-loop output. #[serde(default = "default_seed")] pub seed: u64, /// Local-level frame the input records are expressed in: `false` (the default) is NED, @@ -5873,6 +5878,18 @@ impl Default for SimulationConfig { } impl SimulationConfig { + /// The aiding configuration a run uses: [`Self::aiding`] with its seed filled from the + /// top-level [`Self::seed`] when `[aiding]` names none. + /// + /// An explicit `[aiding] seed` wins, so a study can hold the fault realization fixed + /// while varying the particle filter's seed, or the reverse. + #[must_use] + pub fn resolved_aiding(&self) -> crate::messages::AidingConfig { + let mut aiding = self.aiding.clone(); + aiding.seed = Some(aiding.seed.unwrap_or(self.seed)); + aiding + } + /// Write the configuration to a JSON file (pretty-printed) /// # Errors /// If the file cannot be created or written, or the records cannot be diff --git a/core/tests/example_configs.rs b/core/tests/example_configs.rs index a4491398..962f2fbb 100644 --- a/core/tests/example_configs.rs +++ b/core/tests/example_configs.rs @@ -245,7 +245,8 @@ gnss_degradation: "the aliased section's fault model was silently dropped" ); assert_eq!( - config.aiding.seed, 7, + config.aiding.seed, + Some(7), "the aliased section's seed was silently dropped" ); diff --git a/core/tests/perf_baseline.rs b/core/tests/perf_baseline.rs index a9a2e0ff..956ee443 100644 --- a/core/tests/perf_baseline.rs +++ b/core/tests/perf_baseline.rs @@ -448,7 +448,7 @@ fn scenarios() -> Vec { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built }; @@ -477,7 +477,7 @@ fn scenarios() -> Vec { phase_s: 0.0, }; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built }, estimator, @@ -499,7 +499,7 @@ fn scenarios() -> Vec { start_phase_s: 0.0, }; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built }, estimator: Estimator::Eskf, @@ -525,7 +525,7 @@ fn scenarios() -> Vec { tau_pos_s: None, tau_vel_s: None, }; - built.seed = SEED; + built.seed = Some(SEED); built }, estimator, @@ -547,7 +547,7 @@ fn scenarios() -> Vec { phase_s: 0.0, }; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built }, estimator, @@ -568,7 +568,7 @@ fn scenarios() -> Vec { start_phase_s: 0.0, }; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built }, estimator, diff --git a/core/tests/ukf_conditioning.rs b/core/tests/ukf_conditioning.rs index 7f2e4701..feb19d7f 100644 --- a/core/tests/ukf_conditioning.rs +++ b/core/tests/ukf_conditioning.rs @@ -70,7 +70,7 @@ fn aiding() -> AidingConfig { start_phase_s: 0.0, }; built.fault = GnssFaultModel::None; - built.seed = SEED; + built.seed = Some(SEED); built.baro_bias_index = Some(NAVIGATION_STATES); built } diff --git a/geonav/src/lib.rs b/geonav/src/lib.rs index 90d86a07..75dc30cf 100644 --- a/geonav/src/lib.rs +++ b/geonav/src/lib.rs @@ -2007,7 +2007,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_gravity_map()); @@ -2045,7 +2045,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_gravity_map()); @@ -2079,7 +2079,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_gravity_map()); @@ -2126,7 +2126,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_magnetic_map()); @@ -2821,7 +2821,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_gravity_map()); @@ -2868,7 +2868,7 @@ mod tests { let mut built = AidingConfig::default(); built.scheduler = MeasurementScheduler::PassThrough; built.fault = GnssFaultModel::None; - built.seed = 42; + built.seed = Some(42); built }; let geomap = Rc::new(create_test_gravity_map()); diff --git a/sim/src/main.rs b/sim/src/main.rs index 15550d8b..6bdcd6c7 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -1078,7 +1078,7 @@ fn process_file( // Derived from the filter, not asked for a second time; see // `run_single_closed_loop_simulation` for why (#372). let aiding = { - let mut built = config.aiding.clone(); + let mut built = config.resolved_aiding(); built.baro_bias_index = match filter_config.filter { FilterType::Ukf => ukf_config.baro_bias_index(), FilterType::Ekf => ekf_config.baro_bias_index(), @@ -1240,7 +1240,7 @@ fn process_file( // fifteen-plus states wide, a stray index would pass the barometer's width check // and read a map bias as its own. The Kalman geo path clears it the same way. let aiding = { - let mut aiding = config.aiding.clone(); + let mut aiding = config.resolved_aiding(); aiding.baro_bias_index = None; aiding }; @@ -1878,7 +1878,7 @@ fn run_closed_loop_cli( let mut built = strapdown::messages::AidingConfig::default(); built.scheduler = build_scheduler(&args.scheduler); built.fault = build_fault(&args.fault); - built.seed = args.seed; + built.seed = Some(args.seed); built }; @@ -2108,7 +2108,7 @@ fn geo_settings_from_args( let mut aiding = strapdown::messages::AidingConfig::default(); aiding.scheduler = build_scheduler(&args.scheduler); aiding.fault = build_fault(&args.fault); - aiding.seed = args.seed; + aiding.seed = Some(args.seed); Ok(GeoClosedLoopSettings { filter: args.filter, @@ -2155,7 +2155,7 @@ fn geo_settings_from_config( // Cleared here and derived by the runner from the filter it builds, as the unaided arm of // `process_file` derives it: the index depends on how many map biases precede the // barometric one, which only the runner knows once the maps are loaded. - let mut aiding = config.aiding.clone(); + let mut aiding = config.resolved_aiding(); aiding.baro_bias_index = None; Ok(GeoClosedLoopSettings { @@ -2917,7 +2917,7 @@ fn run_particle_filter( let mut built = strapdown::messages::AidingConfig::default(); built.scheduler = build_scheduler(&args.scheduler); built.fault = build_fault(&args.fault); - built.seed = args.seed; + built.seed = Some(args.seed); // The RBPF carries no barometric bias; see the config-file path. built.baro_bias_index = None; built @@ -3817,7 +3817,8 @@ fn create_config_file() -> Result<(), Box> { let mut built = strapdown::messages::AidingConfig::default(); built.scheduler = scheduler; built.fault = fault; - built.seed = seed; + // Left unset, so the file's top-level `seed` seeds the faults too and editing that one + // key changes the whole run's randomness. built }; @@ -4841,4 +4842,63 @@ magnetic_bias_process_noise_std = 3.0 assert_eq!(synthetic.output, "trajectory.csv"); assert_eq!(synthetic.seed, 7); } + + /// A scenario file's top-level `seed` seeds the GNSS faults unless `[aiding] seed` says + /// otherwise, as `--seed` does on the command line. It used to reach only the particle + /// filter, so two files differing only in `seed` gave byte-identical closed-loop output. + #[test] + fn the_top_level_seed_reaches_the_fault_models() { + let read = |body: &str| { + let dir = tempfile::tempdir().unwrap(); + let path = dir.path().join("seed.toml"); + std::fs::write(&path, body).unwrap(); + SimulationConfig::from_file(&path).unwrap() + }; + let top_level_only = read("mode = \"closed-loop\"\nseed = 7\n"); + assert_eq!(top_level_only.resolved_aiding().seed, Some(7)); + + let both = read("mode = \"closed-loop\"\nseed = 7\n\n[aiding]\nseed = 9\n"); + assert_eq!(both.resolved_aiding().seed, Some(9), "[aiding] seed wins"); + + let neither = read("mode = \"closed-loop\"\n"); + assert_eq!(neither.resolved_aiding().seed, Some(42)); + } + + /// The seed actually changes the faults: the same degraded scenario under two top-level + /// seeds gives two different event streams. + #[test] + fn two_top_level_seeds_give_two_fault_realizations() { + use strapdown::messages::GnssFaultModel; + + let mut synthetic = SyntheticConfig::default(); + synthetic.duration_s = 20.0; + let (_, records) = generate_synthetic(&synthetic, &mut StdRng::seed_from_u64(1)).unwrap(); + let stream_for = |seed: u64| { + let mut config = SimulationConfig::default(); + config.seed = seed; + config.aiding.fault = GnssFaultModel::Degraded { + rho_pos: 0.99, + sigma_pos_m: 10.0, + rho_vel: 0.95, + sigma_vel_mps: 1.0, + r_scale: 1.0, + tau_pos_s: None, + tau_vel_s: None, + }; + let stream = build_event_stream(&records, &config.resolved_aiding(), false).unwrap(); + // The measured values themselves: a model's Debug output does not show them. + let state = nalgebra::DVector::::zeros(15); + stream + .events + .iter() + .filter_map(|event| match event { + Event::Measurement { meas, .. } => meas.get_measurement(&state).ok(), + Event::Imu { .. } => None, + }) + .map(|values| format!("{values:?}")) + .collect::>() + }; + assert_ne!(stream_for(1), stream_for(2)); + assert_eq!(stream_for(1), stream_for(1)); + } } From 591742d1457a1d42d8e68b2d9cd607d9959de1e8 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:11:25 -0400 Subject: [PATCH 11/26] Say that a duty cycle with no start phase opens with its outage duty_cycle_is_on runs start_phase_s of initial availability, then off_s denied and on_s available, repeating. With start_phase_s = 0 there is no initial window, so the run opens with an outage. Several descriptions said the opposite: - the MeasurementScheduler example ("starting in ON state at t=0") and the DutyCycle field docs ("offset before the first toggle"), which now give the timeline and an example with a start phase; - the real_outage_60s and syn_outage_60s scenario descriptions in perf_baseline.rs ("120 s of GNSS then a 60 s outage"), and the same strings in perf_baseline.json, which only render_baseline writes and compare() does not read. No metric, sample count or tolerance in the baseline changes; - the ukf_conditioning.rs bound's derivation ("60 s of GNSS then 60 s"); - examples/configs/simple_dropout.yaml and extended_gnss_denied.yaml, whose start_phase_s: 0.0 now says it opens the run with the outage and what value would open it with GNSS instead. The user guide, quick start, FAQ and performance page already described it correctly. No behaviour changes. Co-Authored-By: Claude Opus 5.5 --- core/src/messages.rs | 19 +++++++++++++------ core/tests/perf_baseline.json | 6 +++--- core/tests/perf_baseline.rs | 4 ++-- core/tests/ukf_conditioning.rs | 3 ++- examples/configs/extended_gnss_denied.yaml | 2 +- examples/configs/simple_dropout.yaml | 2 +- 6 files changed, 22 insertions(+), 14 deletions(-) diff --git a/core/src/messages.rs b/core/src/messages.rs index 84f820fa..55d8bfc7 100644 --- a/core/src/messages.rs +++ b/core/src/messages.rs @@ -47,8 +47,12 @@ use crate::{IMUData, StrapdownError}; /// // Deliver a GNSS fix every 10 seconds, starting at t=0 /// let sched = MeasurementScheduler::FixedInterval { interval_s: 10.0, phase_s: 0.0 }; /// -/// // Alternate 5 s ON, 15 s OFF, starting in ON state at t=0 +/// // Alternate 15 s OFF and 5 s ON. With no start phase the cycle opens with its OFF +/// // window: no fix for t < 15 s, fixes for 15 <= t < 20, none again from t = 20. /// let sched = MeasurementScheduler::DutyCycle { on_s: 5.0, off_s: 15.0, start_phase_s: 0.0 }; +/// +/// // The same cycle after 30 s of initial availability. +/// let sched = MeasurementScheduler::DutyCycle { on_s: 5.0, off_s: 15.0, start_phase_s: 30.0 }; /// ``` #[derive(Clone, Debug, Default, Serialize, Deserialize)] #[serde(tag = "kind", rename_all = "snake_case")] @@ -78,13 +82,17 @@ pub enum MeasurementScheduler { /// /// * `on_s` — Duration of each ON window (seconds). /// * `off_s` — Duration of each OFF window (seconds). - /// * `start_phase_s` — Initial time offset before the first toggle (seconds). + /// * `start_phase_s` — Length of an initial ON window before the cycle starts (seconds). + /// + /// The timeline is `start_phase_s` ON, then `off_s` OFF and `on_s` ON, repeating. So with + /// `start_phase_s = 0` the run **opens with an outage**: the cycle's first window is OFF. DutyCycle { /// Duration of each ON window (seconds). on_s: f64, /// Duration of each OFF window (seconds). off_s: f64, - /// Initial phase offset before the first ON/OFF toggle (seconds). + /// Length of the initial ON window before the first OFF window (seconds); `0.0` opens + /// the run with an OFF window. start_phase_s: f64, }, } @@ -1161,9 +1169,8 @@ fn should_emit(scheduler: &MeasurementScheduler, elapsed_s: f64, next_emit_time: /// Whether a [`MeasurementScheduler::DutyCycle`] is inside an ON window at `elapsed_s`. /// -/// The timeline is `start_phase_s` of initial ON, then `off_s` OFF and `on_s` ON repeating, -/// which is what "initial phase offset before the first ON/OFF toggle" describes: the first -/// toggle takes the scheduler out of its initial ON state. +/// The timeline is `start_phase_s` of initial ON, then `off_s` OFF and `on_s` ON repeating. +/// With `start_phase_s = 0` there is no initial ON window, so the run opens in an OFF one. /// /// Computed from `elapsed_s` directly rather than by stepping a toggle once per sample. The /// stepping version emitted a fix only on the sample where the state flipped *into* ON and diff --git a/core/tests/perf_baseline.json b/core/tests/perf_baseline.json index 4f6d52ff..d657cc69 100644 --- a/core/tests/perf_baseline.json +++ b/core/tests/perf_baseline.json @@ -292,7 +292,7 @@ } }, "real_outage_60s__eskf": { - "description": "test_data.csv, 120 s of GNSS then a 60 s outage, repeating", + "description": "test_data.csv, a 60 s outage then 120 s of GNSS, repeating", "sample_count": 5366, "discarded_channel_samples": 0, "metrics": { @@ -755,7 +755,7 @@ } }, "syn_outage_60s__eskf": { - "description": "synthetic 300 s at 50 Hz, 60 s of GNSS then 60 s of outage", + "description": "synthetic 300 s at 50 Hz, 60 s of outage then 60 s of GNSS", "sample_count": 15000, "discarded_channel_samples": 0, "metrics": { @@ -814,7 +814,7 @@ } }, "syn_outage_60s__ukf": { - "description": "synthetic 300 s at 50 Hz, 60 s of GNSS then 60 s of outage", + "description": "synthetic 300 s at 50 Hz, 60 s of outage then 60 s of GNSS", "sample_count": 15000, "discarded_channel_samples": 0, "metrics": { diff --git a/core/tests/perf_baseline.rs b/core/tests/perf_baseline.rs index 956ee443..2004b135 100644 --- a/core/tests/perf_baseline.rs +++ b/core/tests/perf_baseline.rs @@ -489,7 +489,7 @@ fn scenarios() -> Vec { // usable regression detector on real data. out.push(Scenario { id: "real_outage_60s__eskf".to_string(), - description: "test_data.csv, 120 s of GNSS then a 60 s outage, repeating".to_string(), + description: "test_data.csv, a 60 s outage then 120 s of GNSS, repeating".to_string(), source: Source::Real, gnss: { let mut built = AidingConfig::default(); @@ -558,7 +558,7 @@ fn scenarios() -> Vec { for estimator in [Estimator::Ukf, Estimator::Eskf] { out.push(Scenario { id: format!("syn_outage_60s__{}", estimator.key()), - description: "synthetic 300 s at 50 Hz, 60 s of GNSS then 60 s of outage".to_string(), + description: "synthetic 300 s at 50 Hz, 60 s of outage then 60 s of GNSS".to_string(), source: Source::Synthetic, gnss: { let mut built = AidingConfig::default(); diff --git a/core/tests/ukf_conditioning.rs b/core/tests/ukf_conditioning.rs index feb19d7f..29574d3e 100644 --- a/core/tests/ukf_conditioning.rs +++ b/core/tests/ukf_conditioning.rs @@ -100,7 +100,8 @@ fn solve(records: &[TestDataRecord]) -> Result<(f64, f64), StrapdownError> { /// The bound, in metres of final position. /// -/// Measured over 180 s at 50 Hz with 60 s of GNSS then 60 s of free inertial -- the shape of +/// Measured over 180 s at 50 Hz with 60 s of free inertial then 60 s of GNSS (the duty cycle +/// opens with its OFF window) -- the shape of /// `syn_outage_60s`, which is the row #399 reported moving: /// /// | `ukf_alpha` | final position moved by a one-ulp input change | diff --git a/examples/configs/extended_gnss_denied.yaml b/examples/configs/extended_gnss_denied.yaml index a12c566a..40a648dc 100644 --- a/examples/configs/extended_gnss_denied.yaml +++ b/examples/configs/extended_gnss_denied.yaml @@ -22,7 +22,7 @@ aiding: kind: duty_cycle on_s: 60.0 # 1 minute GNSS available off_s: 300.0 # 5 minutes GNSS denied - start_phase_s: 0.0 + start_phase_s: 0.0 # 0 opens the run with the 5-minute outage, before any fix; 60.0 opens it with GNSS fault: kind: none seed: 42 diff --git a/examples/configs/simple_dropout.yaml b/examples/configs/simple_dropout.yaml index d9dff0ce..945925b6 100644 --- a/examples/configs/simple_dropout.yaml +++ b/examples/configs/simple_dropout.yaml @@ -21,7 +21,7 @@ aiding: kind: duty_cycle on_s: 120.0 # 2 minutes GNSS available off_s: 30.0 # 30 seconds GNSS denied - start_phase_s: 0.0 + start_phase_s: 0.0 # 0 opens the run with the 30 s outage; 120.0 opens it with GNSS fault: kind: none seed: 42 From 06bc5c8b1adf760acc568fcc767520f57fae7c12 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:12:40 -0400 Subject: [PATCH 12/26] Refuse map resolutions finer than the maps support convert_resolution_gravity and convert_resolution_magnetic ended in a catch-all, so a gravity resolution finer than one arc-minute was recorded as one-minute and a magnetic one finer than two arc-minutes as two-minutes, without a word. A run labelled thirty-second gravity aiding was one-minute aiding. Both now refuse a finer label with an error naming the supported resolutions, and check_map_resolutions runs them before any file is read: in GeoClosedLoopSettings::validate (cl --geo and a closed-loop config), at the start of pf, and in run_from_config for a particle-filter config. A test covers both conversions and both closed-loop entry points. conf/rbpf_mag.toml, conf/real/rbpf_mag.toml and examples/configs/geonav_example.toml asked for one-minute magnetic maps, which these runs already recorded as two-minutes; they now say two_minutes, so they keep running with no change in behaviour. The geonav overview said finer labels were recorded as the coarser ones; it now says they are refused. Co-Authored-By: Claude Opus 5.5 --- book/src/geonav/overview.md | 5 +- conf/rbpf_mag.toml | 2 +- conf/real/rbpf_mag.toml | 2 +- examples/configs/geonav_example.toml | 2 +- sim/src/main.rs | 151 ++++++++++++++++++++++++--- 5 files changed, 140 insertions(+), 22 deletions(-) diff --git a/book/src/geonav/overview.md b/book/src/geonav/overview.md index 32695e29..9acc5945 100644 --- a/book/src/geonav/overview.md +++ b/book/src/geonav/overview.md @@ -89,8 +89,9 @@ state exists to absorb. **The resolution flags are labels, not selectors.** The grid used is whatever the map file contains; the resolution is recorded with the map but changes nothing numerically. Gravity -labels stop at `one-minute` and magnetic labels at `two-minutes`, and finer choices are -recorded as those. +labels stop at `one-minute` and magnetic labels at `two-minutes`. A finer choice is refused +before the run starts, with an error listing the supported labels, on the command line and in a +configuration file alike. ## Configuration file diff --git a/conf/rbpf_mag.toml b/conf/rbpf_mag.toml index 076a546e..b797cc5b 100644 --- a/conf/rbpf_mag.toml +++ b/conf/rbpf_mag.toml @@ -75,7 +75,7 @@ magnetic_variation_time_constant_s = 3596.94 # Temperature, 0.5 nT/degC (Strabel et al. 2022), 10% and 1 degC: 0.05 nT # `magnetic_resolution` is a label: the grid is whatever _magnetic.nc holds (earth_wdmam, # 3 arc-minutes). -magnetic_resolution = "one_minute" +magnetic_resolution = "two_minutes" magnetic_bias = 0.0 # nT; the turn-on bias has zero mean magnetic_noise_std = 3.91675 # nT per 10 Hz row magnetic_bias_init_std = 17.0862 # nT; turn-on and drift, in quadrature diff --git a/conf/real/rbpf_mag.toml b/conf/real/rbpf_mag.toml index 8c9b9927..ad957b0b 100644 --- a/conf/real/rbpf_mag.toml +++ b/conf/real/rbpf_mag.toml @@ -28,7 +28,7 @@ zero_vertical_velocity_std_mps = 0.1 [geophysical] # Magnetic anomaly measurements -magnetic_resolution = "one_minute" +magnetic_resolution = "two_minutes" magnetic_bias = 23342.7 magnetic_noise_std = 7537.31 magnetic_bias_init_std = 45902.2 diff --git a/examples/configs/geonav_example.toml b/examples/configs/geonav_example.toml index 8e91caad..a9ae15c4 100644 --- a/examples/configs/geonav_example.toml +++ b/examples/configs/geonav_example.toml @@ -27,7 +27,7 @@ gravity_noise_std = 100.0 # gravity_map_file will be auto-detected if not specified # Magnetic anomaly measurements -magnetic_resolution = "one_minute" +magnetic_resolution = "two_minutes" magnetic_bias = 0.0 magnetic_noise_std = 150.0 # magnetic_map_file will be auto-detected if not specified diff --git a/sim/src/main.rs b/sim/src/main.rs index 6bdcd6c7..542bda01 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -1160,7 +1160,7 @@ fn process_file( None => find_gravity_map(input_file)?, }; let measurement_type = - GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)); + GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)?); Some(Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?)) } else { None @@ -1172,7 +1172,7 @@ fn process_file( None => find_magnetic_map(input_file)?, }; let measurement_type = - GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)); + GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)?); Some(Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?)) } else { None @@ -1420,6 +1420,12 @@ fn run_from_config( return Ok(()); } + // Refused before any file is read, as the command line's resolutions are. + #[cfg(feature = "geonav")] + if let Some(geo) = config.geophysical.as_ref() { + check_map_resolutions(geo.gravity_resolution, geo.magnetic_resolution)?; + } + // Refused before any file is read, as the `--gate-*` flags are. if matches!(config.mode, SimulationMode::ClosedLoop) { let (innovation_gate, gate_recovery) = @@ -1906,10 +1912,27 @@ fn run_closed_loop_cli( // Geophysical Navigation Functions (feature-gated) // ============================================================================ -/// Convert `GeoResolution` to `GravityResolution` +/// The gravity resolutions a map can be fetched at: one degree down to one arc-minute. #[cfg(feature = "geonav")] -const fn convert_resolution_gravity(resolution: GeoResolution) -> GravityResolution { - match resolution { +const SUPPORTED_GRAVITY_RESOLUTIONS: &str = "one-degree, thirty-minutes, twenty-minutes, \ + fifteen-minutes, ten-minutes, six-minutes, five-minutes, four-minutes, three-minutes, \ + two-minutes, one-minute"; + +/// The magnetic resolutions a map can be fetched at: one degree down to two arc-minutes. +#[cfg(feature = "geonav")] +const SUPPORTED_MAGNETIC_RESOLUTIONS: &str = "one-degree, thirty-minutes, twenty-minutes, \ + fifteen-minutes, ten-minutes, six-minutes, five-minutes, four-minutes, three-minutes, \ + two-minutes"; + +/// Convert `GeoResolution` to `GravityResolution`. +/// +/// # Errors +/// For a resolution finer than one arc-minute, which no gravity map here is published at. These +/// used to be accepted and silently recorded as one-minute, so a run labelled thirty-second +/// gravity aiding was one-minute aiding. +#[cfg(feature = "geonav")] +fn convert_resolution_gravity(resolution: GeoResolution) -> Result { + Ok(match resolution { GeoResolution::OneDegree => GravityResolution::OneDegree, GeoResolution::ThirtyMinutes => GravityResolution::ThirtyMinutes, GeoResolution::TwentyMinutes => GravityResolution::TwentyMinutes, @@ -1920,14 +1943,27 @@ const fn convert_resolution_gravity(resolution: GeoResolution) -> GravityResolut GeoResolution::FourMinutes => GravityResolution::FourMinutes, GeoResolution::ThreeMinutes => GravityResolution::ThreeMinutes, GeoResolution::TwoMinutes => GravityResolution::TwoMinutes, - _ => GravityResolution::OneMinute, - } + GeoResolution::OneMinute => GravityResolution::OneMinute, + finer @ (GeoResolution::ThirtySeconds + | GeoResolution::FifteenSeconds + | GeoResolution::ThreeSeconds + | GeoResolution::OneSecond) => { + return Err(format!( + "gravity resolution {finer:?} is finer than any gravity map supports; choose \ + one of: {SUPPORTED_GRAVITY_RESOLUTIONS}" + )); + } + }) } -/// Convert `GeoResolution` to `MagneticResolution` +/// Convert `GeoResolution` to `MagneticResolution`. +/// +/// # Errors +/// For a resolution finer than two arc-minutes, which no magnetic map here is published at. +/// These used to be accepted and silently recorded as two-minute. #[cfg(feature = "geonav")] -const fn convert_resolution_magnetic(resolution: GeoResolution) -> MagneticResolution { - match resolution { +fn convert_resolution_magnetic(resolution: GeoResolution) -> Result { + Ok(match resolution { GeoResolution::OneDegree => MagneticResolution::OneDegree, GeoResolution::ThirtyMinutes => MagneticResolution::ThirtyMinutes, GeoResolution::TwentyMinutes => MagneticResolution::TwentyMinutes, @@ -1937,8 +1973,36 @@ const fn convert_resolution_magnetic(resolution: GeoResolution) -> MagneticResol GeoResolution::FiveMinutes => MagneticResolution::FiveMinutes, GeoResolution::FourMinutes => MagneticResolution::FourMinutes, GeoResolution::ThreeMinutes => MagneticResolution::ThreeMinutes, - _ => MagneticResolution::TwoMinutes, + GeoResolution::TwoMinutes => MagneticResolution::TwoMinutes, + finer @ (GeoResolution::OneMinute + | GeoResolution::ThirtySeconds + | GeoResolution::FifteenSeconds + | GeoResolution::ThreeSeconds + | GeoResolution::OneSecond) => { + return Err(format!( + "magnetic resolution {finer:?} is finer than any magnetic map supports; choose \ + one of: {SUPPORTED_MAGNETIC_RESOLUTIONS}" + )); + } + }) +} + +/// Refuse a gravity or magnetic resolution no map supports, before any file is read. +/// +/// # Errors +/// From [`convert_resolution_gravity`] or [`convert_resolution_magnetic`]. +#[cfg(feature = "geonav")] +fn check_map_resolutions( + gravity: Option, + magnetic: Option, +) -> Result<(), Box> { + if let Some(resolution) = gravity { + convert_resolution_gravity(resolution)?; + } + if let Some(resolution) = magnetic { + convert_resolution_magnetic(resolution)?; } + Ok(()) } /// Auto-detect gravity map file based on input directory @@ -2049,8 +2113,9 @@ impl GeoClosedLoopSettings { /// /// # Errors /// - /// Returns an error when no map is configured, or when the filter is the ESKF, which - /// has no geophysical implementation. The latter matters more than it looks: + /// Returns an error when no map is configured, when a map resolution is finer than any + /// map of that kind supports (see [`check_map_resolutions`]), or when the filter is the + /// ESKF, which has no geophysical implementation. The last matters more than it looks: /// [`FilterType`]'s `#[default]` is `Eskf`, so a configuration file that omits `filter` /// lands here rather than on a filter that works. fn validate(&self) -> Result<(), Box> { @@ -2060,6 +2125,7 @@ impl GeoClosedLoopSettings { `magnetic_resolution` in the `[geophysical]` section of a config file" .into()); } + check_map_resolutions(self.gravity_resolution, self.magnetic_resolution)?; if matches!(self.filter, FilterType::Eskf) { return Err( "ESKF is not yet implemented for geophysical navigation. Choose \ @@ -2570,7 +2636,7 @@ fn run_geo_closed_loop_file( info!("Loading gravity map from: {}", map_path.display()); let measurement_type = - GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)); + GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)?); let map = Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?); info!( "Loaded gravity map with {} x {} grid points", @@ -2591,7 +2657,7 @@ fn run_geo_closed_loop_file( info!("Loading magnetic map from: {}", map_path.display()); let measurement_type = - GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)); + GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)?); let map = Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?); info!( "Loaded magnetic map with {} x {} grid points", @@ -2900,6 +2966,8 @@ fn run_particle_filter( plot: bool, ) -> Result<(), Box> { refuse_removed_particle_filter_flags(args)?; + #[cfg(feature = "geonav")] + check_map_resolutions(args.geo.gravity_resolution, args.geo.magnetic_resolution)?; validate_input_path(&args.sim.input)?; validate_output_path(&args.sim.output)?; @@ -2937,7 +3005,7 @@ fn run_particle_filter( }; info!("Loading gravity map from: {}", map_path.display()); let measurement_type = - GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)); + GeophysicalMeasurementType::Gravity(convert_resolution_gravity(res)?); Some(Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?)) } else { None @@ -2950,7 +3018,7 @@ fn run_particle_filter( }; info!("Loading magnetic map from: {}", map_path.display()); let measurement_type = - GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)); + GeophysicalMeasurementType::Magnetic(convert_resolution_magnetic(res)?); Some(Rc::new(GeoMap::load_geomap(&map_path, measurement_type)?)) } else { None @@ -4901,4 +4969,53 @@ magnetic_bias_process_noise_std = 3.0 assert_ne!(stream_for(1), stream_for(2)); assert_eq!(stream_for(1), stream_for(1)); } + + /// A resolution finer than the maps support is refused, naming the supported ones. It used + /// to be recorded as the coarser one without a word. + #[cfg(feature = "geonav")] + #[test] + fn map_resolutions_finer_than_the_data_are_refused() { + let error = convert_resolution_gravity(GeoResolution::ThirtySeconds).unwrap_err(); + assert!(error.contains("one-minute"), "got: {error}"); + assert!(matches!( + convert_resolution_gravity(GeoResolution::OneMinute), + Ok(GravityResolution::OneMinute) + )); + + let error = convert_resolution_magnetic(GeoResolution::OneMinute).unwrap_err(); + assert!(error.contains("two-minutes"), "got: {error}"); + assert!(matches!( + convert_resolution_magnetic(GeoResolution::TwoMinutes), + Ok(MagneticResolution::TwoMinutes) + )); + + // On both the command line and a config file, before any input is read. + let cli = Cli::try_parse_from([ + "strapdown-sim", + "cl", + "-i", + "in.csv", + "-o", + "out.csv", + "--filter", + "ukf", + "--geo", + "--gravity-resolution", + "thirty-seconds", + ]) + .unwrap(); + let Some(Command::ClosedLoop(args)) = cli.command else { + panic!("expected the `cl` subcommand"); + }; + let settings = geo_settings_from_args(&args, false).unwrap(); + assert!(settings.validate().is_err()); + + let config = config_from_toml( + "mode = \"closed-loop\"\n\n[closed_loop]\nfilter = \"ukf\"\n\n\ + [geophysical]\nmagnetic_resolution = \"one_minute\"\n", + ); + let geo = config.geophysical.as_ref().unwrap(); + let settings = geo_settings_from_config(&config, geo).unwrap(); + assert!(settings.validate().is_err()); + } } From 8bd030483067bd4adc5494040a47176abee584e6 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:13:53 -0400 Subject: [PATCH 13/26] Bring the geonav docs back in line with its API - geonav::build_event_stream's doc listed positional gravity_map, gravity_noise_std, magnetic_map, magnetic_noise_std, geo_interval_s and bias_layout arguments. The function takes is_enu and one &GeophysicalAiding; the doc now lists those. - README.md's library example called build_event_stream with that old seven-argument signature and ran an undefined `ukf`. It now builds a gravity-aided stream against the real API -- checked by compiling it as a test -- and points at run_closed_loop_with_geo and the CLI's runner for the bias-state wiring. - The README's CLI examples used --dropout-start-s/--dropout-duration-s, which do not exist, and two of them omitted --filter, so they ran the default ESKF, which has no geophysical arm and refuses. They now use --sched fixed and pass --filter. The interval flag is --geo-interval-s; --geo-frequency-s is its alias. - CombinedGeophysicalMeasurement claimed a joint update "enables cross-correlation between the two modalities" and is more efficient than sequential updates. Its R is diagonal, so a linearized joint update equals two sequential ones; the doc now says that, and where correlation does enter (H P H^T). Co-Authored-By: Claude Opus 5.5 --- geonav/README.md | 69 +++++++++++++++++++++++++++-------------------- geonav/src/lib.rs | 33 +++++++++++------------ 2 files changed, 56 insertions(+), 46 deletions(-) diff --git a/geonav/README.md b/geonav/README.md index ad0bb6f6..587ad12a 100644 --- a/geonav/README.md +++ b/geonav/README.md @@ -24,11 +24,13 @@ strapdown-sim cl --input data.csv --output out/ \ --gravity-resolution one-minute \ --filter ukf -# Magnetic navigation +# Magnetic navigation. The default filter, the ESKF, has no geophysical arm: pick the +# UKF or the EKF. strapdown-sim cl --input data.csv --output out/ \ --geo \ --magnetic-resolution two-minutes \ - --magnetic-noise-std 150.0 + --magnetic-noise-std 150.0 \ + --filter ekf ``` ### As a Library @@ -40,34 +42,41 @@ Add to your `Cargo.toml`: strapdown-geonav = { path = "../geonav" } ``` -Use in your code: +Use in your code. `build_event_stream` takes the records, the aiding configuration, the +records' frame and a `GeophysicalAiding` describing the maps: ```rust +use std::path::Path; use std::rc::Rc; -use strapdown::sim::run_closed_loop; -use geonav::{GeoMap, GeophysicalMeasurementType, GravityResolution}; -use geonav::build_event_stream; - -// Load a gravity map -let measurement_type = GeophysicalMeasurementType::Gravity(GravityResolution::OneMinute); -let map = GeoMap::load_geomap("gravity_map.nc", measurement_type)?; - -// Build event stream with geophysical measurements -let events = build_event_stream( - &records, - &gnss_config, - Some(Rc::new(map)), - Some(100.0), // gravity noise std (mGal) - None, // no magnetic map - None, - None, -)?; - -// Run simulation. The geonav-specific `geo_closed_loop_*` drivers were removed in favour -// of the one driver in `strapdown-core`, which takes any `NavigationFilter`. -let results = run_closed_loop(&mut ukf, events, None, None)?; + +use geonav::{ + GeoMap, GeophysicalAiding, GeophysicalMeasurementType, GravityResolution, build_event_stream, +}; +use strapdown::messages::{AidingConfig, EventStream}; +use strapdown::sim::TestDataRecord; + +fn gravity_aided_events(records: &[TestDataRecord]) -> Result> { + let map = GeoMap::load_geomap( + Path::new("gravity_map.nc"), + GeophysicalMeasurementType::Gravity(GravityResolution::OneMinute), + )?; + let geophysical = GeophysicalAiding { + gravity_map: Some(Rc::new(map)), + gravity_noise_std: Some(100.0), // mGal + interval_s: Some(10.0), // one gravity measurement every 10 s + // No magnetic map, and a filter that carries no map-bias state. + ..GeophysicalAiding::default() + }; + Ok(build_event_stream(records, &AidingConfig::default(), false, &geophysical)?) +} ``` +Feed the stream to any `NavigationFilter` through `strapdown::sim::run_closed_loop_with_geo`. +A filter that estimates the map bias also needs a `GeoBiasLayout` in +`GeophysicalAiding::bias_layout` and the matching `ExtraStateLayout` on the runner; the rustdoc +for both, and `strapdown-sim`'s geophysical runner (`run_geo_closed_loop_file` in +`sim/src/main.rs`), show the whole arrangement. + ## Command Line Options When using `strapdown-sim --features geonav`: @@ -88,7 +97,8 @@ When using `strapdown-sim --features geonav`: - `--magnetic-map-file`: Custom map file path ### Common Options -- `--geo-frequency-s`: Geophysical measurement frequency in seconds +- `--geo-interval-s`: Seconds *between* geophysical measurements (`--geo-frequency-s` is + accepted as an alias) ## Geophysical Map Files @@ -157,15 +167,16 @@ explicitly. ### GNSS-Denied Navigation with Gravity Aiding ```bash +# One GNSS fix at t = 0 and none after it. strapdown-sim cl --input urban_canyon.csv --output out/ \ - --geo --gravity-resolution one-minute \ - --dropout-start-s 0 --dropout-duration-s 999999 + --geo --gravity-resolution one-minute --filter ukf \ + --sched fixed --interval-s 1000000 ``` ### Intermittent GNSS with Magnetic Aiding ```bash strapdown-sim cl --input flight_data.csv --output out/ \ - --geo --magnetic-resolution five-minutes \ + --geo --magnetic-resolution five-minutes --filter ukf \ --sched duty --on-s 10 --off-s 40 ``` diff --git a/geonav/src/lib.rs b/geonav/src/lib.rs index 75dc30cf..42af48f8 100644 --- a/geonav/src/lib.rs +++ b/geonav/src/lib.rs @@ -1331,13 +1331,16 @@ impl MagneticAnomalyMeasurement { /// Combined gravity and magnetic anomaly measurement model /// -/// When both gravity and magnetic anomaly maps are available, this model jointly processes -/// them as a single 2-dimensional measurement update. This enables cross-correlation between -/// the two modalities and provides a more statistically efficient update than processing them -/// sequentially as independent 1D measurements. +/// When both gravity and magnetic anomaly maps are available, this model processes them as a +/// single 2-dimensional measurement update. /// /// The measurement vector is `[gravity_anomaly, magnetic_anomaly]` and the noise covariance -/// is block-diagonal (assumes independence between gravity and magnetic noise). +/// `R` is **diagonal**: the two sensors' noises are modelled as independent, with no +/// cross-correlation term. The two predicted anomalies are still correlated through the state +/// they share, so the innovation covariance `H P H^T + R` has off-diagonal entries; but with a +/// diagonal `R` a linearized joint update is equivalent to two sequential 1-D updates, so this +/// is a convenience (one update, one sigma-point pass in the UKF) rather than a statistically +/// stronger model. #[derive(Clone, Debug)] pub struct CombinedGeophysicalMeasurement { /// Gravity anomaly measurement model @@ -1453,18 +1456,14 @@ pub struct GeophysicalAiding { /// * `records` - Vector of test data records /// * `cfg` - Aiding configuration: the GNSS schedule and fault model, plus the barometer /// and magnetometer schedules -/// * `gravity_map` - Optional gravity map for measurements -/// * `gravity_noise_std` - Standard deviation for gravity measurement noise (if `gravity_map` is Some) -/// * `magnetic_map` - Optional magnetic map for measurements -/// * `magnetic_noise_std` - Standard deviation for magnetic measurement noise (if `magnetic_map` is Some) -/// * `geo_interval_s` - Seconds *between* geophysical measurements, so a larger value means -/// fewer of them (None for every available measurement) -/// * `bias_layout` - Where the filter that will consume this stream carries its map-bias -/// states, or `None` when it carries none. This is not inferable from the maps: loading a -/// gravity map says a gravity *measurement* is available, not that the filter estimating -/// from it has a state to absorb that map's bias. Getting it from the caller, who knows -/// the filter, is what keeps a stream built for one filter from being read against -/// another's states -- see [`GeoBiasLayout`] and [`resolve_bias_index`]. +/// * `is_enu` - The local-level frame the records are expressed in, as for +/// [`strapdown::messages::build_event_stream`] +/// * `geophysical` - The maps, their noise, the measurement interval and the filter's bias +/// layout; see [`GeophysicalAiding`]. `bias_layout` in particular is not inferable from the +/// maps: loading a gravity map says a gravity *measurement* is available, not that the +/// filter estimating from it has a state to absorb that map's bias. Getting it from the +/// caller, who knows the filter, is what keeps a stream built for one filter from being +/// read against another's states -- see [`GeoBiasLayout`] and [`resolve_bias_index`]. /// /// # Errors /// [`StrapdownError::InvalidConfiguration`] if `records` is empty. The first record supplies From 77ec501738f6defcb53d7a9ee1e84f6b2bb1f9c5 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:14:08 -0400 Subject: [PATCH 14/26] Drop a dataset median from the degraded-GNSS recipe comments Twenty-four conf/ recipes justified the 35 m degraded profile as "about 14x this dataset's 2.58 m median advertised accuracy". The median depends on which recordings data/input holds and does not reproduce on the current data (2.46-3.06 m across trajectories), so the comment went stale the moment the data changed. It now says the wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy, which is the claim the profile actually rests on. Comments only; no key or value changes. Co-Authored-By: Claude Opus 5.5 --- conf/ekf_both.toml | 4 ++-- conf/ekf_degraded.toml | 4 ++-- conf/ekf_grav.toml | 4 ++-- conf/ekf_mag.toml | 4 ++-- conf/rbpf_both.toml | 4 ++-- conf/rbpf_degraded.toml | 4 ++-- conf/rbpf_grav.toml | 4 ++-- conf/rbpf_mag.toml | 4 ++-- conf/real/ekf_both.toml | 4 ++-- conf/real/ekf_degraded.toml | 4 ++-- conf/real/ekf_grav.toml | 4 ++-- conf/real/ekf_mag.toml | 4 ++-- conf/real/rbpf_both.toml | 4 ++-- conf/real/rbpf_degraded.toml | 4 ++-- conf/real/rbpf_grav.toml | 4 ++-- conf/real/rbpf_mag.toml | 4 ++-- conf/real/ukf_both.toml | 4 ++-- conf/real/ukf_degraded.toml | 4 ++-- conf/real/ukf_grav.toml | 4 ++-- conf/real/ukf_mag.toml | 4 ++-- conf/ukf_both.toml | 4 ++-- conf/ukf_degraded.toml | 4 ++-- conf/ukf_grav.toml | 4 ++-- conf/ukf_mag.toml | 4 ++-- 24 files changed, 48 insertions(+), 48 deletions(-) diff --git a/conf/ekf_both.toml b/conf/ekf_both.toml index 0c5513b0..7c4dbe94 100644 --- a/conf/ekf_both.toml +++ b/conf/ekf_both.toml @@ -82,8 +82,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ekf_degraded.toml b/conf/ekf_degraded.toml index d8d4f81a..3333907b 100644 --- a/conf/ekf_degraded.toml +++ b/conf/ekf_degraded.toml @@ -48,8 +48,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ekf_grav.toml b/conf/ekf_grav.toml index 098314e6..a427d9ab 100644 --- a/conf/ekf_grav.toml +++ b/conf/ekf_grav.toml @@ -67,8 +67,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ekf_mag.toml b/conf/ekf_mag.toml index 47b5c988..5cfe5fb0 100644 --- a/conf/ekf_mag.toml +++ b/conf/ekf_mag.toml @@ -67,8 +67,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/rbpf_both.toml b/conf/rbpf_both.toml index b4982fd3..390d397b 100644 --- a/conf/rbpf_both.toml +++ b/conf/rbpf_both.toml @@ -122,8 +122,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/rbpf_degraded.toml b/conf/rbpf_degraded.toml index a338ccfb..90fce1d2 100644 --- a/conf/rbpf_degraded.toml +++ b/conf/rbpf_degraded.toml @@ -84,8 +84,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/rbpf_grav.toml b/conf/rbpf_grav.toml index 21cc1053..326f9b8f 100644 --- a/conf/rbpf_grav.toml +++ b/conf/rbpf_grav.toml @@ -104,8 +104,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/rbpf_mag.toml b/conf/rbpf_mag.toml index b797cc5b..d1880c46 100644 --- a/conf/rbpf_mag.toml +++ b/conf/rbpf_mag.toml @@ -104,8 +104,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ekf_both.toml b/conf/real/ekf_both.toml index 34594bda..20e50f37 100644 --- a/conf/real/ekf_both.toml +++ b/conf/real/ekf_both.toml @@ -51,8 +51,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ekf_degraded.toml b/conf/real/ekf_degraded.toml index 83a91740..01bf4f4a 100644 --- a/conf/real/ekf_degraded.toml +++ b/conf/real/ekf_degraded.toml @@ -38,8 +38,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ekf_grav.toml b/conf/real/ekf_grav.toml index c73e8ffc..8848f2ba 100644 --- a/conf/real/ekf_grav.toml +++ b/conf/real/ekf_grav.toml @@ -48,8 +48,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ekf_mag.toml b/conf/real/ekf_mag.toml index e2c18c73..056ad873 100644 --- a/conf/real/ekf_mag.toml +++ b/conf/real/ekf_mag.toml @@ -49,8 +49,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/rbpf_both.toml b/conf/real/rbpf_both.toml index 1077a415..eae49859 100644 --- a/conf/real/rbpf_both.toml +++ b/conf/real/rbpf_both.toml @@ -55,8 +55,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/rbpf_degraded.toml b/conf/real/rbpf_degraded.toml index f1e586a2..1aaa9424 100644 --- a/conf/real/rbpf_degraded.toml +++ b/conf/real/rbpf_degraded.toml @@ -46,8 +46,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/rbpf_grav.toml b/conf/real/rbpf_grav.toml index f191c0c1..2d450d0b 100644 --- a/conf/real/rbpf_grav.toml +++ b/conf/real/rbpf_grav.toml @@ -54,8 +54,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/rbpf_mag.toml b/conf/real/rbpf_mag.toml index ad957b0b..342cf1d1 100644 --- a/conf/real/rbpf_mag.toml +++ b/conf/real/rbpf_mag.toml @@ -54,8 +54,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ukf_both.toml b/conf/real/ukf_both.toml index c8ecbc46..df948cc3 100644 --- a/conf/real/ukf_both.toml +++ b/conf/real/ukf_both.toml @@ -51,8 +51,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ukf_degraded.toml b/conf/real/ukf_degraded.toml index b7db2079..764e0b6b 100644 --- a/conf/real/ukf_degraded.toml +++ b/conf/real/ukf_degraded.toml @@ -38,8 +38,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ukf_grav.toml b/conf/real/ukf_grav.toml index 1952c489..a633b307 100644 --- a/conf/real/ukf_grav.toml +++ b/conf/real/ukf_grav.toml @@ -48,8 +48,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/real/ukf_mag.toml b/conf/real/ukf_mag.toml index 3c3a9241..6d59efd6 100644 --- a/conf/real/ukf_mag.toml +++ b/conf/real/ukf_mag.toml @@ -49,8 +49,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ukf_both.toml b/conf/ukf_both.toml index 93c333c7..46b53c64 100644 --- a/conf/ukf_both.toml +++ b/conf/ukf_both.toml @@ -82,8 +82,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ukf_degraded.toml b/conf/ukf_degraded.toml index 854e272f..bf586796 100644 --- a/conf/ukf_degraded.toml +++ b/conf/ukf_degraded.toml @@ -48,8 +48,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ukf_grav.toml b/conf/ukf_grav.toml index 2a4e4e37..b11269a3 100644 --- a/conf/ukf_grav.toml +++ b/conf/ukf_grav.toml @@ -67,8 +67,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is diff --git a/conf/ukf_mag.toml b/conf/ukf_mag.toml index 1fc8c1bc..0bafb990 100644 --- a/conf/ukf_mag.toml +++ b/conf/ukf_mag.toml @@ -67,8 +67,8 @@ speed_mps_max = 5000.0 # Smartphone GNSS is 3-5 m under good multipath and over 10 m in harsh multipath, with # semi-urban CEP95 around 10-12 m. Under jamming the tracked satellite count falls and GDOP # rises, so the true error and the advertised accuracy inflate together. 35 m of steady-state -# wander is about 14x this dataset's 2.58 m median advertised accuracy -- a receiver still -# producing fixes, but bad ones. +# wander is roughly an order of magnitude above a phone GNSS fix's few-metre accuracy -- a +# receiver still producing fixes, but bad ones. # # `tau_pos_s` and `tau_vel_s` are what make this comparable across schedules. Without them # `rho_pos` is applied once per emitted fix, so the correlation time is From 57aeed70ff713cca102e9e0c6b4e772f691837f7 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:14:19 -0400 Subject: [PATCH 15/26] List lint-min and test-min in the pull-request checklist CONTRIBUTING.md's "Before you open a pull request" block gave fmt-check, lint and the workspace tests. CI also gates on the --no-default-features configuration of strapdown-core, which those cannot see: an item used only behind a feature gate is dead without it. The block now lists cargo lint-min and cargo test-min and says why. Co-Authored-By: Claude Opus 5.5 --- CONTRIBUTING.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 279df8e3..acf23a08 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -39,15 +39,21 @@ or put the release binary on your `PATH`. ## Before you open a pull request -The workspace enforces a strict lint gate rather than a warning-level one. Both of these must +The workspace enforces a strict lint gate rather than a warning-level one. All of these must be clean, and CI runs exactly the same commands on the same pinned 1.91 toolchain: ```bash cargo fmt-check # cargo fmt --all -- --check cargo lint # cargo clippy --workspace --all-targets --all-features -- -D warnings +cargo lint-min # cargo clippy -p strapdown-core --all-targets --no-default-features -- -D warnings cargo test --workspace --all-features +cargo test-min # cargo test -p strapdown-core --no-default-features ``` +`cargo lint` alone is not the whole gate. The two configurations disagree about what is dead: +an item used only behind a feature gate is live under `--all-features` and unused without it, +so `cargo lint-min` and `cargo test-min` catch what `cargo lint` cannot. + `clippy::pedantic` and `clippy::nursery` are denied workspace-wide, as are `missing_docs` and the zero-panic lints (`unwrap_used`, `expect_used`, `panic`) in library code -- return a `StrapdownError` instead. Every `pub` item, including struct fields and enum variants, needs a From 032dc0c3109bca778959abea7ffd5e366b55a994 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:15:16 -0400 Subject: [PATCH 16/26] Describe the forced update as the code applies it GateRecovery::forced_update_after was documented as "the number of consecutive rejections after which the next measurement is applied". GatePolicy::decide counts the measurement it is deciding: the N-th consecutive failure is itself forced through. With the default 5 that is four rejections and a forced fifth, which is what core/examples/aiding.rs prints (t = 61-64 rejected, t = 65 forced). The field, the struct's recovery list, DEFAULT_FORCED_UPDATE_AFTER, UpdateOutcome::forced, MIN_FORCED_UPDATE_AFTER and the module summary now say so, as do --gate-force-after's help, the aiding example's header and the tutorial, closed-loop and configuration pages; filters/measurements.md already had it right. The warning logged on a forced update said "after N consecutive rejections" and now says N in a row have failed. Behaviour is unchanged, so no baseline moves. Co-Authored-By: Claude Opus 5.5 --- book/src/examples/tutorial-basic.md | 6 ++--- book/src/user-guide/closed-loop.md | 2 +- book/src/user-guide/configuration.md | 2 +- core/examples/aiding.rs | 2 +- core/src/gating.rs | 39 ++++++++++++++++------------ sim/src/main.rs | 10 ++++--- 6 files changed, 34 insertions(+), 27 deletions(-) diff --git a/book/src/examples/tutorial-basic.md b/book/src/examples/tutorial-basic.md index c922e424..5adfd914 100644 --- a/book/src/examples/tutorial-basic.md +++ b/book/src/examples/tutorial-basic.md @@ -49,9 +49,9 @@ without a recovery path the next fix disagrees by *more*, is rejected harder, an rejection quietly turns into dead reckoning for the rest of the run. `GateRecovery` is what prevents that: every rejection inflates the filter's uncertainty in the directions that measurement observed -- so the next fix of the same kind is judged against a covariance that -grew -- and after five consecutive rejections a measurement is applied regardless, on the -reasoning that a belief contradicted five times running is likelier to be wrong than the sensor -contradicting it. `set_gate_recovery` +grew -- and when five measurements in a row fail the gate, the fifth is applied regardless +(four rejections, then a forced update), on the reasoning that a belief contradicted five times +running is likelier to be wrong than the sensor contradicting it. `set_gate_recovery` tunes it, `GateRecovery::none()` switches it off, and `UpdateOutcome::forced` says whether an accepted fix was accepted on its merits or by that escape. diff --git a/book/src/user-guide/closed-loop.md b/book/src/user-guide/closed-loop.md index 31bf9528..fde9d695 100644 --- a/book/src/user-guide/closed-loop.md +++ b/book/src/user-guide/closed-loop.md @@ -199,7 +199,7 @@ normalized innovation squared (NIS) is implausibly large for the filter's own co |---|---|---| | `--gate-confidence

` | unset (no gate) | reject a measurement whose NIS exceeds the $\chi^2$ quantile at probability `P`, for that measurement's own degrees of freedom. Must lie strictly inside (0, 1) | | `--gate-inflation ` | `2` | each rejection multiplies the filter's uncertainty by this factor, in the directions the rejected measurement observed. At least 1.0; 1.0 disables it | -| `--gate-force-after ` | `5` | after this many consecutive rejections, apply the next measurement regardless. `0` disables it; `1` is refused, because it would be no gate at all | +| `--gate-force-after ` | `5` | when this many measurements in a row fail the gate, apply the last of them regardless: with `5`, four are rejected and the fifth is forced. `0` disables it; `1` is refused, because it would be no gate at all | Because the threshold is the $\chi^2$ quantile at each measurement's own dimension, one confidence level means the same thing for a 1-DOF barometer reading and a multi-DOF GNSS fix. diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index 6b8be26e..e7c1e173 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -386,7 +386,7 @@ Consulted only when a gate is installed. Either field may be omitted and keeps i | Key | Default | Meaning | |---|---|---| | `rejection_inflation` | `2.0` | each rejection multiplies the filter's uncertainty by this factor in the directions the rejected measurement observed; `1.0` disables it | -| `forced_update_after` | `5` | after this many consecutive rejections, apply the next measurement regardless; `0` disables it (YAML may also write `null`) | +| `forced_update_after` | `5` | when this many measurements in a row fail the gate, apply the last of them regardless (with `5`: four rejected, the fifth forced); `0` disables it (YAML may also write `null`) | These values are checked before a run starts, exactly as the command-line flags `--gate-confidence`, `--gate-inflation` and `--gate-force-after` are: a confidence outside (0, 1), diff --git a/core/examples/aiding.rs b/core/examples/aiding.rs index e9e43be3..35f4674d 100644 --- a/core/examples/aiding.rs +++ b/core/examples/aiding.rs @@ -15,7 +15,7 @@ //! 2. **A magnetometer heading.** A body-frame field is synthesised for a known heading and //! turned back into a yaw measurement, with WMM declination applied. //! 3. **A spoofed fix.** A chi-squared innovation gate rejects a GNSS fix 200 m off, and the -//! default [`GateRecovery`] forces a fix through after five consecutive rejections. +//! default [`GateRecovery`] rejects four fixes in a row and forces the fifth through. //! //! None of the pseudo-measurements here are applied by `strapdown-sim`: the CLI's event //! stream carries GNSS position and velocity, barometric altitude and magnetometer yaw. ZUPT, diff --git a/core/src/gating.rs b/core/src/gating.rs index d2d97822..586c16f0 100644 --- a/core/src/gating.rs +++ b/core/src/gating.rs @@ -27,8 +27,8 @@ //! - [`InnovationGate`] -- the accept/reject policy, either a $\chi^2$ quantile //! evaluated at the measurement's own degrees of freedom or a fixed threshold. //! - [`GateRecovery`] -- how the filter climbs back out of a rejection, by inflating -//! its covariance and, after enough consecutive rejections, applying a measurement -//! anyway. Without it a gate is a one-way door (#340). +//! its covariance and, when enough consecutive measurements fail the gate, applying the +//! last of them anyway. Without it a gate is a one-way door (#340). //! - [`GatePolicy`] -- the gate, its recovery and the rejection streak they share; //! this is what a filter stores, and [`GateDecision`] is what it gets back. //! - [`UpdateOutcome`] -- what every [`NavigationFilter::update`] now returns, so a @@ -98,8 +98,8 @@ pub struct UpdateOutcome { pub dof: usize, /// Whether the correction was applied to the state. pub accepted: bool, - /// Whether the correction was applied *despite* failing the gate, because - /// [`GateRecovery::forced_update_after`] consecutive rejections had accumulated. + /// Whether the correction was applied *despite* failing the gate, because it was the + /// [`GateRecovery::forced_update_after`]-th consecutive measurement to fail it. /// /// Only ever true together with [`Self::accepted`]. Reported rather than folded /// into `accepted` because the two mean different things to a reader of the logs: @@ -281,8 +281,8 @@ pub const DEFAULT_REJECTION_INFLATION: f64 = 2.0; /// Smallest meaningful [`GateRecovery::forced_update_after`]. /// -/// One would apply a measurement the moment the gate rejected it, which is not a gate at -/// all; zero reads as "immediately" and means the same thing. [`GateRecovery::new`] refuses +/// One would force through the first measurement to fail the gate, i.e. every one, which is +/// not a gate at all; zero reads as "immediately" and means the same thing. [`GateRecovery::new`] refuses /// both, and because deserialization bypasses it, [`GatePolicy::decide`] ignores them too -- /// the escape is simply switched off, leaving [`GateRecovery::rejection_inflation`] as the /// way back. That is the conservative direction for *this* knob: a threshold below 2 would @@ -290,14 +290,15 @@ pub const DEFAULT_REJECTION_INFLATION: f64 = 2.0; /// slower. pub const MIN_FORCED_UPDATE_AFTER: usize = 2; -/// Default number of consecutive rejections after which an update is forced through. +/// Default length of the failing streak whose last measurement is forced through: four +/// rejections, then the fifth consecutive failure is applied. /// /// Five 1 Hz fixes is a few seconds of disagreement -- long enough that a genuine /// multipath burst or a single corrupted fix is still rejected on its own merits, /// short enough that a filter whose own state is the thing that is wrong cannot /// coast for a minute before finding out. Combined with -/// [`DEFAULT_REJECTION_INFLATION`] it also bounds the inflation: at most $2^4 = 16$ -/// times the covariance accumulates before a measurement is admitted. +/// [`DEFAULT_REJECTION_INFLATION`] it also bounds the inflation: the four rejections +/// accumulate at most $2^4 = 16$ times the covariance before a measurement is admitted. pub const DEFAULT_FORCED_UPDATE_AFTER: usize = 5; /// How a filter recovers from a rejected measurement. @@ -319,8 +320,8 @@ pub const DEFAULT_FORCED_UPDATE_AFTER: usize = 5; /// [`Self::rejection_inflation`] on every rejection, in the directions the rejected /// measurement observed, so the claimed uncertainty grows geometrically while the error /// grows polynomially and the gate re-opens on its own. -/// 2. **Forced update.** After [`Self::forced_update_after`] consecutive rejections, -/// apply the next measurement whatever its NIS: a belief contradicted that many +/// 2. **Forced update.** When [`Self::forced_update_after`] consecutive measurements fail +/// the gate, apply the last of them whatever its NIS: a belief contradicted that many /// times running is more likely wrong than the sensor contradicting it. /// /// Inflation is the mechanism that keeps the covariance *honest*, and it is the one doing @@ -338,7 +339,7 @@ pub const DEFAULT_FORCED_UPDATE_AFTER: usize = 5; /// ```rust /// use strapdown::gating::GateRecovery; /// -/// // The default: double P per rejection, force an update after five in a row. +/// // The default: double P per rejection; reject four in a row, force the fifth. /// let recovery = GateRecovery::default(); /// assert_eq!(recovery.forced_update_after, Some(5)); /// @@ -369,8 +370,12 @@ pub struct GateRecovery { /// correction weighted by a velocity uncertainty of tens of m/s, and the run reached /// 719 m/s of reported ground speed. pub rejection_inflation: f64, - /// Number of consecutive rejections after which the next measurement is applied - /// regardless of its NIS, or `None` to never force one. + /// Length of the failing streak whose last measurement is applied regardless of its + /// NIS, or `None` to never force one. + /// + /// The count includes the measurement being decided: with `Some(5)`, four measurements + /// in a row are rejected and the fifth to fail the gate is applied and reported as + /// [`UpdateOutcome::forced`], so the inflation behind it is that of four rejections. /// /// Counted across every sensor, like /// [`HealthLimits::nis_pos_consec_fail`](crate::sim::health::HealthLimits::nis_pos_consec_fail), @@ -380,7 +385,7 @@ pub struct GateRecovery { /// the navigation state. /// /// A value below [`MIN_FORCED_UPDATE_AFTER`] switches the escape off rather than - /// forcing on the first rejection: [`Self::new`] refuses `Some(0)` and `Some(1)`, and a + /// forcing the first measurement to fail: [`Self::new`] refuses `Some(0)` and `Some(1)`, and a /// config that deserializes past it is ignored here for the same reason -- forcing /// immediately would disable the gate the user configured. /// @@ -718,8 +723,8 @@ impl GatePolicy { // overruled by its own sensors, which is worth seeing in a default log even // at one line per `limit` fixes. log::warn!( - "{filter_name}: forcing a measurement through the gate after \ - {streak} consecutive rejections, NIS = {nis:.3} > {threshold:.3} (dof {dof})" + "{filter_name}: forcing a measurement through the gate, {streak} in a row \ + having failed it, NIS = {nis:.3} > {threshold:.3} (dof {dof})" ); self.consecutive_rejections = 0; return GateDecision { diff --git a/sim/src/main.rs b/sim/src/main.rs index 542bda01..bee9d285 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -570,11 +570,13 @@ struct ClosedLoopSimArgs { #[arg(long, value_name = "FACTOR", default_value_t = DEFAULT_REJECTION_INFLATION)] gate_inflation: f64, - /// Apply a measurement despite the gate after this many consecutive rejections. + /// Apply the COUNT-th consecutive measurement to fail the gate despite it. /// - /// Only used together with `--gate-confidence`. A belief contradicted this many - /// times running is likelier to be wrong than the sensor contradicting it. Zero - /// never forces an update, which leaves `--gate-inflation` as the only way back. + /// Only used together with `--gate-confidence`. With the default 5, four measurements in + /// a row are rejected and the fifth is forced through. A belief contradicted this many + /// times running is likelier to be wrong than the sensor contradicting it. Zero never + /// forces an update, which leaves `--gate-inflation` as the only way back; 1 is refused, + /// since it would force every measurement and so be no gate at all. #[arg(long, value_name = "COUNT", default_value_t = DEFAULT_FORCED_UPDATE_AFTER)] gate_force_after: usize, From 2f798e71189255eca04bfaca1f6eafb40243a18c Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:18:03 -0400 Subject: [PATCH 17/26] Remove a dead assert from matrix_square_root matrix_square_root returns StrapdownError::NotSquare for a non-square matrix and then asserted the same condition. The assert could never fire, contradicted the function's "Does not panic" section, and was the zero-panic policy's one surviving assert! in library code. Co-Authored-By: Claude Opus 5.5 --- core/src/linalg.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/core/src/linalg.rs b/core/src/linalg.rs index e2cef7b9..7d909627 100644 --- a/core/src/linalg.rs +++ b/core/src/linalg.rs @@ -55,10 +55,6 @@ pub fn matrix_square_root(matrix: &DMatrix) -> Result, Strapdo }); } - assert!( - matrix.is_square(), - "matrix_square_root: matrix must be square" - ); // 1) Symmetrize to kill round-off asymmetry let p = symmetrize(matrix); // 2) Equilibrated Cholesky (fast path) From 4aa4c7b469574ee3e9d937fcdc74bda289d9da6d Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:18:03 -0400 Subject: [PATCH 18/26] Correct core docs that went stale behind later fixes - EkfConfig::imu_quality said the value was inert on the EKF because its Jacobian had no nav-bias block (#394). The EKF predict now widens F with widen_with_imu_bias_coupling, so the bias prior matters; the doc says so. - InsEngineConfig::initial_covariance_diagonal said None uses initialize_eskf's defaults. Position, velocity and attitude match, but the engine's DEFAULT_INITIAL_COVARIANCE keeps fixed 1e-6/1e-8 bias variances where initialize_eskf derives them from imu_quality. Both docs now say that; the values are unchanged, since moving them would move the examples' output. - IMUQuality::velocity_process_noise said the filters propagate with "no internal dt scaling". Since #374 every filter takes a density and forms Q = q dt, so the doc now warns that passing this per-step variance to a filter would apply dt twice, and gives the density. - Four kalman.rs doctests built filters with P0 = 1e-6 on every state and Q = 1e-9, the #308 units trap: 1e-6 rad^2 is a 6 km horizontal sigma. They now use per-state units, the IMU grade's bias prior and DEFAULT_PROCESS_NOISE_DENSITY, and still pass. - linearize.rs said altitude is positive down in NED; it is height above the ellipsoid, positive up in both frames. body_rotation_vector_to_euler_jacobian said callers fall back to differencing at gimbal lock; its one caller, the ESKF update, zeroes the attitude columns instead, for the reason its own comment gives. - RbpfConfig::horizontal_process_noise_std_m and the five conf/rbpf_*.toml comments said 1 m/sqrt(s) held the reference recording to 3.8 m. That was the first measurement, before later particle-filter fixes; the current accuracy baseline (real_rbpf_slice__rbpf) is 3.235 m, which book/src/filters/rbpf.md already gives. Comments only in conf/. Co-Authored-By: Claude Opus 5.5 --- conf/rbpf_both.toml | 4 +-- conf/rbpf_degraded.toml | 4 +-- conf/rbpf_grav.toml | 4 +-- conf/rbpf_mag.toml | 4 +-- conf/rbpf_truth.toml | 4 +-- core/src/engine.rs | 13 ++++++--- core/src/kalman.rs | 64 +++++++++++++++++++++++++++++++++++------ core/src/lib.rs | 13 ++++++--- core/src/linearize.rs | 9 ++++-- core/src/rbpf.rs | 4 ++- core/src/sim.rs | 13 ++++----- 11 files changed, 100 insertions(+), 36 deletions(-) diff --git a/conf/rbpf_both.toml b/conf/rbpf_both.toml index 390d397b..85852339 100644 --- a/conf/rbpf_both.toml +++ b/conf/rbpf_both.toml @@ -39,8 +39,8 @@ attitude_init_std_rad = 0.1 velocity_process_noise_std_mps = 1e-3 attitude_process_noise_std_rad = 0.01 # Random walk on the sampled horizontal position, m/sqrt(s). The paper's eq. 19 is zero, which -# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.8 m -# at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. +# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.2 m +# horizontal RMSE at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. horizontal_process_noise_std_m = [1.0, 1.0] # Third-order barometer loop in the mechanization, with the paper's two barometer states. The # paper gives no gains: the loop places its three poles at -1/tau (Titterton & Weston). The diff --git a/conf/rbpf_degraded.toml b/conf/rbpf_degraded.toml index 90fce1d2..60c96769 100644 --- a/conf/rbpf_degraded.toml +++ b/conf/rbpf_degraded.toml @@ -39,8 +39,8 @@ attitude_init_std_rad = 0.1 velocity_process_noise_std_mps = 1e-3 attitude_process_noise_std_rad = 0.01 # Random walk on the sampled horizontal position, m/sqrt(s). The paper's eq. 19 is zero, which -# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.8 m -# at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. +# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.2 m +# horizontal RMSE at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. horizontal_process_noise_std_m = [1.0, 1.0] # Third-order barometer loop in the mechanization, with the paper's two barometer states. The # paper gives no gains: the loop places its three poles at -1/tau (Titterton & Weston). The diff --git a/conf/rbpf_grav.toml b/conf/rbpf_grav.toml index 326f9b8f..d2923524 100644 --- a/conf/rbpf_grav.toml +++ b/conf/rbpf_grav.toml @@ -38,8 +38,8 @@ attitude_init_std_rad = 0.1 velocity_process_noise_std_mps = 1e-3 attitude_process_noise_std_rad = 0.01 # Random walk on the sampled horizontal position, m/sqrt(s). The paper's eq. 19 is zero, which -# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.8 m -# at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. +# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.2 m +# horizontal RMSE at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. horizontal_process_noise_std_m = [1.0, 1.0] # Third-order barometer loop in the mechanization, with the paper's two barometer states. The # paper gives no gains: the loop places its three poles at -1/tau (Titterton & Weston). The diff --git a/conf/rbpf_mag.toml b/conf/rbpf_mag.toml index d1880c46..44d1b40e 100644 --- a/conf/rbpf_mag.toml +++ b/conf/rbpf_mag.toml @@ -38,8 +38,8 @@ attitude_init_std_rad = 0.1 velocity_process_noise_std_mps = 1e-3 attitude_process_noise_std_rad = 0.01 # Random walk on the sampled horizontal position, m/sqrt(s). The paper's eq. 19 is zero, which -# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.8 m -# at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. +# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.2 m +# horizontal RMSE at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. horizontal_process_noise_std_m = [1.0, 1.0] # Third-order barometer loop in the mechanization, with the paper's two barometer states. The # paper gives no gains: the loop places its three poles at -1/tau (Titterton & Weston). The diff --git a/conf/rbpf_truth.toml b/conf/rbpf_truth.toml index 816ab5a9..d5c7c891 100644 --- a/conf/rbpf_truth.toml +++ b/conf/rbpf_truth.toml @@ -39,8 +39,8 @@ attitude_init_std_rad = 0.1 velocity_process_noise_std_mps = 1e-3 attitude_process_noise_std_rad = 0.01 # Random walk on the sampled horizontal position, m/sqrt(s). The paper's eq. 19 is zero, which -# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.8 m -# at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. +# diverges with GNSS-rate fixes on this data (17 km on the reference recording, against 3.2 m +# horizontal RMSE at 1 m/sqrt(s)); [0.0, 0.0] reproduces it. horizontal_process_noise_std_m = [1.0, 1.0] # Third-order barometer loop in the mechanization, with the paper's two barometer states. The # paper gives no gains: the loop places its three poles at -1/tau (Titterton & Weston). The diff --git a/core/src/engine.rs b/core/src/engine.rs index 5652404a..424cd236 100644 --- a/core/src/engine.rs +++ b/core/src/engine.rs @@ -120,8 +120,10 @@ pub struct InsEngineConfig { /// Process noise covariance diagonal, 15 elements. `None` uses /// [`DEFAULT_PROCESS_NOISE_DENSITY`]. pub process_noise_diagonal: Option>, - /// Initial error-state covariance diagonal, 15 elements. `None` uses the same defaults - /// as [`crate::sim::initialize_eskf`]. + /// Initial error-state covariance diagonal, 15 elements. `None` uses the engine's own + /// default: [`crate::sim::initialize_eskf`]'s position, velocity and attitude variances, + /// but fixed IMU-bias variances of `1e-6` (m/s^2)^2 and `1e-8` (rad/s)^2, where + /// `initialize_eskf` derives them from its `imu_quality`. pub initial_covariance_diagonal: Option>, /// Initial accelerometer bias estimate, m/s^2, body frame. pub initial_accel_bias: [f64; 3], @@ -144,8 +146,11 @@ impl Default for InsEngineConfig { /// Default initial error-state covariance diagonal for the 15-state ESKF. /// -/// Matches [`crate::sim::initialize_eskf`]: position and velocity error variances, then -/// attitude, then accelerometer and gyro bias. +/// Position, velocity and attitude match [`crate::sim::initialize_eskf`]'s defaults. The +/// accelerometer and gyro bias entries do not: they are the fixed `1e-6`/`1e-8` that +/// `initialize_eskf` used before it took its bias prior from +/// [`crate::IMUQuality::initial_bias_covariance`]. Left as they are because moving them would +/// move every engine result, the examples' included. /// /// The position block is taken from that function's own constants rather than copied as /// literals. The copy it replaces read `1e-6, 1e-6, 1e-4` -- correctly *labelled* diff --git a/core/src/kalman.rs b/core/src/kalman.rs index a9546584..3459ccd9 100644 --- a/core/src/kalman.rs +++ b/core/src/kalman.rs @@ -1162,11 +1162,23 @@ impl NavigationFilter for UnscentedKalmanFilter { /// is_enu: true, /// }; /// +/// // P0 in each state's own units: position in rad^2, rad^2, m^2 (#308), then velocity in +/// // (m/s)^2, attitude in rad^2 and the IMU grade's bias prior. +/// let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; +/// let mut covariance = vec![horizontal_std_rad.powi(2), horizontal_std_rad.powi(2), 100.0]; +/// covariance.extend([1e-3; 3]); +/// covariance.extend([1e-5; 3]); +/// covariance.extend(strapdown::IMUQuality::Consumer.initial_bias_covariance()); +/// // Q is a density, a variance per second: the filter forms Q_k = q * dt itself (#374). +/// let process_noise = DMatrix::from_diagonal(&nalgebra::DVector::from_row_slice( +/// &strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY, +/// )); +/// /// let mut ekf = ExtendedKalmanFilter::new( /// &initial_state, /// &[0.0; 6], // IMU biases (3 accel + 3 gyro) -/// vec![1e-6; 15], // Initial covariance diagonal -/// DMatrix::from_diagonal(&nalgebra::DVector::from_vec(vec![1e-9; 15])), // Process noise +/// covariance, +/// process_noise, /// true, // use_biases /// ); /// @@ -1262,11 +1274,23 @@ impl ExtendedKalmanFilter { /// use nalgebra::DMatrix; /// /// let initial_state = InitialState::default(); + /// // P0 in each state's own units: position in rad^2, rad^2, m^2 (#308), then velocity in + /// // (m/s)^2, attitude in rad^2 and the IMU grade's bias prior. + /// let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; + /// let mut covariance = vec![horizontal_std_rad.powi(2), horizontal_std_rad.powi(2), 100.0]; + /// covariance.extend([1e-3; 3]); + /// covariance.extend([1e-5; 3]); + /// covariance.extend(strapdown::IMUQuality::Consumer.initial_bias_covariance()); + /// // Q is a density, a variance per second: the filter forms Q_k = q * dt itself (#374). + /// let process_noise = DMatrix::from_diagonal(&nalgebra::DVector::from_row_slice( + /// &strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY, + /// )); + /// /// let ekf = ExtendedKalmanFilter::new( /// &initial_state, /// &[0.0; 6], - /// vec![1e-6; 15], - /// DMatrix::from_diagonal(&nalgebra::DVector::from_vec(vec![1e-9; 15])), + /// covariance, + /// process_noise, /// true, /// ); /// ``` @@ -1804,11 +1828,23 @@ impl NavigationFilter for ExtendedKalmanFilter { /// is_enu: true, /// }; /// +/// // P0 in each state's own units: position in rad^2, rad^2, m^2 (#308), then velocity in +/// // (m/s)^2, attitude in rad^2 and the IMU grade's bias prior. +/// let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; +/// let mut covariance = vec![horizontal_std_rad.powi(2), horizontal_std_rad.powi(2), 100.0]; +/// covariance.extend([1e-3; 3]); +/// covariance.extend([1e-5; 3]); +/// covariance.extend(strapdown::IMUQuality::Consumer.initial_bias_covariance()); +/// // Q is a density, a variance per second: the filter forms Q_k = q * dt itself (#374). +/// let process_noise = DMatrix::from_diagonal(&nalgebra::DVector::from_row_slice( +/// &strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY, +/// )); +/// /// let mut eskf = ErrorStateKalmanFilter::new( /// &initial_state, /// &[0.0; 6], // Initial IMU biases (3 accel + 3 gyro) -/// vec![1e-6; 15], // Initial error covariance diagonal -/// DMatrix::from_diagonal(&nalgebra::DVector::from_vec(vec![1e-9; 15])), // Process noise +/// covariance, +/// process_noise, /// ); /// /// // Predict with IMU data @@ -2043,11 +2079,23 @@ impl ErrorStateKalmanFilter { /// use nalgebra::DMatrix; /// /// let initial_state = InitialState::default(); + /// // P0 in each state's own units: position in rad^2, rad^2, m^2 (#308), then velocity in + /// // (m/s)^2, attitude in rad^2 and the IMU grade's bias prior. + /// let horizontal_std_rad = 10.0 * strapdown::earth::METERS_TO_RADIANS; + /// let mut covariance = vec![horizontal_std_rad.powi(2), horizontal_std_rad.powi(2), 100.0]; + /// covariance.extend([1e-3; 3]); + /// covariance.extend([1e-5; 3]); + /// covariance.extend(strapdown::IMUQuality::Consumer.initial_bias_covariance()); + /// // Q is a density, a variance per second: the filter forms Q_k = q * dt itself (#374). + /// let process_noise = DMatrix::from_diagonal(&nalgebra::DVector::from_row_slice( + /// &strapdown::sim::DEFAULT_PROCESS_NOISE_DENSITY, + /// )); + /// /// let eskf = ErrorStateKalmanFilter::new( /// &initial_state, /// &[0.0; 6], - /// vec![1e-6; 15], - /// DMatrix::from_diagonal(&nalgebra::DVector::from_vec(vec![1e-9; 15])), + /// covariance, + /// process_noise, /// ); /// ``` pub fn new( diff --git a/core/src/lib.rs b/core/src/lib.rs index ab01f52f..2c42e9b0 100644 --- a/core/src/lib.rs +++ b/core/src/lib.rs @@ -509,10 +509,15 @@ impl IMUQuality { /// \\(K^2 \\, dt / 3600\\). /// /// # Units - /// Returns (m/s)^2 -- a variance, not a spectral density. Both filters propagate as - /// \\(P_{k+1} = F `P_k` F^T + Q\\) with no internal `dt` scaling, so what they consume is - /// the per-step increment this returns. That is why `dt` is a parameter: the same IMU - /// grade yields a different `Q` at 100 Hz than at 1 Hz. + /// Returns (m/s)^2 -- the per-step **variance** \\(Q_k\\) accumulated over `dt`, not a + /// spectral density. That is why `dt` is a parameter: the same IMU grade yields a + /// different `Q_k` at 100 Hz than at 1 Hz. + /// + /// Do not hand it to a filter as its process noise. Since #374 every filter here takes a + /// **density** -- a variance per second, as in + /// [`DEFAULT_PROCESS_NOISE_DENSITY`](crate::sim::DEFAULT_PROCESS_NOISE_DENSITY) -- and forms + /// \\(Q_k = q \\Delta t\\) itself, so passing this would apply `dt` twice. The density + /// for a filter's velocity rows is this value divided by `dt`, i.e. `K^2 / 3600`. /// /// Consistent with the per-sample sigma used by the synthetic IMU generator in /// [`crate::sim`], which scales the same coefficient by `sqrt(sample_rate_hz / 3600)`. diff --git a/core/src/linearize.rs b/core/src/linearize.rs index 6f6a145e..642efaf6 100644 --- a/core/src/linearize.rs +++ b/core/src/linearize.rs @@ -13,7 +13,8 @@ //! ``` //! where: //! - `lat`, `lon`: latitude and longitude in radians -//! - `alt`: altitude in meters (positive up in ENU, positive down in NED) +//! - `alt`: altitude in meters above the ellipsoid, positive up in **both** ENU and NED; it is +//! not a "down" coordinate (only the vertical *velocity* changes sign with the frame) //! - `v_n`, `v_e`, `v_d`: velocity components in m/s (NED/ENU local-level frame) //! - `roll`, `pitch`, `yaw`: Euler angles in radians (XYZ rotation sequence) //! @@ -466,7 +467,11 @@ pub fn attitude_reset_jacobian(delta_theta: &Vector3) -> nalgebra::Matrix3< /// # Returns /// /// `None` at gimbal lock, where the Euler chart stops being a chart and no finite matrix is -/// the right answer. Callers fall back to differencing the expected measurement directly. +/// the right answer. The one caller, the ESKF's measurement update, then **zeroes** the +/// attitude columns of its measurement Jacobian rather than approximating them: a model that +/// observes only attitude gets a zero gain and the update is a no-op, and a model with other +/// columns keeps them. Finite-differencing the measurement was an earlier fallback and was +/// dropped, because near 90 degrees of pitch the quotients are finite but unbounded. /// /// # Example /// diff --git a/core/src/rbpf.rs b/core/src/rbpf.rs index 52d5d42d..c880c150 100644 --- a/core/src/rbpf.rs +++ b/core/src/rbpf.rs @@ -388,7 +388,9 @@ pub struct RbpfConfig { /// the particles' spread, and resampling on a metre-level fix destroys that spread. On the /// reference recording the conditional velocity sigma fell to millimetres per second, /// GNSS velocity fixes stopped correcting anything, and the solution ran 17 km off; 1 m per - /// root-second held it to 3.8 m. The configurations under `conf/` set that, and so does + /// root-second holds it to 3.2 m horizontal RMSE (the `real_rbpf_slice__rbpf` row of + /// `core/tests/perf_baseline.json`; the 3.8 m first measured predates later particle-filter + /// fixes). The configurations under `conf/` set that, and so does /// `strapdown-sim` by default ([`crate::sim::DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M`]); /// only this library-level default keeps the paper's value. pub horizontal_process_noise_std_m: Vector2, diff --git a/core/src/sim.rs b/core/src/sim.rs index 799dd9e1..448e03f0 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -3911,13 +3911,12 @@ pub struct EkfConfig { /// different answer and none of them modelled any hardware -- see that method for the /// measurement, and for why it made the UKF-versus-ESKF comparison in #371 meaningless. /// - /// **On this filter the value is currently inert**, and measurably so: any value produces - /// bit-identical output, because the EKF's state-transition Jacobian has no - /// $\partial(\text{nav})/\partial(\text{bias})$ block, so `P[0..9, 9..15]` starts at zero - /// and stays there and the gain over the bias rows is always zero (#394). It is set - /// correctly here anyway: the field is what the filter *claims*, the claim should be true - /// whether or not anything reads it today, and #394's fix makes it load-bearing without - /// touching this line. + /// It matters on this filter. The EKF's predict widens its state-transition Jacobian with + /// the $\partial(\text{nav})/\partial(\text{bias})$ blocks + /// ([`crate::linearize::widen_with_imu_bias_coupling`]), so the navigation-bias + /// cross-covariance grows from this prior and the gain reaches the bias rows. Until #394 + /// that block was missing, `P[0..9, 9..15]` stayed zero and this value changed nothing; + /// it was set correctly then so that the fix would make it load-bearing as it is now. pub imu_quality: crate::IMUQuality, /// Local-level frame of the records: `false` (the default) is NED, `true` is ENU. /// From 7baddfe44d00c87f615174ec4d797fba922b29a3 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:19:20 -0400 Subject: [PATCH 19/26] Rerun the two tutorial examples and print what they print now tutorial-basic.md showed basic_ins ending 0.00 m from truth with 3.01/2.32/1.08 m position sigmas; the example now prints 0.10 m and 0.80/0.78/1.04 m. The page read the 3 m as the filter being unable to beat its aiding source. That no longer holds: having fused a fix and a velocity fix every second, the filter reports less than any one fix advertises, which is what averaging repeated fixes should do. The prose now says so, and notes that the page used to show otherwise. tutorial-gps-degradation.md showed a 49.4 m peak, a 0.2987 deg pitch error and a 0.0511 m/s^2 gravity leak; gnss_outage now prints 54.0 m, 0.2879 deg and 0.0493 m/s^2, and its table rows moved with them (0.2 m before the outage, 0.1 m after recovery). The explanation -- the filter absorbs the accelerometer bias into pitch, and the tilt's gravity leak nearly cancels it -- still holds and is kept, with the ratio to the 360 m textbook figure updated to "nearly seven". Co-Authored-By: Claude Opus 5.5 --- book/src/examples/tutorial-basic.md | 16 +++++++----- book/src/examples/tutorial-gps-degradation.md | 25 ++++++++++--------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/book/src/examples/tutorial-basic.md b/book/src/examples/tutorial-basic.md index 5adfd914..09ed4392 100644 --- a/book/src/examples/tutorial-basic.md +++ b/book/src/examples/tutorial-basic.md @@ -80,13 +80,17 @@ altitude fixed. If a first integration produces those two symptoms together, che ```text start: NavSolution(t: 0.000 s, 39.9500000 deg, -75.1600000 deg, 12.00 m, ...) -after 60 s: NavSolution(t: 60.000 s, 39.9553996 deg, -75.1600000 deg, 12.14 m, ...) -travelled 600.0 m north; expected 600.0 m (difference 0.00 m) -position uncertainty (1-sigma): 3.01 m north, 2.32 m east, 1.08 m vertical +after 60 s: NavSolution(t: 60.000 s, 39.9554004 deg, -75.1600000 deg, 12.11 m, ...) +travelled 600.1 m north; expected 600.0 m (difference 0.10 m) +position uncertainty (1-sigma): 0.80 m north, 0.78 m east, 1.04 m vertical ``` -600 m in 60 s at 10 m/s, and the reported north uncertainty settles near the 3 m accuracy the -synthetic fixes advertise, which is what a correctly-tuned filter should do: with no vehicle -dynamics to exploit it cannot do better than its aiding source. +600 m in 60 s at 10 m/s, to a tenth of a metre. The reported uncertainty is well inside the 3 m +horizontal and 5 m vertical the fixes advertise, and that is correct rather than optimistic: the +filter has fused sixty position fixes and sixty 0.2 m/s velocity fixes through a motion model +that lets the vehicle wander very little between them, and the combination is tighter than any +single fix. (This page used to show about 3 m here and read it as the filter being unable to do +better than its aiding source; the current code prints the numbers above, and a filter that +averages repeated fixes is expected to beat each one.) Next: [Tutorial: GPS Degradation](./tutorial-gps-degradation.md). diff --git a/book/src/examples/tutorial-gps-degradation.md b/book/src/examples/tutorial-gps-degradation.md index c0c386f3..89ff59b8 100644 --- a/book/src/examples/tutorial-gps-degradation.md +++ b/book/src/examples/tutorial-gps-degradation.md @@ -15,15 +15,16 @@ consumer MEMS part after turn-on. ```text t (s) error (m) GNSS - 60 0.0 ok - 70 0.2 denied - 100 2.0 denied - 140 14.9 denied - 180 49.4 denied - 190 0.0 ok + 60 0.2 ok + 70 0.4 denied + 100 2.6 denied + 140 16.8 denied + 180 54.0 denied + 190 0.1 ok ``` -Flat while aided, growing while coasting, and back to zero within a single fix of recovery. +Flat while aided, growing while coasting, and back to a decimetre within a single fix of +recovery. (The example prints every 10 s; these are a selection of its rows.) The growth is quadratic because a constant acceleration error integrates twice into position. ## Why the drift is smaller than the textbook figure @@ -34,19 +35,19 @@ Double-integrating the bias over the outage predicts 360 m: 0.5 × 0.05 m/s² × (120 s)² = 360 m ``` -The observed error is 49.4 m, seven times smaller. The reason is worth reading off the filter +The observed error is 54.0 m, nearly seven times smaller. The reason is worth reading off the filter state rather than guessing at, and the example prints it: ```text accelerometer bias present: 0.0500 m/s^2 (forward axis) bias the filter estimated: 0.0001 m/s^2 <- it barely moved -pitch error at outage start: 0.2987 deg -gravity that tilt leaks forward: 0.0511 m/s^2 <- nearly cancels the bias +pitch error at outage start: 0.2879 deg +gravity that tilt leaks forward: 0.0493 m/s^2 <- nearly cancels the bias ``` The filter did **not** estimate the bias. It absorbed it into *attitude* instead: it believes -the vehicle is pitched nose-up by three tenths of a degree, and the gravity that tilt leaks -into the forward axis very nearly cancels the bias. +the vehicle is pitched by just under three tenths of a degree, and the gravity that tilt leaks +into the forward axis, 0.0493 m/s², very nearly cancels the 0.05 m/s² bias. That is not a defect. A constant forward specific-force bias and a small pitch error produce the same horizontal acceleration signature, so position and velocity aiding cannot separate From acb91717a27d534f5459d5e3e7af685450f04613 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:22:02 -0400 Subject: [PATCH 20/26] Give the ESKF's state count correctly in the core README core/README.md called cl's default filter "the 15-state ESKF". With the barometric bias estimated by default it carries sixteen error states unless --no-estimate-baro-bias is given, as the CLI help now says. Co-Authored-By: Claude Opus 5.5 --- core/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/README.md b/core/README.md index d0509b77..6ccabcd6 100644 --- a/core/README.md +++ b/core/README.md @@ -55,7 +55,7 @@ The toolbox is designed for research, teaching, and development purposes and aim `strapdown-rs` is intended to be both a source code library included into your INS software and simulation environment as well as very light-weight INS simulator. The library provides a set of modules modeling the WGS84 Earth ellipsoid, a common 9-state strapdown forward mechanization, and a set of navigation filters for estimating position, velocity, and orientation from inertial measurement unit (IMU) data. -The simulation program provides a simple command line interface for running various configurations of the INS. It runs dead reckoning (`dr`), closed-loop Kalman filtering (`cl`: the 15-state ESKF by default, with the UKF and EKF selectable) and the particle filter (`pf`). It can simulate various scenarios such as intermittent GPS, GPS degradation, and more. The simulation is designed to be easy to use and provides a simple API for generating datasets for further navigation processing or research. +The simulation program provides a simple command line interface for running various configurations of the INS. It runs dead reckoning (`dr`), closed-loop Kalman filtering (`cl`: the error-state Kalman filter by default -- 15 error states plus a barometric bias state unless `--no-estimate-baro-bias` -- with the UKF and EKF selectable) and the particle filter (`pf`). It can simulate various scenarios such as intermittent GPS, GPS degradation, and more. The simulation is designed to be easy to use and provides a simple API for generating datasets for further navigation processing or research. `strapdown-sim` includes built-in logging capabilities using the Rust `log` crate. Library functions use log macros for diagnostic output that can be captured by any logging backend. From aa587f88023b40bcce12e46945665c76388c0c0a Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:26:19 -0400 Subject: [PATCH 21/26] Remove earth::gravitation, which double-counted the centrifugal term, and nav-types with it gravitation added the Earth's centrifugal acceleration to gravity(), which is Somigliana normal gravity and already includes it. The vertical component was therefore about 0.034 m/s^2 too large at the equator, and the unit test pinned the wrong value (GE + 0.0339). The doc also called the result ENU with gravity along -Z, while the function returned +g on Z. Nothing called it: the mechanization uses gravity() along the vertical, which is the correct normal-gravity vector, so there is nothing to repair it into. It was also the only code in the workspace that used nav-types, so that dependency and its transitive crates go too. The JOSS paper said the mechanization was 'built on nav-types', which this makes plainly untrue; the claim and its bibliography entry are removed in the same commit so the paper never disagrees with the code it ships with. The crate and module docs that credited nav-types with the coordinate conversions are corrected as well. Co-Authored-By: Claude Opus 5.5 --- Cargo.lock | 60 ++--------------- book/src/resources/publications.md | 1 - core/Cargo.toml | 1 - core/src/earth.rs | 104 ++--------------------------- core/src/lib.rs | 3 +- papers/joss/paper.bib | 7 -- papers/joss/paper.md | 3 +- 7 files changed, 11 insertions(+), 168 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 24126e44..23b953fd 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1239,22 +1239,6 @@ dependencies = [ "simd-adler32", ] -[[package]] -name = "nalgebra" -version = "0.32.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b5c17de023a86f59ed79891b2e5d5a94c705dbe904a5b5c9c952ea6221b03e4" -dependencies = [ - "approx", - "matrixmultiply", - "nalgebra-macros 0.2.2", - "num-complex", - "num-rational", - "num-traits", - "simba 0.8.1", - "typenum", -] - [[package]] name = "nalgebra" version = "0.34.2" @@ -1281,25 +1265,14 @@ dependencies = [ "glam 0.31.1", "glam 0.32.1", "matrixmultiply", - "nalgebra-macros 0.3.0", + "nalgebra-macros", "num-complex", "num-rational", "num-traits", - "simba 0.9.1", + "simba", "typenum", ] -[[package]] -name = "nalgebra-macros" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.117", -] - [[package]] name = "nalgebra-macros" version = "0.3.0" @@ -1311,15 +1284,6 @@ dependencies = [ "syn 2.0.117", ] -[[package]] -name = "nav-types" -version = "0.5.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "79beb749c537c9cccddfa3039ace093404d4bb42c28313c28cac0342f3d4fd23" -dependencies = [ - "nalgebra 0.32.6", -] - [[package]] name = "ndarray" version = "0.17.2" @@ -1978,19 +1942,6 @@ version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" -[[package]] -name = "simba" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" -dependencies = [ - "approx", - "num-complex", - "num-traits", - "paste", - "wide", -] - [[package]] name = "simba" version = "0.9.1" @@ -2036,8 +1987,7 @@ dependencies = [ "log", "mcap", "memmap2", - "nalgebra 0.34.2", - "nav-types", + "nalgebra", "netcdf", "rand", "rand_distr", @@ -2059,7 +2009,7 @@ dependencies = [ "assert_approx_eq", "chrono", "log", - "nalgebra 0.34.2", + "nalgebra", "netcdf", "strapdown-core", "world_magnetic_model", @@ -2073,7 +2023,7 @@ dependencies = [ "clap", "env_logger", "log", - "nalgebra 0.34.2", + "nalgebra", "plotters", "rand", "rayon", diff --git a/book/src/resources/publications.md b/book/src/resources/publications.md index f7f33069..7b7f9859 100644 --- a/book/src/resources/publications.md +++ b/book/src/resources/publications.md @@ -49,5 +49,4 @@ The geophysical-navigation work that `strapdown-geonav` supports (see | Sensor Logger (the app the input format comes from) | | | World Magnetic Model | | | `world_magnetic_model` crate (the WMM implementation used) | | -| `nav-types` crate (geodetic and ECEF types used by `earth`) | | | GMT remote datasets (the source of the gravity and magnetic maps) | | diff --git a/core/Cargo.toml b/core/Cargo.toml index 3e9b4c5e..3104dcca 100644 --- a/core/Cargo.toml +++ b/core/Cargo.toml @@ -70,7 +70,6 @@ full = ["clap", "hdf5", "netcdf", "mcap"] [dependencies] angle = "0.5.0" -nav-types = "0.5.2" serde_json = "1.0" serde_yaml = "0.9" toml = "0.7" diff --git a/core/src/earth.rs b/core/src/earth.rs index 3c25d3b9..0fb1879d 100644 --- a/core/src/earth.rs +++ b/core/src/earth.rs @@ -4,11 +4,9 @@ //! geophysical features (gravity and magnetic field). The Earth is modeled as an ellipsoid //! (WGS84) with a semi-major axis and a semi-minor axis. The Earth's gravity is modeled as //! a function of the latitude and altitude using the Somigliana method. The Earth's -//! rotation rate is also included in this module. This module relies on the `nav-types` -//! crate for the coordinate types and conversions, but provides additional functionality -//! for calculating rotations for the strapdown navigation filters. This permits the -//! transformation of additional quantities (velocity, acceleration, etc.) between the -//! Earth-centered Earth-fixed (ECEF) frame and the local-level frame. +//! rotation rate is also included in this module, along with the rotations between the +//! Earth-centered Earth-fixed (ECEF) frame and the local-level frame that the strapdown +//! navigation filters need to transform velocity, acceleration and similar quantities. //! //! # Coordinate Systems //! The WGS84 ellipsoidal model is the primary model used for the Earth's shape. This crate @@ -20,21 +18,13 @@ //! frame is a right-handed Cartesian coordinate system with the origin at the sensor's //! center of mass. The body frame is defined by the sensor's orientation. //! -//! For basic positional conversions, the [`nav-types`](https://crates.io/crates/nav-types) -//! crate is used. This crate provides the `WGS84` and `ECEF` types for representing the -//! Earth's position in geodetic and Cartesian coordinates, respectively. The `nav-types` -//! crate also provides the necessary conversions between the two coordinate systems. -//! //! # Rotation Functions -//! The rotations needed for the strapdown navigation filters are not directly supported -//! by the `nav-types` crate. These functions provide the necessary rotations that are +//! These functions provide the rotations that are //! primarily used for projecting the velocity and acceleration vectors. The rotations //! are primarily used to convert between the ECEF and local-level frames. The rotations //! from the local level frame to the body frame can be taken care of by the `nalgebra` //! crate, which provides the necessary rotation matrices using the Rotation3 type. -use crate::{wrap_latitude, wrap_to_180}; use nalgebra::{Matrix3, Vector3}; -use nav_types::{ECEF, WGS84}; use world_magnetic_model::GeomagneticField; /// Earth's rotation rate rad/s ($\omega_{ie}$) @@ -490,52 +480,6 @@ pub fn gravity(latitude: &f64, altitude: &f64) -> f64 { / (1.0 - ECCENTRICITY_SQUARED * sin_lat * sin_lat).sqrt(); g0 - 3.08e-6 * altitude } -/// Calculate the gravitational force vector in the local-level frame including rotational effects. -/// -/// The [gravity model](https://en.wikipedia.org/wiki/Gravity_of_Earth) is based on the [Somigliana -/// method](https://en.wikipedia.org/wiki/Theoretical_gravity#Somigliana_equation), which models -/// the Earth's gravity as a function of the latitude and altitude. The gravity model is used to -/// calculate the gravitational force vector in the local-level frame. This is then combined -/// with the rotational effects of the Earth to calculate the effective gravity vector: the -/// centrifugal acceleration is added as a vector to [`gravity`]'s vertical scalar. -/// -/// Because [`gravity`] is Somigliana *normal* gravity, which already includes the centrifugal -/// magnitude, the vertical component here counts it twice -- about 0.034 m/s^2 at the equator, -/// which the `gravitation` unit test pins. Nothing in the mechanization calls this function. -/// -/// *Note:* This function uses the ENU convention, thus gravity acts along the negative Z-axis -/// (downward) in the local-level frame. -/// -/// # Arguments -/// - `latitude` - The WGS84 latitude in degrees -/// - `longitude` - The WGS84 longitude in degrees -/// - `altitude` - The WGS84 altitude in meters -/// -/// # Returns -/// The gravitational force vector in m/s^2 in the local-level frame -/// -/// # Example -/// ```rust -/// use strapdown::earth; -/// let latitude: f64 = 45.0; -/// let longitude: f64 = 90.0; -/// let altitude: f64 = 1000.0; -/// let grav = earth::gravitation(&latitude, &longitude, &altitude); -/// ``` -pub fn gravitation(latitude: &f64, longitude: &f64, altitude: &f64) -> Vector3 { - let latitude = wrap_latitude(*latitude); - let longitude = wrap_to_180(*longitude); - let wgs84: WGS84 = WGS84::from_degrees_and_meters(latitude, longitude, *altitude); - let ecef: ECEF = ECEF::from(wgs84); - // Get centrifugal terms in ECEF - let ecef_vec: Vector3 = Vector3::new(ecef.x(), ecef.y(), ecef.z()); - let omega_ie: Matrix3 = vector_to_skew_symmetric(&RATE_VECTOR); - // Get rotation and gravity in LLA - let rot: Matrix3 = ecef_to_lla(&latitude, &longitude); - let gravity: Vector3 = Vector3::new(0.0, 0.0, gravity(&latitude, altitude)); - // Calculate the effective gravity vector combining gravity and centrifugal terms - gravity + rot * omega_ie * omega_ie * ecef_vec -} /// Calculate the local gravity anomaly, in **milligal**, from IMU accelerometer measurements /// /// This function calculates the local gravity anomaly by comparing the observed gravity from the @@ -946,46 +890,6 @@ mod tests { assert_approx_eq!(grav, GE); } #[test] - fn gravitation() { - // test equatorial gravity - let latitude: f64 = 0.0; - let altitude: f64 = 0.0; - let grav: Vector3 = super::gravitation(&latitude, &0.0, &altitude); - assert_approx_eq!(grav[0], 0.0); - assert_approx_eq!(grav[1], 0.0); - assert_approx_eq!(grav[2], (GE + 0.0339), 1e-4); - // test polar gravity - let latitude: f64 = 90.0; - let grav: Vector3 = super::gravitation(&latitude, &0.0, &altitude); - assert_approx_eq!(grav[0], 0.0); - assert_approx_eq!(grav[1], 0.0); - assert_approx_eq!(grav[2], GP, 1e-2); - } - #[test] - fn gravitation_centrifugal_term_deflects_north() { - // `gravitation` is the only caller of `ecef_to_lla` left after #319, and the two - // cases above sit at the equator and the pole, where row 0 of `C_e^n` cannot be - // told apart from the pre-#319 version. Away from those, the centrifugal vector - // -w^2 (x, y, 0) picks up a North component through row 0: - // - // north = w^2 (r_e + h) sin(L) cos(L), east = 0 (exactly, at any longitude) - // - // This is the plumb-line deflection, ~0.017 m/s^2 at 45 degrees. The pre-#319 - // matrix put a longitude-dependent number here instead. - for longitude in [0.0_f64, 45.0, -122.0, 179.0] { - let latitude: f64 = 45.0; - let altitude: f64 = 1000.0; - let (_, r_e, _) = principal_radii(&latitude, &altitude); - let grav: Vector3 = super::gravitation(&latitude, &longitude, &altitude); - let expected_north: f64 = RATE.powi(2) - * (r_e + altitude) - * latitude.to_radians().sin() - * latitude.to_radians().cos(); - assert_approx_eq!(grav[0], expected_north, 1e-9); - assert_approx_eq!(grav[1], 0.0, 1e-12); - } - } - #[test] fn magnetic_radial_field() { // Using magnetic co-latitude [0, 180] let lat: f64 = 0.0; diff --git a/core/src/lib.rs b/core/src/lib.rs index 2c42e9b0..1671f011 100644 --- a/core/src/lib.rs +++ b/core/src/lib.rs @@ -14,8 +14,7 @@ //! [user guide](https://jbrodovsky.github.io/strapdown-rs/) covers the concepts and the //! `strapdown-sim` command-line tool. //! -//! This crate is primarily built off of three additional dependencies: -//! - [`nav-types`](https://crates.io/crates/nav-types): Provides basic coordinate types and conversions. +//! This crate is primarily built off of two additional dependencies: //! - [`nalgebra`](https://crates.io/crates/nalgebra): Provides the linear algebra tools for the filters. //! - [`rand`](https://crates.io/crates/rand) and [`rand_distr`](https://crates.io/crates/rand_distr): Provides random number generation for noise and simulation (primarily for particle filter methods). //! diff --git a/papers/joss/paper.bib b/papers/joss/paper.bib index e7464265..e01df989 100644 --- a/papers/joss/paper.bib +++ b/papers/joss/paper.bib @@ -27,13 +27,6 @@ @misc{wmm url = {https://www.ncei.noaa.gov/products/world-magnetic-model} } -@misc{nav-types, - author = {Nordmoen, Jørgen and Feuerstein, Marco}, - title = {nav-types: Easy to use navigation types for {Rust}}, - year = {2022}, - url = {https://github.com/nordmoen/nav-types} -} - @misc{gnss-ins-sim, author = {{Aceinna, Inc.}}, title = {gnss-ins-sim: {GNSS}/{INS} simulator for testing and prototyping navigation algorithms}, diff --git a/papers/joss/paper.md b/papers/joss/paper.md index e284b48a..8aea2c51 100644 --- a/papers/joss/paper.md +++ b/papers/joss/paper.md @@ -110,8 +110,7 @@ require no filter changes. All randomness is seeded. ## Functionality -- Strapdown mechanization on the WGS84 ellipsoid [@wgs84] with Somigliana gravity, built on - `nav-types` [@nav-types]. +- Strapdown mechanization on the WGS84 ellipsoid [@wgs84] with Somigliana gravity. - Aiding by: - GNSS position and velocity; - barometric altitude, with bias estimation; From 8cd664a0dae597910afe69eefc7a0a505891e6c1 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:33:38 -0400 Subject: [PATCH 22/26] Make the synthetic barometer and GNSS velocity independent sensors, and re-bless strapdown-sim syn modelled two sensors with no independent noise. - relativeAltitude was the noisy GNSS altitude minus its first value. The barometer shared the GNSS's noise draw, so on synthetic data it could aid nothing. - --baro-noise-std-pa reached only the pressure column, which no estimator reads, and pressure was written in pascals while TestDataRecord documents millibars. - speed and bearing were noise-free while speedAccuracy claimed the 2.5 m horizontal position noise, so velocity fixes were perfect but weighted as 2.5 m/s. Now: - relativeAltitude comes from the noisy pressure through the same isothermal atmosphere that expected_barometric_pressure uses, referenced to the first noisy reading as a phone's is. Its first-sample error is the constant barometric bias the filters estimate. - The default pressure noise is the sea-level equivalent of the 2.24 m the filters assume (BAROMETRIC_ALTITUDE_NOISE_M), about 27 Pa, not an unsourced 50 Pa (4.2 m). - pressure is written in hPa. - New --gnss-velocity-noise-mps (default 0.5) perturbs north/east velocity on its own RNG stream, so the IMU, GNSS position and barometer realizations are unchanged. It is written as speedAccuracy, with bearingAccuracy the angle that error subtends at the current speed. earth::barometric_altitude, needed for the inverse, mixed the lapse-rate and isothermal models and returned about -152 km for the pressure of 100 m. Its test only checked for a finite result, under a comment saying the formula looked wrong. It is now the exact inverse of expected_barometric_pressure, and the test round-trips it. Re-blessed. Only the six syn_* scenarios move; every real-data row is identical. Horizontal RMSE in syn_cruise_1hz falls from 2.60 m to 1.72 m, because velocity fixes now carry an honest 0.5 m/s. Vertical RMSE falls from 2.28 m to 1.52 m, because the barometer is now an independent aid. ESKF attitude error rises from 0.42 to 0.65 deg geodesic, because real velocity noise now reaches tilt. syn_dead_reckoning moves because dr takes its initial velocity from the first record's speed and bearing. Co-Authored-By: Claude Opus 5.5 --- book/src/development/baseline-tables.md | 32 ++--- core/src/earth.rs | 44 +++--- core/src/sim.rs | 157 +++++++++++++++++++-- core/tests/perf_baseline.json | 178 ++++++++++++------------ sim/src/main.rs | 12 +- 5 files changed, 284 insertions(+), 139 deletions(-) diff --git a/book/src/development/baseline-tables.md b/book/src/development/baseline-tables.md index b73c8d7a..4df5b3fa 100644 --- a/book/src/development/baseline-tables.md +++ b/book/src/development/baseline-tables.md @@ -13,12 +13,12 @@ overwrites this file, and the gate fails if it is stale. See #380. --> | `real_outage_60s__eskf` | 5,366 | 257.380 | 4.618 | 645.701 | 2,166.400 | 1.834 | -0.236 | 10.750 | 0.604 | | `real_degraded__ukf` | 5,366 | 50.700 | 40.636 | 88.053 | 140.311 | 13.878 | 0.438 | 2.487 | 0.787 | | `real_degraded__eskf` | 5,366 | 50.702 | 40.635 | 88.051 | 140.315 | 13.864 | 0.433 | 2.469 | 0.794 | -| `syn_cruise_1hz__ukf` | 15,000 | 2.600 | 2.214 | 4.354 | 6.784 | 2.279 | -0.685 | 0.581 | 0.304 | -| `syn_cruise_1hz__ekf` | 15,000 | 2.600 | 2.213 | 4.355 | 6.785 | 2.279 | -0.686 | 0.581 | 0.304 | -| `syn_cruise_1hz__eskf` | 15,000 | 2.600 | 2.179 | 4.397 | 6.785 | 2.154 | -0.549 | 0.559 | 0.262 | -| `syn_outage_60s__ukf` | 15,000 | 24.024 | 1.379 | 48.123 | 125.428 | 2.045 | -0.039 | 1.290 | 0.284 | -| `syn_outage_60s__eskf` | 15,000 | 23.444 | 1.406 | 55.373 | 112.796 | 1.872 | -0.229 | 1.346 | 0.234 | -| `syn_dead_reckoning` | 6,000 | 316.860 | 110.128 | 715.856 | 835.634 | 86.591 | -64.255 | 9.363 | 1.896 | +| `syn_cruise_1hz__ukf` | 15,000 | 1.717 | 1.334 | 3.043 | 4.377 | 1.518 | -0.781 | 0.358 | 0.179 | +| `syn_cruise_1hz__ekf` | 15,000 | 1.717 | 1.334 | 3.043 | 4.377 | 1.518 | -0.781 | 0.358 | 0.179 | +| `syn_cruise_1hz__eskf` | 15,000 | 1.734 | 1.343 | 3.034 | 5.588 | 1.450 | -0.724 | 0.358 | 0.163 | +| `syn_outage_60s__ukf` | 15,000 | 24.797 | 3.392 | 55.798 | 80.304 | 1.116 | -0.281 | 1.257 | 0.177 | +| `syn_outage_60s__eskf` | 15,000 | 28.656 | 3.390 | 66.482 | 95.768 | 1.104 | -0.396 | 1.453 | 0.139 | +| `syn_dead_reckoning` | 6,000 | 302.347 | 104.578 | 685.614 | 802.620 | 86.505 | -64.190 | 9.161 | 1.894 | | `real_rbpf_slice__rbpf` | 1,200 | 3.235 | 0.579 | 6.004 | 27.672 | 0.795 | -0.131 | 1.231 | 0.388 | ### Attitude @@ -33,11 +33,11 @@ overwrites this file, and the gate fails if it is stale. See #380. --> | `real_outage_60s__eskf` | 5,366 | 3.561 | 3.264 | 21.704 | 22.219 | | `real_degraded__ukf` | 5,366 | 3.497 | 3.005 | 24.300 | 24.725 | | `real_degraded__eskf` | 5,366 | 3.417 | 3.107 | 24.167 | 24.593 | -| `syn_cruise_1hz__ukf` | 15,000 | 0.298 | 0.251 | 0.156 | 0.420 | -| `syn_cruise_1hz__ekf` | 15,000 | 0.298 | 0.251 | 0.155 | 0.420 | -| `syn_cruise_1hz__eskf` | 15,000 | 0.291 | 0.254 | 0.155 | 0.416 | -| `syn_outage_60s__ukf` | 15,000 | 0.231 | 0.206 | 0.189 | 0.363 | -| `syn_outage_60s__eskf` | 15,000 | 0.222 | 0.249 | 0.248 | 0.416 | +| `syn_cruise_1hz__ukf` | 15,000 | 0.230 | 0.258 | 0.216 | 0.407 | +| `syn_cruise_1hz__ekf` | 15,000 | 0.230 | 0.258 | 0.233 | 0.417 | +| `syn_cruise_1hz__eskf` | 15,000 | 0.476 | 0.335 | 0.293 | 0.651 | +| `syn_outage_60s__ukf` | 15,000 | 0.184 | 0.289 | 0.395 | 0.523 | +| `syn_outage_60s__eskf` | 15,000 | 0.196 | 0.319 | 0.444 | 0.581 | | `syn_dead_reckoning` | 6,000 | 0.468 | 1.106 | 0.494 | 1.301 | | `real_rbpf_slice__rbpf` | 1,200 | 3.080 | 3.107 | 21.847 | 22.252 | @@ -53,10 +53,10 @@ overwrites this file, and the gate fails if it is stale. See #380. --> | `real_outage_60s__eskf` | 5,366 | 42.922 | 40.595 | 0.614 | 0.853 | | `real_degraded__ukf` | 5,366 | 5,474.700 | 5,473.870 | 0.074 | 0.093 | | `real_degraded__eskf` | 5,366 | 5,474.380 | 5,473.580 | 0.074 | 0.094 | -| `syn_cruise_1hz__ukf` | 15,000 | 4.774 | 4.776 | 1.000 | 0.913 | -| `syn_cruise_1hz__ekf` | 15,000 | 4.774 | 4.776 | 1.000 | 0.913 | -| `syn_cruise_1hz__eskf` | 15,000 | 4.573 | 4.575 | 1.000 | 0.928 | -| `syn_outage_60s__ukf` | 15,000 | 2.341 | 2.341 | 0.999 | 0.966 | -| `syn_outage_60s__eskf` | 15,000 | 2.228 | 2.229 | 0.999 | 0.963 | +| `syn_cruise_1hz__ukf` | 15,000 | 3.272 | 3.272 | 1.000 | 1.000 | +| `syn_cruise_1hz__ekf` | 15,000 | 3.272 | 3.272 | 1.000 | 1.000 | +| `syn_cruise_1hz__eskf` | 15,000 | 3.170 | 3.171 | 1.000 | 1.000 | +| `syn_outage_60s__ukf` | 15,000 | 1.426 | 1.431 | 1.000 | 0.998 | +| `syn_outage_60s__eskf` | 15,000 | 1.346 | 1.350 | 1.000 | 1.000 | | `syn_dead_reckoning` | 6,000 | -- | -- | -- | -- | | `real_rbpf_slice__rbpf` | 1,200 | 18.121 | 10.327 | 0.951 | 0.951 | diff --git a/core/src/earth.rs b/core/src/earth.rs index 0fb1879d..86dec8bc 100644 --- a/core/src/earth.rs +++ b/core/src/earth.rs @@ -100,19 +100,22 @@ pub const UNIVERSAL_GAS_CONSTANT: f64 = 8.314462618; pub const STANDARD_LAPSE_RATE: f64 = 0.0065; /// Calculate a barometric altitude from a measured pressure /// -/// This function calculates the altitude above sea level based on the measured pressure -/// using the barometric formula. The formula assumes a standard atmosphere and uses the -/// universal gas constant, standard lapse rate, and molar mass of dry air. +/// The altitude above sea level of a pressure `pressure` in an isothermal atmosphere at +/// [`SEA_LEVEL_TEMPERATURE`], referenced to [`SEA_LEVEL_PRESSURE`]: +/// +/// $$ h = \frac{R T_0}{g_0 M} \ln\frac{P_0}{P} $$ +/// +/// This is the exact inverse of [`expected_barometric_pressure`] at the standard sea-level +/// pressure, and [`relative_barometric_altitude`] referenced to that pressure. It replaces a +/// formula that mixed the lapse-rate and isothermal models and returned about -152 km for the +/// pressure of 100 m. /// /// # Parameters /// - `pressure` - The measured pressure in Pascals /// # Returns /// The calculated altitude in meters above sea level pub fn barometric_altitude(pressure: &f64) -> f64 { - let exponent: f64 = -(UNIVERSAL_GAS_CONSTANT * STANDARD_LAPSE_RATE) / (G0 * MOLAR_MASS_DRY_AIR); - (SEA_LEVEL_PRESSURE / STANDARD_LAPSE_RATE) - * (pressure / SEA_LEVEL_PRESSURE - 1.0) - * exponent.exp() + relative_barometric_altitude(*pressure, SEA_LEVEL_PRESSURE, None) } /// Calculate the relative barometric altitude from a measured pressure /// @@ -1071,24 +1074,19 @@ mod tests { ); } + /// `barometric_altitude` inverts `expected_barometric_pressure`. + /// + /// The old test only asserted a finite result, under a comment admitting the formula looked + /// wrong; it was: the pressure of 100 m came back as about -152 km. #[test] fn test_barometric_altitude() { - // Test barometric altitude calculation - // Note: The current barometric_altitude function appears to have an incorrect formula - // This test validates that it executes without error and returns a value - - let pressure = SEA_LEVEL_PRESSURE; - let altitude = barometric_altitude(&pressure); - // The function should at least execute and return a finite value - assert!(altitude.is_finite(), "Altitude should be a finite number"); - - // Test at reduced pressure - let pressure = 89875.0; - let altitude = barometric_altitude(&pressure); - assert!( - altitude.is_finite(), - "Altitude should be a finite number for reduced pressure" - ); + assert_approx_eq!(barometric_altitude(&SEA_LEVEL_PRESSURE), 0.0, 1e-9); + for altitude in [-400.0, 0.0, 100.0, 1000.0, 8000.0] { + let pressure = expected_barometric_pressure(altitude, SEA_LEVEL_PRESSURE); + assert_approx_eq!(barometric_altitude(&pressure), altitude, 1e-6); + } + // Lower pressure is higher altitude. + assert!(barometric_altitude(&89_875.0) > barometric_altitude(&95_000.0)); } #[test] diff --git a/core/src/sim.rs b/core/src/sim.rs index 448e03f0..025687d0 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -6215,8 +6215,28 @@ const fn default_gnss_vertical_noise_m() -> f64 { 5.0 } +/// Per-sample barometric pressure noise for [`SyntheticConfig`], pascals. +/// +/// The pressure noise whose altitude equivalent at sea level is +/// [`crate::measurements::BAROMETRIC_ALTITUDE_NOISE_M`], the one-sigma the filters assume for +/// a barometer by default, so that a default filter's $R$ describes a default synthetic +/// barometer. The isothermal barometric formula gives +/// $\mathrm dh/\mathrm dP = -R T_0 / (g_0 M P_0)$, about 0.083 m per pascal, so this is about +/// 27 Pa. const fn default_baro_noise_std_pa() -> f64 { - 50.0 + crate::measurements::BAROMETRIC_ALTITUDE_NOISE_M + * crate::earth::G0 + * crate::earth::MOLAR_MASS_DRY_AIR + * crate::earth::SEA_LEVEL_PRESSURE + / (crate::earth::UNIVERSAL_GAS_CONSTANT * crate::earth::SEA_LEVEL_TEMPERATURE) +} + +/// Per-axis GNSS velocity noise for [`SyntheticConfig`], metres per second. +/// +/// Applied to the north and east velocity before they are written as `speed` and `bearing`, +/// and written as `speedAccuracy`, which is the one-sigma the filters use for the velocity fix. +const fn default_gnss_velocity_noise_mps() -> f64 { + 0.5 } /// Per-axis magnetometer noise for [`SyntheticConfig`], microtesla. @@ -6272,6 +6292,13 @@ const MAGNETOMETER_NOISE_STREAM_OFFSET: u64 = 0x4d41_474e_4554_4f00; /// [`MAGNETOMETER_NOISE_STREAM_OFFSET`] for the same argument one level up. const MAGNETOMETER_HARD_IRON_STREAM_OFFSET: u64 = 0x4841_5244_4952_4f4e; +/// Offset separating the GNSS velocity noise stream from the trajectory's. +/// +/// Velocity noise was added after the trajectory stream's draw order was fixed, so it has its +/// own stream for the reason [`MAGNETOMETER_NOISE_STREAM_OFFSET`] gives: the IMU, GNSS position +/// and barometer realizations stay what they were. +const GNSS_VELOCITY_NOISE_STREAM_OFFSET: u64 = 0x474e_5353_5645_4c00; + /// The true magnetic field at a point, in the navigation frame, microtesla. /// /// Returns the field in whichever frame `is_enu` selects, so the caller can rotate it into the @@ -6383,7 +6410,13 @@ pub struct SyntheticConfig { /// GNSS vertical position noise standard deviation in meters #[serde(default = "default_gnss_vertical_noise_m")] pub gnss_vertical_noise_m: f64, - /// Barometric pressure noise standard deviation in Pascals + /// GNSS velocity noise standard deviation per horizontal axis, m/s. It perturbs the + /// `speed` and `bearing` columns and is written as `speedAccuracy`. + #[serde(default = "default_gnss_velocity_noise_mps")] + pub gnss_velocity_noise_mps: f64, + /// Barometric pressure noise standard deviation in Pascals. It perturbs `pressure`, and + /// `relativeAltitude` is computed from the perturbed pressure, so it is also the + /// barometric altitude noise the filters see: about 0.083 m per pascal near sea level. #[serde(default = "default_baro_noise_std_pa")] pub baro_noise_std_pa: f64, /// Magnetometer noise standard deviation per axis, microtesla. @@ -6427,6 +6460,7 @@ impl Default for SyntheticConfig { no_noise: false, gnss_horizontal_noise_m: default_gnss_horizontal_noise_m(), gnss_vertical_noise_m: default_gnss_vertical_noise_m(), + gnss_velocity_noise_mps: default_gnss_velocity_noise_mps(), baro_noise_std_pa: default_baro_noise_std_pa(), mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -6621,7 +6655,6 @@ pub fn generate_synthetic( let dt = 1.0 / config.sample_rate_hz; let n_steps = (config.duration_s * config.sample_rate_hz).round() as usize; - let initial_alt = state.altitude; // Draw per-trajectory bias offsets (constant for the full run) let accel_bias = { @@ -6676,6 +6709,18 @@ pub fn generate_synthetic( // scenario that takes no measurements at all, moving when the magnetometer was added. A // separate stream makes a re-bless attributable: a number that moves, moved because of the // heading aid. + let gnss_velocity_dist = Normal::new(0.0_f64, config.gnss_velocity_noise_mps) + .unwrap_or_else(|_| crate::normal_with_std(0.5)); + let mut gnss_velocity_rng = { + use rand::SeedableRng as _; + rand::rngs::StdRng::seed_from_u64( + config.seed.wrapping_add(GNSS_VELOCITY_NOISE_STREAM_OFFSET), + ) + }; + // The barometer reports height relative to its first reading, as Sensor Logger's + // `relativeAltitude` does. That first reading is noisy, so its error is a constant offset in + // every later relative altitude: the barometric bias the filters estimate. + let mut baro_reference_pressure: Option = None; let mut mag_rng = { use rand::SeedableRng as _; rand::rngs::StdRng::seed_from_u64( @@ -6813,8 +6858,24 @@ pub fn generate_synthetic( true_pressure + rng.sample(baro_dist) }; - let speed = state.velocity_north.hypot(state.velocity_east); - let bearing = state.velocity_east.atan2(state.velocity_north).to_degrees(); + let reference_pressure = *baro_reference_pressure.get_or_insert(out_pressure); + // `relativeAltitude` is derived from the noisy pressure through the same isothermal + // atmosphere `expected_barometric_pressure` uses, so `--baro-noise-std-pa` is the noise + // the barometric measurement carries. It used to be the GNSS altitude minus its first + // value, which gave the barometer the GNSS's noise draw and made it no independent aid. + let relative_altitude = + earth::relative_barometric_altitude(out_pressure, reference_pressure, None); + + let (velocity_north_fix, velocity_east_fix) = if config.no_noise { + (state.velocity_north, state.velocity_east) + } else { + ( + state.velocity_north + gnss_velocity_rng.sample(gnss_velocity_dist), + state.velocity_east + gnss_velocity_rng.sample(gnss_velocity_dist), + ) + }; + let speed = velocity_north_fix.hypot(velocity_east_fix); + let bearing = velocity_east_fix.atan2(velocity_north_fix).to_degrees(); let attitude_quaternion = nalgebra::UnitQuaternion::from_rotation_matrix(&state.attitude); // Gravity vector in body frame (NED: [0,0,g]) @@ -6876,8 +6937,11 @@ pub fn generate_synthetic( altitude: out_alt, speed, bearing, - bearing_accuracy: config.gnss_horizontal_noise_m, - speed_accuracy: config.gnss_horizontal_noise_m, + // The course's one-sigma, in degrees: the angle a velocity error of + // `gnss_velocity_noise_mps` subtends at this speed, which tends to 90 degrees as + // the platform stops and the course becomes meaningless. + bearing_accuracy: config.gnss_velocity_noise_mps.atan2(speed).to_degrees(), + speed_accuracy: config.gnss_velocity_noise_mps, vertical_accuracy: config.gnss_vertical_noise_m, horizontal_accuracy: config.gnss_horizontal_noise_m, // `TestDataRecord` documents roll/pitch/yaw as radians, and the quaternion is @@ -6902,8 +6966,10 @@ pub fn generate_synthetic( mag_x: mag_body[0], mag_y: mag_body[1], mag_z: mag_body[2], - relative_altitude: out_alt - initial_alt, - pressure: out_pressure, + relative_altitude, + // Hectopascals (millibars), the unit `TestDataRecord::pressure` documents and + // Sensor Logger writes; the barometric formula works in pascals. + pressure: out_pressure / 100.0, grav_x: grav_body[0], grav_y: grav_body[1], grav_z: grav_body[2], @@ -6944,6 +7010,7 @@ mod tests { no_noise: false, gnss_horizontal_noise_m: 1.0, gnss_vertical_noise_m: 1.0, + gnss_velocity_noise_mps: 0.5, baro_noise_std_pa: 1.0, mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -7003,6 +7070,7 @@ mod tests { no_noise: true, gnss_horizontal_noise_m: 1.0, gnss_vertical_noise_m: 1.0, + gnss_velocity_noise_mps: 0.5, baro_noise_std_pa: 1.0, mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -7310,6 +7378,7 @@ mod tests { no_noise: true, gnss_horizontal_noise_m: 2.5, gnss_vertical_noise_m: 5.0, + gnss_velocity_noise_mps: 0.5, baro_noise_std_pa: 50.0, mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -7363,6 +7432,7 @@ mod tests { no_noise: false, gnss_horizontal_noise_m: 2.5, gnss_vertical_noise_m: 5.0, + gnss_velocity_noise_mps: 0.5, baro_noise_std_pa: 50.0, mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -10919,6 +10989,7 @@ mod tests { no_noise: false, gnss_horizontal_noise_m: 3.0, gnss_vertical_noise_m: 5.0, + gnss_velocity_noise_mps: 0.5, baro_noise_std_pa: 30.0, mag_noise_std_ut: default_mag_noise_std_ut(), mag_hard_iron_std_ut: default_mag_hard_iron_std_ut(), @@ -10984,6 +11055,74 @@ mod tests { ); } + /// The synthetic barometer and GNSS velocity are independent sensors in the right units. + /// + /// Before this, `relativeAltitude` was the noisy GNSS altitude minus its first value, so + /// the barometer shared the GNSS's noise draw and aided nothing, while + /// `baro_noise_std_pa` reached only `pressure`, which no estimator reads. `pressure` was in + /// pascals against a documented millibars, and `speed`/`bearing` were noise-free under a + /// `speedAccuracy` that claimed otherwise. + #[test] + fn synthetic_barometer_and_velocity_are_independent_sensors() { + use rand::SeedableRng; + + let mut config = synthetic_config_for_tests(); + config.duration_s = 300.0; + config.sample_rate_hz = 10.0; + config.baro_noise_std_pa = 12.0; + config.gnss_velocity_noise_mps = 0.5; + let mut rng = rand::rngs::StdRng::seed_from_u64(42); + let (_truth, records) = generate_synthetic(&config, &mut rng).expect("synthetic run"); + let n = records.len() as f64; + let mean = |values: &[f64]| values.iter().sum::() / values.len() as f64; + let std = |values: &[f64]| { + let m = mean(values); + (values.iter().map(|v| (v - m).powi(2)).sum::() / values.len() as f64).sqrt() + }; + + // Barometric noise: about 0.083 m per pascal, independent of the GNSS altitude noise. + let relative: Vec = records.iter().map(|r| r.relative_altitude).collect(); + let gnss: Vec = records.iter().map(|r| r.altitude).collect(); + let expected_std = 12.0 * 0.0832; + assert!( + (std(&relative) - expected_std).abs() < 0.1 * expected_std, + "relativeAltitude std {:.3} m, expected about {expected_std:.3} m", + std(&relative) + ); + let (mean_relative, mean_gnss) = (mean(&relative), mean(&gnss)); + let covariance = relative + .iter() + .zip(&gnss) + .map(|(a, b)| (a - mean_relative) * (b - mean_gnss)) + .sum::() + / n; + let correlation = covariance / (std(&relative) * std(&gnss)); + assert!( + correlation.abs() < 0.1, + "barometer and GNSS altitude must be independent, correlation {correlation:.3}" + ); + + // Pressure is written in hectopascals. + let pressure = mean(&records.iter().map(|r| r.pressure).collect::>()); + assert!( + (800.0..1100.0).contains(&pressure), + "pressure should be hPa, got {pressure}" + ); + + // Velocity is noisy, and speedAccuracy says by how much. + assert!( + records + .iter() + .all(|r| (r.speed_accuracy - 0.5).abs() < 1e-12) + ); + let speeds: Vec = records.iter().map(|r| r.speed).collect(); + assert!( + std(&speeds) > 0.1, + "speed should carry the velocity noise, std {:.3}", + std(&speeds) + ); + } + /// Hard iron is off unless asked for, and does something when it is. /// /// The default matters: a hard-iron offset is constant in the *body* frame, so it biases diff --git a/core/tests/perf_baseline.json b/core/tests/perf_baseline.json index d657cc69..0052356e 100644 --- a/core/tests/perf_baseline.json +++ b/core/tests/perf_baseline.json @@ -526,56 +526,56 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 0.419835 + "value": 0.41712 }, "containment_3sigma_horizontal": { "value": 1.0 }, "containment_3sigma_vertical": { - "value": 0.913333, + "value": 1.0, "note": "Down under #375. With the barometer scheduled at 1 Hz instead of firing on all fifty records a second, the vertical error is larger and carries a systematic bias the filter does not model, so the 3-sigma envelope no longer covers it. The horizontal containment on the same row is unchanged. This is the #372 measurement, taken without the 50 Hz aiding that used to hide it." }, "horizontal_cep50_m": { - "value": 2.21346 + "value": 1.3339 }, "horizontal_cep95_m": { - "value": 4.35456 + "value": 3.04254 }, "horizontal_max_m": { - "value": 6.78487 + "value": 4.37738 }, "horizontal_rmse_m": { - "value": 2.60038 + "value": 1.71747 }, "nees_position": { - "value": 4.77432 + "value": 3.27218 }, "npes_position": { - "value": 4.77642, + "value": 3.27237, "note": "Up under #375, entirely through the vertical channel: the horizontal columns on this row moved by less than half a percent, while `vertical_rmse_m` went ~0.5 to ~2.2-2.8 m and `vertical_bias_m` from about zero to -0.6 to -2.0 m. A normalised error takes that unmodelled bias into a covariance that did not grow to meet it. The gap is #372's to close; the 50 Hz barometer was masking it." }, "pitch_rmse_deg": { - "value": 0.251359 + "value": 0.258456 }, "roll_rmse_deg": { - "value": 0.298455 + "value": 0.229772 }, "velocity_horizontal_rmse_mps": { - "value": 0.581468 + "value": 0.357811 }, "velocity_vertical_rmse_mps": { - "value": 0.303787 + "value": 0.179165 }, "vertical_bias_m": { - "value": -0.685527, + "value": -0.781039, "note": "About zero before #375 and -0.6 to -2.0 m after it. This is the number that makes the case for #372's barometer bias state: at 50 Hz the aid was strong enough to crush a systematic altitude offset, and at the 1 Hz a real barometer logs at, it is what the vertical channel actually does. Nothing here models it." }, "vertical_rmse_m": { - "value": 2.2788, + "value": 1.51761, "note": "Roughly 4x worse than at a 50 Hz barometer, and the honest price of #375 scheduling it. `generate_synthetic` fills `relative_altitude` on every record, so before #375 this 50 Hz scenario received fifty pressure readings a second, each entering as an independent fix; averaging fifty samples cuts the effective noise about sevenfold and pinned altitude to the truth. At the 1 Hz the reference recording actually logs, it does not. Note `vertical_bias_m` on the same row moved from about zero to -0.6 to -2.0 m: what is left is a systematic offset, not noise, and an unmodelled offset under a tight R is exactly #372. The real-data rows did not move at all -- they were already 1 Hz." }, "yaw_rmse_deg": { - "value": 0.154872 + "value": 0.233207 } } }, @@ -585,56 +585,56 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 0.416223 + "value": 0.651224 }, "containment_3sigma_horizontal": { "value": 1.0 }, "containment_3sigma_vertical": { - "value": 0.9282, + "value": 1.0, "note": "Down under #375. With the barometer scheduled at 1 Hz instead of firing on all fifty records a second, the vertical error is larger and carries a systematic bias the filter does not model, so the 3-sigma envelope no longer covers it. The horizontal containment on the same row is unchanged. This is the #372 measurement, taken without the 50 Hz aiding that used to hide it." }, "horizontal_cep50_m": { - "value": 2.17875 + "value": 1.34257 }, "horizontal_cep95_m": { - "value": 4.39676 + "value": 3.03376 }, "horizontal_max_m": { - "value": 6.78527 + "value": 5.5877 }, "horizontal_rmse_m": { - "value": 2.60049 + "value": 1.73368 }, "nees_position": { - "value": 4.57318 + "value": 3.17038 }, "npes_position": { - "value": 4.57509, + "value": 3.17054, "note": "Up under #375, entirely through the vertical channel: the horizontal columns on this row moved by less than half a percent, while `vertical_rmse_m` went ~0.5 to ~2.2-2.8 m and `vertical_bias_m` from about zero to -0.6 to -2.0 m. A normalised error takes that unmodelled bias into a covariance that did not grow to meet it. The gap is #372's to close; the 50 Hz barometer was masking it." }, "pitch_rmse_deg": { - "value": 0.253631 + "value": 0.334606 }, "roll_rmse_deg": { - "value": 0.291448 + "value": 0.476203 }, "velocity_horizontal_rmse_mps": { - "value": 0.559304 + "value": 0.358017 }, "velocity_vertical_rmse_mps": { - "value": 0.26227 + "value": 0.163043 }, "vertical_bias_m": { - "value": -0.548654, + "value": -0.723594, "note": "About zero before #375 and -0.6 to -2.0 m after it. This is the number that makes the case for #372's barometer bias state: at 50 Hz the aid was strong enough to crush a systematic altitude offset, and at the 1 Hz a real barometer logs at, it is what the vertical channel actually does. Nothing here models it." }, "vertical_rmse_m": { - "value": 2.15404, + "value": 1.45019, "note": "Roughly 4x worse than at a 50 Hz barometer, and the honest price of #375 scheduling it. `generate_synthetic` fills `relative_altitude` on every record, so before #375 this 50 Hz scenario received fifty pressure readings a second, each entering as an independent fix; averaging fifty samples cuts the effective noise about sevenfold and pinned altitude to the truth. At the 1 Hz the reference recording actually logs, it does not. Note `vertical_bias_m` on the same row moved from about zero to -0.6 to -2.0 m: what is left is a systematic offset, not noise, and an unmodelled offset under a tight R is exactly #372. The real-data rows did not move at all -- they were already 1 Hz." }, "yaw_rmse_deg": { - "value": 0.154836 + "value": 0.292882 } } }, @@ -644,56 +644,56 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 0.420284 + "value": 0.407469 }, "containment_3sigma_horizontal": { "value": 1.0 }, "containment_3sigma_vertical": { - "value": 0.913333, + "value": 1.0, "note": "Down under #375. With the barometer scheduled at 1 Hz instead of firing on all fifty records a second, the vertical error is larger and carries a systematic bias the filter does not model, so the 3-sigma envelope no longer covers it. The horizontal containment on the same row is unchanged. This is the #372 measurement, taken without the 50 Hz aiding that used to hide it." }, "horizontal_cep50_m": { - "value": 2.21354 + "value": 1.33386 }, "horizontal_cep95_m": { - "value": 4.35363 + "value": 3.04266 }, "horizontal_max_m": { - "value": 6.78424 + "value": 4.3774 }, "horizontal_rmse_m": { - "value": 2.60023 + "value": 1.71746 }, "nees_position": { - "value": 4.77389 + "value": 3.27225 }, "npes_position": { - "value": 4.77601, + "value": 3.27244, "note": "Up under #375, entirely through the vertical channel: the horizontal columns on this row moved by less than half a percent, while `vertical_rmse_m` went ~0.5 to ~2.2-2.8 m and `vertical_bias_m` from about zero to -0.6 to -2.0 m. A normalised error takes that unmodelled bias into a covariance that did not grow to meet it. The gap is #372's to close; the 50 Hz barometer was masking it." }, "pitch_rmse_deg": { - "value": 0.251292 + "value": 0.258302 }, "roll_rmse_deg": { - "value": 0.298342 + "value": 0.229841 }, "velocity_horizontal_rmse_mps": { - "value": 0.581347 + "value": 0.357774 }, "velocity_vertical_rmse_mps": { - "value": 0.303771 + "value": 0.179116 }, "vertical_bias_m": { - "value": -0.685495, + "value": -0.781093, "note": "About zero before #375 and -0.6 to -2.0 m after it. This is the number that makes the case for #372's barometer bias state: at 50 Hz the aid was strong enough to crush a systematic altitude offset, and at the 1 Hz a real barometer logs at, it is what the vertical channel actually does. Nothing here models it." }, "vertical_rmse_m": { - "value": 2.27874, + "value": 1.51769, "note": "Roughly 4x worse than at a 50 Hz barometer, and the honest price of #375 scheduling it. `generate_synthetic` fills `relative_altitude` on every record, so before #375 this 50 Hz scenario received fifty pressure readings a second, each entering as an independent fix; averaging fifty samples cuts the effective noise about sevenfold and pinned altitude to the truth. At the 1 Hz the reference recording actually logs, it does not. Note `vertical_bias_m` on the same row moved from about zero to -0.6 to -2.0 m: what is left is a systematic offset, not noise, and an unmodelled offset under a tight R is exactly #372. The real-data rows did not move at all -- they were already 1 Hz." }, "yaw_rmse_deg": { - "value": 0.15642, + "value": 0.215592, "note": "A fraction of a degree, down from 108.3 with no magnetometer at all (#369) and from 2.011 before the initial gyro-bias prior was sized from the IMU grade. That 2.011 was read here as #371 leaking through; it was not. The UKF and EKF shipped a gyro-bias variance of 1e-3 -- a sigma of 1.81 deg/s against a Consumer part's 0.028 -- while the ESKF shipped 1e-8, so no cross-filter reading of this column before that fix was comparing filters. The EKF reads about 0.28 deg here and the ESKF about 0.18." } } @@ -704,7 +704,7 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 1.30061 + "value": 1.30077 }, "containment_3sigma_horizontal": { "value": null @@ -713,16 +713,16 @@ "value": null }, "horizontal_cep50_m": { - "value": 110.128 + "value": 104.578 }, "horizontal_cep95_m": { - "value": 715.856 + "value": 685.614 }, "horizontal_max_m": { - "value": 835.634 + "value": 802.62 }, "horizontal_rmse_m": { - "value": 316.86 + "value": 302.347 }, "nees_position": { "value": null @@ -732,25 +732,25 @@ "note": "`sim::dead_reckoning` emits no covariance, so the three consistency metrics are `null` here. This scenario is what exercises that path end to end." }, "pitch_rmse_deg": { - "value": 1.10586 + "value": 1.106 }, "roll_rmse_deg": { - "value": 0.46791 + "value": 0.468209 }, "velocity_horizontal_rmse_mps": { - "value": 9.36277 + "value": 9.16108 }, "velocity_vertical_rmse_mps": { - "value": 1.89618 + "value": 1.89435 }, "vertical_bias_m": { - "value": -64.2546 + "value": -64.19 }, "vertical_rmse_m": { - "value": 86.5915 + "value": 86.505 }, "yaw_rmse_deg": { - "value": 0.493822 + "value": 0.493674 } } }, @@ -760,56 +760,56 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 0.415987 + "value": 0.580771 }, "containment_3sigma_horizontal": { - "value": 0.999333 + "value": 1.0 }, "containment_3sigma_vertical": { - "value": 0.9632 + "value": 0.999933 }, "horizontal_cep50_m": { - "value": 1.40559 + "value": 3.38976 }, "horizontal_cep95_m": { - "value": 55.3728 + "value": 66.4822 }, "horizontal_max_m": { - "value": 112.796, + "value": 95.7677, "regress_fraction": 0.25, "note": "As `real_outage_60s__eskf`: a single sample at the end of a coast." }, "horizontal_rmse_m": { - "value": 23.4435, + "value": 28.6563, "note": "Worse under #375, which is the same vertical story as the `syn_cruise_1hz` rows and, on this scenario, a compounding one: through a 60 s GNSS outage the barometer is the only aiding source there is, so dropping it from 50 Hz to 1 Hz removes almost all of it. Roll and pitch degrade with it, and horizontal dead reckoning follows the attitude. 24 m of horizontal error over a 60 s free-inertial coast on consumer-grade IMU is a realistic figure; the 12 m it replaces was flattered by a barometer no hardware runs at." }, "nees_position": { - "value": 2.22843 + "value": 1.34565 }, "npes_position": { - "value": 2.22874 + "value": 1.34974 }, "pitch_rmse_deg": { - "value": 0.249346 + "value": 0.318865 }, "roll_rmse_deg": { - "value": 0.222441 + "value": 0.196131 }, "velocity_horizontal_rmse_mps": { - "value": 1.34614 + "value": 1.45267 }, "velocity_vertical_rmse_mps": { - "value": 0.234172 + "value": 0.138904 }, "vertical_bias_m": { - "value": -0.228706 + "value": -0.395834 }, "vertical_rmse_m": { - "value": 1.87189, + "value": 1.10382, "note": "Worse under #375, which is the same vertical story as the `syn_cruise_1hz` rows and, on this scenario, a compounding one: through a 60 s GNSS outage the barometer is the only aiding source there is, so dropping it from 50 Hz to 1 Hz removes almost all of it. Roll and pitch degrade with it, and horizontal dead reckoning follows the attitude. 24 m of horizontal error over a 60 s free-inertial coast on consumer-grade IMU is a realistic figure; the 12 m it replaces was flattered by a barometer no hardware runs at." }, "yaw_rmse_deg": { - "value": 0.24773 + "value": 0.443852 } } }, @@ -819,62 +819,62 @@ "discarded_channel_samples": 0, "metrics": { "attitude_geodesic_rmse_deg": { - "value": 0.362645 + "value": 0.523277 }, "containment_3sigma_horizontal": { - "value": 0.999333 + "value": 1.0 }, "containment_3sigma_vertical": { - "value": 0.965533 + "value": 0.997933 }, "horizontal_cep50_m": { - "value": 1.3786 + "value": 3.39238 }, "horizontal_cep95_m": { - "value": 48.1232, + "value": 55.7976, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "horizontal_max_m": { - "value": 125.428, + "value": 80.3037, "regress_fraction": 0.25, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "horizontal_rmse_m": { - "value": 24.0243, + "value": 24.7965, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "nees_position": { - "value": 2.3409, + "value": 1.42647, "note": "Around 4.2, and within a fraction of a percent of `npes_position`. Both fell by an order of magnitude when the initial gyro-bias prior was sized from the IMU grade rather than left at a copy-pasted 1e-3 -- see `horizontal_rmse_m`'s note. The NEES/NPES ratio was never what was wrong with this row: the diagonal-only form has agreed with the full quadratic form throughout, before and after." }, "npes_position": { - "value": 2.34102, + "value": 1.43089, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "pitch_rmse_deg": { - "value": 0.205908 + "value": 0.288876 }, "roll_rmse_deg": { - "value": 0.230734, + "value": 0.184033, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "velocity_horizontal_rmse_mps": { - "value": 1.29049, + "value": 1.25674, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "velocity_vertical_rmse_mps": { - "value": 0.2844, + "value": 0.176541, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "vertical_bias_m": { - "value": -0.0393296 + "value": -0.28067 }, "vertical_rmse_m": { - "value": 2.04476, + "value": 1.11618, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." }, "yaw_rmse_deg": { - "value": 0.189331, + "value": 0.395479, "note": "Read this row as the one the v1.0 spine spent the longest on, and as a lesson about uncontrolled comparisons. It read 809 m of horizontal RMSE, a NEES of 44.67 and 72.68 deg of yaw, and that was attributed here to #371 -- the UKF's linear mean over sigma-point Euler triples -- on the strength of `syn_outage_60s__eskf` reading 0.255 deg on the identical stream. That comparison was not controlled. The UKF and EKF shipped an initial gyro-bias variance of 1e-3, a sigma of 1.81 deg/s against the 0.028 deg/s a Consumer-grade part actually has, while the ESKF shipped 1e-8: the two filters were never handed the same prior. Sizing all three from `IMUQuality::initial_bias_covariance` took this row to about 27 m, a NEES near 4.2 and roughly 0.22 deg of yaw -- past the ESKF's own numbers on the same stream. Fixing #371 afterwards moved the yaw by 0.007 deg and improved the position. The prior was the defect; the chart error was real but is not what this row measured." } } diff --git a/sim/src/main.rs b/sim/src/main.rs index bee9d285..1539953c 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -268,8 +268,15 @@ struct SyntheticArgs { #[arg(long, default_value_t = 5.0)] gnss_vertical_noise_m: f64, - /// Barometric pressure noise standard deviation in Pascals - #[arg(long, default_value_t = 50.0)] + /// GNSS velocity noise standard deviation per horizontal axis in m/s. Perturbs `speed` and + /// `bearing`, and is written as `speedAccuracy`. + #[arg(long, default_value_t = strapdown::sim::SyntheticConfig::default().gnss_velocity_noise_mps)] + gnss_velocity_noise_mps: f64, + + /// Barometric pressure noise standard deviation in Pascals. `relativeAltitude` is derived + /// from the noisy pressure, about 0.083 m per pascal near sea level. The default matches + /// the 2.24 m barometric noise the filters assume. + #[arg(long, default_value_t = strapdown::sim::SyntheticConfig::default().baro_noise_std_pa)] baro_noise_std_pa: f64, /// Magnetometer noise standard deviation in microtesla, per axis. @@ -1661,6 +1668,7 @@ fn run_synthetic(args: &SyntheticArgs) -> Result<(), Box> { built.no_noise = args.no_noise; built.gnss_horizontal_noise_m = args.gnss_horizontal_noise_m; built.gnss_vertical_noise_m = args.gnss_vertical_noise_m; + built.gnss_velocity_noise_mps = args.gnss_velocity_noise_mps; built.baro_noise_std_pa = args.baro_noise_std_pa; built.mag_noise_std_ut = args.mag_noise_std_ut; built.mag_hard_iron_std_ut = args.mag_hard_iron_std_ut; From 9a2e098b9663a14655d59a615b83c9061491ae6d Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:51:49 -0400 Subject: [PATCH 23/26] Rerun every command in the book against the current binary and correct what went stale The book was written before cd635d2 (independent synthetic barometer and GNSS velocity) and the CLI fixes in 35ab5d4 and its neighbours landed. Every CLI command, config file and cargo example in SUMMARY.md's pages was rerun on a fresh syn trajectory; these are the pages whose output or statements no longer held. Synthetic generator (cd635d2): - synthetic.md, data-format.md, quick-start.md: relativeAltitude is now the height of a noisy pressure, independent of the GNSS altitude and offset by its first reading; pressure is hPa, not Pa; speed/bearing carry --gnss-velocity-noise-mps (new flag, default 0.5) and speedAccuracy/bearingAccuracy describe it; the barometric default is about 26.86 Pa, not 50. The [synthetic] example gains gnss_velocity_noise_mps. - Every printed number that depends on a syn trajectory moved: quick-start's last ESKF row and score table, the PF tutorial's score tables, the health-limit speed in simulations.md (49.798 m/s, from the first record's noisy speed), the gated-event count in closed-loop.md (7, was 78), and the kalman_filters and eskf example outputs in kalman.md and eskf.md. - particle-filter.md and the PF tutorial said zero horizontal process noise diverges on the synthetic runs. It now finishes on both; the tutorial shows its score (NEES 9.8, containment 0.93: over-confident) and both pages point at the recorded-data divergence the library documents instead. CLI fixes (35ab5d4 and others): - dr now applies the health limits, so `dr` on the 600 s syn trajectory stops at the 500 m/s speed bound. quick-start, dead-reckoning, data-format and the PF tutorial pass --health-speed-mps-max 1000 and say why; the blank-acc_x error is now NonFinite { what: "filter state" }. architecture.md names dead_reckoning_with_limits, and configuration.md no longer says config-file dr ignores the limits. - --parallel works on subcommands (faq.md said it needed --config); generate_plot also plots dr results; a bad --gate-confidence now reports through the gate validator. - The single-file ENU refusal no longer logs an [ERROR] line first (coordinate-frames). - AidingConfig.seed is Option (scenarios.md's table said 42). - Synthetic mode reads [logging] as well as [synthetic]. Other corrections found on the way: output-format.md said mcap is Rust-only (it builds bundled LZ4 and Zstandard C code); the FAQ's toolchain list omitted the C compiler that plotting and mcap need; closed-loop.md said the duty run had two outages (it has four); geonav overview said unused map-bias columns are NaN (they are empty); ukf.md described the conditioning test's duty cycle as starting on; combo_duty_hijack.yaml opens with its outage; the RUST_LOG example now shows only the first line, since that dr run now exits 1. quick-start's score.py is reformatted the way `ruff format --check` (CI's python.yml, which also formats Markdown code blocks) requires. baseline-tables.md is untouched; prose that quotes accuracy agrees with it. Co-Authored-By: Claude Opus 5.5 --- book/src/development/architecture.md | 7 +- book/src/development/performance.md | 4 +- book/src/examples/configurations.md | 2 +- book/src/examples/tutorial-particle-filter.md | 121 ++++++++++-------- book/src/faq.md | 17 ++- book/src/filters/eskf.md | 12 +- book/src/filters/kalman.md | 12 +- book/src/filters/ukf.md | 4 +- book/src/geonav/overview.md | 5 +- book/src/gnss/scenarios.md | 2 +- book/src/quick-start.md | 54 ++++---- book/src/user-guide/closed-loop.md | 8 +- book/src/user-guide/configuration.md | 11 +- book/src/user-guide/coordinate-frames.md | 1 - book/src/user-guide/data-format.md | 14 +- book/src/user-guide/dead-reckoning.md | 12 +- book/src/user-guide/logging.md | 4 +- book/src/user-guide/output-format.md | 2 +- book/src/user-guide/particle-filter.md | 9 +- book/src/user-guide/simulations.md | 13 +- book/src/user-guide/synthetic.md | 47 ++++--- 21 files changed, 207 insertions(+), 154 deletions(-) diff --git a/book/src/development/architecture.md b/book/src/development/architecture.md index 3111999e..8223f9a0 100644 --- a/book/src/development/architecture.md +++ b/book/src/development/architecture.md @@ -88,8 +88,11 @@ Every simulation mode is the same pipeline with a different estimator in the mid The variations are: -- **Dead reckoning** (`dr`) skips the event stream: `sim::dead_reckoning(records, is_enu)` - mechanizes the IMU rows directly and reports no covariance. +- **Dead reckoning** (`dr`) skips the event stream: + `sim::dead_reckoning_with_limits(records, is_enu, health_limits, execution_limits)` + mechanizes the IMU rows directly, checks the position, speed and wall-clock limits after each + step, and reports no covariance. `sim::dead_reckoning(records, is_enu)` is the same with no + limits. - **Closed loop** (`cl`) builds one of the Kalman filters and calls `run_closed_loop`; with geophysical maps it calls `run_closed_loop_with_geo`, which also reports the map biases. - **Particle filter** (`pf`) consumes the same `EventStream` through an event loop in diff --git a/book/src/development/performance.md b/book/src/development/performance.md index e29afc36..eaae4c12 100644 --- a/book/src/development/performance.md +++ b/book/src/development/performance.md @@ -161,8 +161,8 @@ Read the rows rather than any summary here; these are the comparisons worth maki exact truth. Compare its three rows column by column; the attitude table shows what a 1 Hz magnetometer and GNSS velocity make of yaw on a synthetic trajectory. - **The outage rows are dominated by the outages.** On `real_outage_60s__eskf` and both - `syn_outage_60s` rows, CEP50 stays at the level of the full-rate rows while RMSE, CEP95 and - the maximum are set by the coasts. That is the shape free-inertial drift gives: most + `syn_outage_60s` rows, CEP50 stays within a few metres of the full-rate rows while RMSE, CEP95 + and the maximum, an order of magnitude larger, are set by the coasts. That is the shape free-inertial drift gives: most epochs are aided and accurate, and the error concentrates at the ends of the outages. - **`syn_dead_reckoning` is the unaided reference.** It is only 120 s long, and its consistency columns are empty because dead reckoning reports no covariance. diff --git a/book/src/examples/configurations.md b/book/src/examples/configurations.md index 49be3a90..24fdf571 100644 --- a/book/src/examples/configurations.md +++ b/book/src/examples/configurations.md @@ -78,7 +78,7 @@ files' `output`, and run each with `--config`. | `slowbias.yaml` | `pass_through` | `slow_bias`, 0.02 m/s north, `q_bias` 1e-6 | soft spoofing: each fix looks plausible | | `slowbias_rot.yaml` | `pass_through` | `slow_bias`, as above with `q_bias` 5e-6 and the direction rotating at 1e-3 rad/s | the same, drifting in a turning direction | | `hijack.yaml` | `pass_through` | `hijack`, 50 m north from 120 s for 60 s | hard spoofing: an abrupt offset | -| `combo_duty_hijack.yaml` | `duty_cycle`, 10 s on / 3 s off | `hijack`, 10 m north and 10 m east from 150 s for 120 s | duty-cycled availability plus hard spoofing | +| `combo_duty_hijack.yaml` | `duty_cycle`, 10 s on / 3 s off, no initial on | `hijack`, 10 m north and 10 m east from 150 s for 120 s | duty-cycled availability plus hard spoofing | Spoofing scenarios are worth pairing with an innovation gate (`--gate-confidence` on the command line). A `hijack` produces a large normalized innovation squared the moment it starts, diff --git a/book/src/examples/tutorial-particle-filter.md b/book/src/examples/tutorial-particle-filter.md index 025f89a1..64b5ddb3 100644 --- a/book/src/examples/tutorial-particle-filter.md +++ b/book/src/examples/tutorial-particle-filter.md @@ -22,10 +22,10 @@ strapdown-sim syn -o drive_truth.csv --duration-s 600 --seed 42 \ ``` ```text -2026-10-01 20:52:19.901 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) -2026-10-01 20:52:19.912 [INFO] - Sensor records written to drive.csv -2026-10-01 20:52:19.993 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) -2026-10-01 20:52:20.000 [INFO] - Truth trajectory written to drive_truth.csv +2026-10-01 21:44:03.088 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 21:44:03.097 [INFO] - Sensor records written to drive.csv +2026-10-01 21:44:03.155 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 21:44:03.160 [INFO] - Truth trajectory written to drive_truth.csv ``` `syn` output is NED, which is the default for every subcommand, so no `--enu` is needed. (The @@ -38,11 +38,11 @@ strapdown-sim pf -i drive.csv -o pf_out.csv ``` ```text -2026-10-01 20:52:20.005 [INFO] - Processing file: drive.csv -2026-10-01 20:52:20.029 [INFO] - Read 6000 records from drive.csv -2026-10-01 20:52:20.030 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.775 m/s^2 against a local gravity of 9.801 m/s^2 -2026-10-01 20:52:21.164 [INFO] - Results written to pf_out.csv -2026-10-01 20:52:21.164 [INFO] - Particle filter simulation complete +2026-10-01 21:44:03.164 [INFO] - Processing file: drive.csv +2026-10-01 21:44:03.185 [INFO] - Read 6000 records from drive.csv +2026-10-01 21:44:03.186 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.775 m/s^2 against a local gravity of 9.801 m/s^2 +2026-10-01 21:44:04.269 [INFO] - Results written to pf_out.csv +2026-10-01 21:44:04.269 [INFO] - Particle filter simulation complete ``` The "Mechanizing input as NED" line is the frame check: the mean vertical specific force has the @@ -68,14 +68,14 @@ cargo run -p strapdown-core --example score_run -- pf_out.csv drive_truth.csv ```text aligned samples 5990 -horizontal RMSE 0.988 m -horizontal CEP50 0.808 m -horizontal CEP95 1.751 m -horizontal max 3.375 m -vertical RMSE 1.958 m -horizontal velocity RMSE 0.157 m/s -yaw RMSE 0.409 deg -position NEES (consistent at 3) 13.332 +horizontal RMSE 0.920 m +horizontal CEP50 0.758 m +horizontal CEP95 1.619 m +horizontal max 2.706 m +vertical RMSE 0.888 m +horizontal velocity RMSE 0.193 m/s +yaw RMSE 0.404 deg +position NEES (consistent at 3) 3.534 3-sigma horizontal containment 1.000 ``` @@ -138,25 +138,25 @@ cargo run -p strapdown-core --example score_run -- pf_config_out.csv drive_truth ``` ```text -2026-10-01 20:52:39.076 [INFO] - Loading configuration from rbpf_syn.toml -2026-10-01 20:52:39.076 [INFO] - Configuration loaded successfully -2026-10-01 20:52:39.076 [INFO] - Mode: ParticleFilter +2026-10-01 21:44:12.099 [INFO] - Loading configuration from rbpf_syn.toml +2026-10-01 21:44:12.099 [INFO] - Configuration loaded successfully +2026-10-01 21:44:12.099 [INFO] - Mode: ParticleFilter ... -2026-10-01 20:52:39.104 [INFO] - Running particle filter simulation -2026-10-01 20:52:43.908 [INFO] - Results written to pf_config_out.csv +2026-10-01 21:44:12.117 [INFO] - Running particle filter simulation +2026-10-01 21:44:16.836 [INFO] - Results written to pf_config_out.csv ``` ```text aligned samples 5990 -horizontal RMSE 2.362 m -horizontal CEP50 1.994 m -horizontal CEP95 3.962 m -horizontal max 6.800 m -vertical RMSE 2.676 m -horizontal velocity RMSE 0.570 m/s -yaw RMSE 0.416 deg -position NEES (consistent at 3) 7.880 -3-sigma horizontal containment 0.999 +horizontal RMSE 1.845 m +horizontal CEP50 1.613 m +horizontal CEP95 3.007 m +horizontal max 4.470 m +vertical RMSE 1.415 m +horizontal velocity RMSE 0.479 m/s +yaw RMSE 0.418 deg +position NEES (consistent at 3) 3.056 +3-sigma horizontal containment 1.000 ``` The [RBPF configuration table](../filters/rbpf.md#configuration) explains every key. Three are @@ -179,21 +179,32 @@ library's `RbpfConfig` keeps that default; `strapdown-sim` does not. On the same ```bash strapdown-sim pf -i drive.csv -o pf_zero.csv --sched fixed --interval-s 1 \ --horizontal-process-noise-std-m 0 0 +cargo run -p strapdown-core --example score_run -- pf_zero.csv drive_truth.csv ``` ```text -2026-10-01 20:52:45.263 [INFO] - Processing file: drive.csv -2026-10-01 20:52:45.290 [INFO] - Read 6000 records from drive.csv -2026-10-01 20:52:45.291 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.775 m/s^2 against a local gravity of 9.801 m/s^2 -Error: OutOfRange { what: "speed", value: 500.1655942836976, min: 0.0, max: 500.0 } +aligned samples 5990 +horizontal RMSE 2.025 m +horizontal CEP50 1.624 m +horizontal CEP95 3.550 m +horizontal max 6.068 m +vertical RMSE 1.409 m +horizontal velocity RMSE 0.509 m/s +yaw RMSE 0.603 deg +position NEES (consistent at 3) 9.786 +3-sigma horizontal containment 0.929 ``` -The run diverges until the health monitor's speed limit (`--health-speed-mps-max`, 500 m/s by -default) stops it, and no output file is written. With zero noise each epoch's time update is a -noiseless observation of the velocity and tilt errors, which moves their uncertainty out of the -shared covariance and into the particles' spread; resampling on metre-level fixes then destroys -that spread. [RBPF](../filters/rbpf.md#horizontal-process-noise-why-the-default-is-1-ms) gives the -full account. +The same command with the default `1 1` (drop the last flag) scores a horizontal RMSE of +1.870 m, a NEES of 3.141 and a containment of 1.000. On this short, clean synthetic drive the +zero-noise run finishes and its error is only slightly larger, but its covariance is no longer +honest: a NEES three times the ideal and 93% three-sigma containment mean the filter claims more +certainty than it has. With zero noise each epoch's time update is a noiseless observation of +the velocity and tilt errors, which moves their uncertainty out of the shared covariance and +into the particles' spread, and resampling on metre-level fixes then destroys that spread. On +the reference recording that overconfidence grows until the solution runs kilometres off; +[RBPF](../filters/rbpf.md#horizontal-process-noise-why-the-default-is-1-ms) gives the full +account. ## The same trajectory through the ESKF @@ -207,15 +218,15 @@ cargo run -p strapdown-core --example score_run -- cl_out.csv drive_truth.csv ```text aligned samples 5990 -horizontal RMSE 2.072 m -horizontal CEP50 1.738 m -horizontal CEP95 3.519 m -horizontal max 5.070 m -vertical RMSE 2.336 m -horizontal velocity RMSE 0.450 m/s -yaw RMSE 0.335 deg -position NEES (consistent at 3) 5.192 -3-sigma horizontal containment 1.000 +horizontal RMSE 1.574 m +horizontal CEP50 1.380 m +horizontal CEP95 2.653 m +horizontal max 3.994 m +vertical RMSE 1.183 m +horizontal velocity RMSE 0.371 m/s +yaw RMSE 0.498 deg +position NEES (consistent at 3) 2.879 +3-sigma horizontal containment 0.999 ``` One synthetic straight-line drive is not a ranking of the two filters. The RBPF exists for @@ -226,9 +237,13 @@ The gated comparison across scenarios, including the RBPF's own row, is on [RBPF](../filters/rbpf.md#sparse-gnss-what-this-structure-cannot-recover-from) documents a regime (one fix a minute) in which it diverges where the EKF does not. -For the unaided baseline, `strapdown-sim dr -i drive.csv -o dr_out.csv` scored the same way -reports no consistency metrics (a dead-reckoning run has no covariance) and a horizontal error -that grows to tens of kilometres over the ten minutes on this consumer-grade IMU. +For the unaided baseline, run +`strapdown-sim dr -i drive.csv -o dr_out.csv --health-speed-mps-max 1000`. The raised speed +limit is needed because `dr` applies the health limits, and the unaided velocity error passes the +default 500 m/s before the ten minutes are up; without it the run stops with an `OutOfRange` +error on `speed` and writes nothing. Scored the same way, it reports no consistency metrics (a +dead-reckoning run has no covariance) and a horizontal error that reaches 108.8 km by the end, a +horizontal RMSE of 41.3 km, on this consumer-grade IMU. ## Where next diff --git a/book/src/faq.md b/book/src/faq.md index 0c162b76..67f244e7 100644 --- a/book/src/faq.md +++ b/book/src/faq.md @@ -41,8 +41,9 @@ of ongoing PhD research, and `strapdown-geonav` in particular is experimental. See [System Requirements](./installation/requirements.md). In summary: - Rust 1.91 or later (`rust-toolchain.toml` fetches it); -- a C/C++ compiler and cmake 3.26 or newer, but only for features that build a C library - (HDF5, netCDF, `geonav`); `cargo build -p strapdown-core` needs neither; +- a C compiler for features that bundle a C library (`strapdown-sim`'s default `plotting`, + `mcap`), and a C/C++ compiler with cmake 3.26 or newer for HDF5, netCDF and `geonav`; + `cargo build -p strapdown-core` needs neither; - no system libraries at build time; - libfontconfig at *run* time, for `strapdown-sim`'s `--plot` (the `plotting` feature, on by default). It is loaded on demand, so a machine without it builds fine and fails at the @@ -136,13 +137,11 @@ tracks wall-clock time yet. What is measured and gated is navigation *accuracy*: ### Can I run simulations in parallel? -Across files, yes. Point `input` at a directory and set `parallel = true` in the config file -(or pass `--parallel` with `--config`), and the files are processed concurrently on a thread -pool. Each file's run is itself sequential, as navigation is. - -In this release, `--parallel` takes effect only together with `--config`. With a subcommand -(`strapdown-sim --parallel cl -i dir/ -o out/`) the directory's files are processed one after -another. +Across files, yes. Point the input at a directory and pass `--parallel` +(`strapdown-sim --parallel cl -i dir/ -o out/`), or set `parallel = true` in a config file, and +the files are processed concurrently on a thread pool. `--parallel` works the same way with +`dr`, `cl`, `pf` and `--config`; `syn`, `config` and `ol` refuse it. Each file's run is itself +sequential, as navigation is. ### The particle filter is slow diff --git a/book/src/filters/eskf.md b/book/src/filters/eskf.md index a0500c64..9afdff8b 100644 --- a/book/src/filters/eskf.md +++ b/book/src/filters/eskf.md @@ -174,15 +174,15 @@ It printed: ```text scored 3000 samples against truth -horizontal error: RMSE 8.49 m, max 54.72 m -end of outage: error 52.98 m, filter 1-sigma 378.53 m north, 379.08 m east -final accelerometer bias estimate: [-0.0541, -0.0320, -0.0694] m/s^2 -final gyroscope bias estimate: [0.00005, -0.00025, 0.00036] rad/s -final barometric bias estimate: 3.280 m +horizontal error: RMSE 20.65 m, max 105.72 m +end of outage: error 105.72 m, filter 1-sigma 336.56 m north, 341.87 m east +final accelerometer bias estimate: [0.0599, 0.0160, -0.0716] m/s^2 +final gyroscope bias estimate: [0.00023, -0.00028, 0.00041] rad/s +final barometric bias estimate: 4.012 m ``` The "end of outage" line is the last sample before GNSS returns. On this run the filter's own -horizontal uncertainty at that point is several times its actual error: it is conservative, +horizontal uncertainty at that point is about three times its actual error: it is conservative, not over-confident, while coasting. Whether that holds in general is what the consistency metrics on [Performance Baselines](../development/performance.md) measure: the NEES and the three-sigma containment columns, read together. diff --git a/book/src/filters/kalman.md b/book/src/filters/kalman.md index 57b5e272..23dc4b1f 100644 --- a/book/src/filters/kalman.md +++ b/book/src/filters/kalman.md @@ -93,12 +93,12 @@ cargo run -p strapdown-core --example kalman_filters It printed: ```text -ESKF, initialize_eskf: 15 states, 2.763 m from truth at the end, last update NIS 3.530 (3 dof, accepted: true) -EKF, initialize_ekf: 15 states, 2.761 m from truth at the end, last update NIS 3.530 (3 dof, accepted: true) -UKF, initialize_ukf: 15 states, 2.759 m from truth at the end, last update NIS 3.533 (3 dof, accepted: true) -ESKF, constructor: 15 states, 2.762 m from truth at the end, last update NIS 3.529 (3 dof, accepted: true) -EKF, constructor: 15 states, 2.762 m from truth at the end, last update NIS 3.529 (3 dof, accepted: true) -UKF, constructor: 15 states, 2.760 m from truth at the end, last update NIS 3.532 (3 dof, accepted: true) +ESKF, initialize_eskf: 15 states, 2.757 m from truth at the end, last update NIS 3.535 (3 dof, accepted: true) +EKF, initialize_ekf: 15 states, 2.760 m from truth at the end, last update NIS 3.531 (3 dof, accepted: true) +UKF, initialize_ukf: 15 states, 2.758 m from truth at the end, last update NIS 3.534 (3 dof, accepted: true) +ESKF, constructor: 15 states, 2.759 m from truth at the end, last update NIS 3.532 (3 dof, accepted: true) +EKF, constructor: 15 states, 2.760 m from truth at the end, last update NIS 3.531 (3 dof, accepted: true) +UKF, constructor: 15 states, 2.757 m from truth at the end, last update NIS 3.534 (3 dof, accepted: true) ``` On a short straight drive with a 2.5 m fix every second, all six end up where the fixes put diff --git a/book/src/filters/ukf.md b/book/src/filters/ukf.md index e5952c90..17a524b3 100644 --- a/book/src/filters/ukf.md +++ b/book/src/filters/ukf.md @@ -93,8 +93,8 @@ the transform samples the local nonlinearity rather than a shell several sigma o `core/tests/ukf_conditioning.rs` holds both ends of that: - `one_ulp_of_input_cannot_move_the_solution_by_a_centimetre` runs the UKF twice through a - 180 s, 50 Hz synthetic scenario with GNSS duty-cycled 60 s on and 60 s off, the second time with the initial - latitude moved by one ulp, and requires the final positions to agree within 1 cm. The test's + 180 s, 50 Hz synthetic scenario with GNSS duty-cycled 60 s off and 60 s on (no start phase, + so it opens with the outage), the second time with the initial latitude moved by one ulp, and requires the final positions to agree within 1 cm. The test's own documentation records 1.76 m at the old $\alpha$ and 0.18 mm at 0.1. - `the_shipped_sigma_point_weights_do_not_cancel_away_the_mantissa` checks that the default weights spend no more than 2.5 of `f64`'s digits on cancellation, so lowering $\alpha$ again diff --git a/book/src/geonav/overview.md b/book/src/geonav/overview.md index 9acc5945..8bcfcb28 100644 --- a/book/src/geonav/overview.md +++ b/book/src/geonav/overview.md @@ -124,7 +124,7 @@ start_phase_s = 0.0 | key | CLI counterpart | |---|---| -| `gravity_resolution`, `magnetic_resolution` | `--gravity-resolution`, `--magnetic-resolution` (values in snake case: `one_minute`, `two_minutes`, ...) | +| `gravity_resolution`, `magnetic_resolution` | `--gravity-resolution`, `--magnetic-resolution` (in the file the values are snake case, `one_minute`, `two_minutes`, ...; on the command line, kebab case) | | `gravity_map_file`, `magnetic_map_file` | `--gravity-map-file`, `--magnetic-map-file` | | `gravity_noise_std`, `magnetic_noise_std` | `--gravity-noise-std`, `--magnetic-noise-std` | | `gravity_bias`, `magnetic_bias` | `--gravity-bias`, `--magnetic-bias` | @@ -156,7 +156,8 @@ both maps for each trajectory and writes them under exactly these names; see The output CSV always carries `gravity_bias`, `gravity_bias_cov`, `magnetic_bias` and `magnetic_bias_cov` columns (see [Output Format](../user-guide/output-format.md)). For each channel a geophysically aided run uses, they hold the filter's estimate of the map bias, in -the map's unit, and its variance; for a channel the run does not use, they are `NaN`. +the map's unit, and its variance; for a channel the run does not use, they are empty (a +gravity-only `pf --geo` run leaves both `magnetic_bias` cells blank). ## Example diff --git a/book/src/gnss/scenarios.md b/book/src/gnss/scenarios.md index eeeaf7c1..8c07dfe0 100644 --- a/book/src/gnss/scenarios.md +++ b/book/src/gnss/scenarios.md @@ -24,7 +24,7 @@ that spelling is still accepted as an alias; the recipes under `conf/` use it. | `magnetometer_scheduler` | scheduler | `fixed_interval`, 1 s | when magnetometer heading is delivered | | `baro_noise_std_m` | m (1σ) | 2.236 (√5) | barometric altitude noise standard deviation | | `baro_bias_index` | integer or unset | unset | which filter state holds the barometric bias (library use; see below) | -| `seed` | integer | 42 | seed for the fault model's random draws | +| `seed` | integer or unset | unset | seed for the fault model's random draws; unset takes the file's top-level `seed`, or `DEFAULT_AIDING_SEED` (42) when an event stream is built from Rust | | `max_imu_gap_s` | s | 5.0 | longest inertial gap tolerated before the run is refused; `<= 0` disables | A complete example that runs as written (`input`/`output` pointed at a synthetic trajectory): diff --git a/book/src/quick-start.md b/book/src/quick-start.md index 0f88752c..27f5e05f 100644 --- a/book/src/quick-start.md +++ b/book/src/quick-start.md @@ -16,13 +16,15 @@ See [Installation](./installation/installation.md) for the details and the build ```console $ strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 -2026-10-01 20:37:53.960 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) -2026-10-01 20:37:53.971 [INFO] - Sensor records written to synthetic.csv +2026-10-01 21:34:29.872 [INFO] - Generated 6000 synthetic records (600.0 s at 10 Hz) +2026-10-01 21:34:29.880 [INFO] - Sensor records written to synthetic.csv ``` With no other flags, `syn` writes ten minutes of a **stationary** platform at 0° N, 0° E and zero altitude, sampled at 10 Hz, with a consumer-grade IMU error model (`--imu-grade consumer`) -and a noisy GNSS fix on every row (2.5 m horizontal, 5 m vertical, one sigma). The flags to +and a noisy GNSS fix on every row (2.5 m horizontal, 5 m vertical and 0.5 m/s per velocity +axis, one sigma). The barometer is an independent sensor: `relativeAltitude` is computed from a +noisy pressure (about 27 Pa, or 2.24 m, one sigma by default). The flags to move it, rotate it, change the IMU grade or the sample rate are listed by `strapdown-sim syn --help`; see [Synthetic Trajectories](./user-guide/synthetic.md). @@ -71,13 +73,13 @@ Representation](./user-guide/state-representation.md) lays the states out. The log begins: ```text -2026-10-01 20:39:38.048 [INFO] - Running in closed-loop mode with Error-State Kalman Filter (ESKF) -2026-10-01 20:39:38.048 [INFO] - Processing file: synthetic.csv -2026-10-01 20:39:38.076 [INFO] - Read 6000 records from synthetic.csv -2026-10-01 20:39:38.076 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 -2026-10-01 20:39:38.077 [INFO] - Initialized event stream with 13198 events -2026-10-01 20:39:38.077 [INFO] - Initialized ESKF -2026-10-01 20:39:38.077 [INFO] - Starting closed-loop navigation filter with 13198 events +2026-10-01 21:34:29.887 [INFO] - Running in closed-loop mode with Error-State Kalman Filter (ESKF) +2026-10-01 21:34:29.887 [INFO] - Processing file: synthetic.csv +2026-10-01 21:34:29.903 [INFO] - Read 6000 records from synthetic.csv +2026-10-01 21:34:29.903 [INFO] - Mechanizing input as NED: mean vertical specific force over the first 10 record(s) is -9.754 m/s^2 against a local gravity of 9.780 m/s^2 +2026-10-01 21:34:29.904 [INFO] - Initialized event stream with 13198 events +2026-10-01 21:34:29.904 [INFO] - Initialized ESKF +2026-10-01 21:34:29.904 [INFO] - Starting closed-loop navigation filter with 13198 events ``` followed by a progress line every ten events and, at the end, `Results written to eskf.csv`. @@ -95,7 +97,7 @@ last row of `eskf.csv` is: ```text timestamp,latitude,longitude,altitude,velocity_north,velocity_east,velocity_vertical,roll,pitch,yaw -2025-01-01T00:09:59.900Z,-2.097736146341953e-6,-1.9488296989009557e-6,-0.6980524721961896,-0.06949807183747997,-0.08759779166041143,0.05404302021868538,-0.00384387940069942,-0.0007849300822555636,0.005312090469945534 +2025-01-01T00:09:59.900Z,-1.244253386640393e-6,-1.7581270456255539e-6,-0.42194145354743773,0.015484151883163197,0.0568049993997672,0.038121929489373735,0.009803423792142807,0.006982138212381999,-0.002044723575180731 ``` Latitude and longitude are in degrees, altitude in metres (positive up), velocities in m/s and @@ -128,11 +130,14 @@ elements plus the barometric bias. See [Kalman Filters](./filters/kalman.md). ## 4. Dead reckoning ```bash -strapdown-sim --log-level warn dr -i synthetic.csv -o dr.csv +strapdown-sim --log-level warn dr -i synthetic.csv -o dr.csv --health-speed-mps-max 1000 ``` `dr` propagates the IMU from the first record with no aiding at all, so its error grows without -bound. (`ol` is a different subcommand, reserved for an open-loop mode that is **not +bound. It applies the same health limits as the filters, and on this file the unaided velocity +passes the default 500 m/s ceiling about 580 s in: without `--health-speed-mps-max 1000` the run +stops with `Error: OutOfRange { what: "speed", value: 500.12376354277296, min: 0.0, max: 500.0 }`, +exits 1 and writes no file. (`ol` is a different subcommand, reserved for an open-loop mode that is **not implemented**: it writes no output. Use `dr` for dead reckoning.) ## 5. The particle filter @@ -189,8 +194,11 @@ for path in sys.argv[1:]: errors = [] for t, e in zip(truth, est): d_north = math.radians(float(e["latitude"]) - float(t["latitude"])) * R - d_east = (math.radians(float(e["longitude"]) - float(t["longitude"])) - * R * math.cos(math.radians(float(t["latitude"])))) + d_east = ( + math.radians(float(e["longitude"]) - float(t["longitude"])) + * R + * math.cos(math.radians(float(t["latitude"]))) + ) errors.append(math.hypot(d_north, d_east)) rms = math.sqrt(sum(x * x for x in errors) / len(errors)) print(f"{path:16s} horizontal RMS {rms:10.3f} m final {errors[-1]:10.3f} m") @@ -198,19 +206,19 @@ for path in sys.argv[1:]: ```console $ python3 score.py dr.csv eskf.csv ekf.csv ukf.csv pf.csv eskf_outage.csv -dr.csv horizontal RMS 41518.686 m final 109415.047 m -eskf.csv horizontal RMS 0.739 m final 0.318 m -ekf.csv horizontal RMS 0.737 m final 0.318 m -ukf.csv horizontal RMS 0.737 m final 0.318 m -pf.csv horizontal RMS 1.006 m final 0.667 m -eskf_outage.csv horizontal RMS 18.347 m final 0.318 m +dr.csv horizontal RMS 41364.109 m final 109129.025 m +eskf.csv horizontal RMS 0.524 m final 0.239 m +ekf.csv horizontal RMS 0.522 m final 0.239 m +ukf.csv horizontal RMS 0.522 m final 0.240 m +pf.csv horizontal RMS 0.921 m final 0.528 m +eskf_outage.csv horizontal RMS 15.671 m final 0.240 m ``` Read these as a demonstration of the workflow, not as a comparison of the filters: they come from one stationary trajectory and one seed. Unaided, the consumer-grade IMU drifts by about 110 km in ten minutes. Every aided run stays at the metre level. The outage run's final error -matches the uninterrupted run's because the last 100 s have GNSS again, while its RMS carries -the four outages. The filters' measured accuracy on the reference scenarios is on the +matches the uninterrupted run's to within a millimetre because the last 100 s have GNSS again, +while its RMS carries the four outages. The filters' measured accuracy on the reference scenarios is on the [Performance Baselines](./development/performance.md) page. ## 8. The same run from a scenario file diff --git a/book/src/user-guide/closed-loop.md b/book/src/user-guide/closed-loop.md index fde9d695..b0a337e2 100644 --- a/book/src/user-guide/closed-loop.md +++ b/book/src/user-guide/closed-loop.md @@ -124,8 +124,8 @@ strapdown-sim cl -i synthetic.csv -o results/duty.csv --sched duty --on-s 100 - ``` On the 600 s, 10 Hz trajectory above, the pass-through run's event stream holds 13,198 events -and the duty-cycle run's 11,199 -- the difference is the 1,999 GNSS fixes the two outages -withhold, as `Initialized event stream with ... events` reports at `info` level. +and the duty-cycle run's 11,199 -- the difference is the 1,999 GNSS fixes its four 50 s outages +(0-50 s, 150-200 s, 300-350 s and 450-500 s) withhold, as `Initialized event stream with ... events` reports at `info` level. ## GNSS faults: what the fixes say @@ -210,9 +210,9 @@ installed. ```console $ strapdown-sim cl -i synthetic.csv -o results/gated.csv --gate-confidence 0.999 --log-level warn -[WARN] - closed-loop run completed with 78 of 13198 events gated out by the innovation test +[WARN] - closed-loop run completed with 7 of 13198 events gated out by the innovation test $ strapdown-sim cl -i synthetic.csv -o results/bad.csv --gate-confidence 1.5 -Error: OutOfRange { what: "innovation gate confidence", value: 1.5, min: 0.0, max: 1.0 } +Error: "the --gate-* flags: innovation gate confidence = 1.5 is outside the valid range [0, 1]" ``` The gate is distinct from the run-level `--nis-pos-max`/`--nis-pos-consec-fail` health check, diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index e7c1e173..f1de0b0c 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -118,7 +118,7 @@ restructured, which are refused by name; see [below](#particle_filter). | `seed` | `42` | the run's seed: the particle filter's, and the fault models' unless `[aiding] seed` is set; see [Seeds](#seeds) | | `is_enu` | `false` | the input records are ENU rather than NED; as `--enu` | | `parallel` | `false` | process a directory's files concurrently | -| `generate_plot` | `false` | write a PNG plot beside each closed-loop or particle-filter result | +| `generate_plot` | `false` | write a PNG plot beside each dead-reckoning, closed-loop or particle-filter result | The input and output path rules -- directory versus file, CSV only, refusing to overwrite an input -- are the command line's; see [Running Simulations](./simulations.md#input-and-output-paths). @@ -134,9 +134,10 @@ The input and output path rules -- directory versus file, CSV only, refusing to | `open-loop` | `ol` | -- (**not implemented**: exits with an error) | `mode = "dr"` is rejected as an unknown variant. Every mode reads `[aiding]`, -`[execution_limits]`, `[health_limits]` and `[logging]`, except that dead reckoning ignores the -aiding and the limits as it does on the command line, and synthetic mode ignores everything but -`[synthetic]`. +`[execution_limits]`, `[health_limits]` and `[logging]`, with two exceptions. Dead reckoning has +no aiding, so it ignores `[aiding]`; it applies the execution limits and the position and speed +health limits, as `dr` does on the command line, but not `cov_diag_max` or the NIS pair. Synthetic +mode reads only `[synthetic]` and `[logging]`. ## `[aiding]` @@ -456,7 +457,7 @@ value means fewer measurements. Its old name, `geo_frequency_s`, is still accept ## `[synthetic]` -Read when `mode = "synthetic"`, and the only section that mode reads. `output` and `duration_s` +Read when `mode = "synthetic"`, and the only section that mode reads besides `[logging]`. `output` and `duration_s` are required; the rest mirror the `syn` flags, with the IMU grade spelled `imu_quality` and the initial state in a `[synthetic.initial_state]` table. The full key list and a worked example are on [Synthetic Trajectories](./synthetic.md#the-synthetic-configuration-section). diff --git a/book/src/user-guide/coordinate-frames.md b/book/src/user-guide/coordinate-frames.md index 4cd22292..4c0a3566 100644 --- a/book/src/user-guide/coordinate-frames.md +++ b/book/src/user-guide/coordinate-frames.md @@ -96,7 +96,6 @@ on the field `is_enu`, and the message names the flag to change. ```console $ strapdown-sim --log-level warn cl --enu -i synthetic.csv -o eskf_enu.csv -2026-10-01 20:42:51.596 [ERROR] - Error running closed-loop simulation on synthetic.csv: invalid configuration for `is_enu`: mean vertical specific force over the first 10 record(s) is -9.75 m/s^2, but ENU mechanization expects +9.78 m/s^2 at rest: these look like NED records. Mechanizing them as ENU would double-count gravity and integrate at 2 g. Declare the frame that matches the data (drop `--enu`, or set `is_enu = false` in the config file), or re-record it in ENU. Error: InvalidConfiguration { field: "is_enu", reason: "mean vertical specific force over the first 10 record(s) is -9.75 m/s^2, but ENU mechanization expects +9.78 m/s^2 at rest: these look like NED records. Mechanizing them as ENU would double-count gravity and integrate at 2 g. Declare the frame that matches the data (drop `--enu`, or set `is_enu = false` in the config file), or re-record it in ENU." } ``` diff --git a/book/src/user-guide/data-format.md b/book/src/user-guide/data-format.md index 2f6fd8c8..8ab26b40 100644 --- a/book/src/user-guide/data-format.md +++ b/book/src/user-guide/data-format.md @@ -32,7 +32,7 @@ example). | `roll`, `pitch`, `yaw` | rad | device attitude as Euler angles, in the app's own convention | | `mag_x`, `mag_y`, `mag_z` | µT | magnetometer, body frame | | `relativeAltitude` | m | barometric altitude change since the recording started | -| `pressure` | mbar in a Sensor Logger export; Pa in `syn` output | barometric pressure | +| `pressure` | hPa (mbar) | barometric pressure; `syn` writes hPa too | | `grav_x`, `grav_y`, `grav_z` | m/s² | the device's gravity estimate, body frame | The IMU data is raw: the accelerometer columns include gravity, and the strapdown mechanization @@ -96,10 +96,11 @@ no rows at all is an error, so a wrong schema cannot silently produce an empty r ## Synthetic data `strapdown-sim syn` writes exactly this layout, in NED, with GNSS on every row and the accuracy -columns set to the configured noise levels. Its truth is a separate file, written with +columns set from the configured noise levels. Its truth is a separate file, written with `--no-noise` in the [output format](./output-format.md) rather than this one. The details of what -`syn` puts in each column -- including that its `relativeAltitude` is derived from the noisy GNSS -altitude and its `speed`/`bearing` are noise-free -- are on +`syn` puts in each column -- including that its `relativeAltitude` is derived from a noisy +pressure independent of the GNSS altitude, and that its `speed`/`bearing` carry their own +velocity noise, reported in `speedAccuracy` -- are on [Synthetic Trajectories](./synthetic.md#what-it-writes). ## Frame Convention @@ -119,9 +120,12 @@ strapdown-sim dr -i recording.csv -o results/ --enu # Anything generated by `syn` -- NED, so no flag strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 -strapdown-sim dr -i synthetic.csv -o results/ +strapdown-sim dr -i synthetic.csv -o results/ --health-speed-mps-max 1000 ``` +(The raised speed limit is for this ten-minute unaided run, whose drift passes the default +500 m/s; see [Dead Reckoning](./dead-reckoning.md).) + The flag applies to `dr`, `ol`, `cl` and `pf` alike, and the same setting is available as `is_enu = true` at the top level of a config file. `syn --enu` generates ENU data, so `syn --enu` and `dr --enu` pair up the way plain `syn` and plain `dr` do. diff --git a/book/src/user-guide/dead-reckoning.md b/book/src/user-guide/dead-reckoning.md index 42c004ad..d1d6209b 100644 --- a/book/src/user-guide/dead-reckoning.md +++ b/book/src/user-guide/dead-reckoning.md @@ -6,7 +6,7 @@ output is the free-inertial solution, with all of its drift. ```bash strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 -strapdown-sim dr -i synthetic.csv -o results/dr.csv +strapdown-sim dr -i synthetic.csv -o results/dr.csv --health-speed-mps-max 1000 ``` ```console @@ -16,7 +16,11 @@ strapdown-sim dr -i synthetic.csv -o results/dr.csv [INFO] - Results written to results/dr.csv ``` -(Timestamps trimmed from the log lines.) The flags are only the shared ones -- `-i`, `-o`, +(Timestamps trimmed from the log lines.) `--health-speed-mps-max 1000` is there because `dr` +applies the health limits: on this ten-minute file the unaided velocity passes the default +500 m/s about 580 s in, and without the flag the run stops with +`Error: OutOfRange { what: "speed", value: 500.12376354277296, min: 0.0, max: 500.0 }` and writes +no file. The flags are only the shared ones -- `-i`, `-o`, `--enu` and the limit flags -- described in [Running Simulations](./simulations.md). `dr` takes no seed, because nothing in it is random. @@ -27,7 +31,7 @@ The initial state is taken from the first record: | State | Source in the first record | |---|---| | latitude, longitude, altitude | `latitude`, `longitude`, `altitude` | -| north and east velocity | `speed` and `bearing` (a NaN in either gives zero) | +| north and east velocity | `speed` and `bearing` (a NaN in either gives zero). On `syn` output these carry the GNSS velocity noise, so a `dr` run starts with that velocity error | | vertical velocity | zero; the format carries no vertical rate | | attitude | the quaternion `qw, qx, qy, qz` (not the `roll`/`pitch`/`yaw` columns) | @@ -45,7 +49,7 @@ Three practical consequences of having no filter: - **Every record must carry all six inertial channels.** The filters skip a record with a missing accelerometer or gyroscope value and coast across the gap; `dr` has no notion of a gap and fails on it. Blanking a single `acc_x` cell in a synthetic file stops the run with - `Error: NonFinite { what: "propagated attitude matrix" }`, where `cl` on the same file logs a + `Error: NonFinite { what: "filter state" }`, where `cl` on the same file logs a tolerated 0.2 s gap and finishes. - **The position and speed limits apply.** `dr` checks the wall-clock budgets (`--max-wall-clock-*`, `--max-no-progress-s`) and the latitude, longitude, altitude and speed diff --git a/book/src/user-guide/logging.md b/book/src/user-guide/logging.md index 28412a69..8c485e1b 100644 --- a/book/src/user-guide/logging.md +++ b/book/src/user-guide/logging.md @@ -81,8 +81,8 @@ at all.** `from_env` and `from_default_env` do -- and nothing in the crate calls `parse_env`. Verified: ```console -$ RUST_LOG=off strapdown-sim dr -i input.csv -o output.csv -2026-09-17 23:45:46.856 [INFO] - Running in Dead Reckoning mode with input: input.csv +$ RUST_LOG=off strapdown-sim dr -i input.csv -o output.csv 2>&1 | head -1 +2026-10-01 21:42:29.555 [INFO] - Running in Dead Reckoning mode with input: input.csv ``` `--log-level` is the only control, and because `filter_level` sets one global level there are diff --git a/book/src/user-guide/output-format.md b/book/src/user-guide/output-format.md index 1cd26751..a10fe53f 100644 --- a/book/src/user-guide/output-format.md +++ b/book/src/user-guide/output-format.md @@ -107,7 +107,7 @@ strapdown-core = { git = "https://github.com/jbrodovsky/strapdown-rs", features ``` `hdf5` and `netcdf` compile libhdf5 and libnetcdf from vendored sources, so they need a C -compiler and cmake 3.26 or newer; `mcap` is Rust only. The `full` feature turns on all three +compiler and cmake 3.26 or newer; `mcap` needs a C compiler for its bundled LZ4 and Zstandard. The `full` feature turns on all three plus `clap`. See [System Requirements](../installation/requirements.md). `TestDataRecord`, the input record, has the same three writers and readers, behind the same diff --git a/book/src/user-guide/particle-filter.md b/book/src/user-guide/particle-filter.md index 4c4fe1ea..ed0f9010 100644 --- a/book/src/user-guide/particle-filter.md +++ b/book/src/user-guide/particle-filter.md @@ -78,9 +78,12 @@ $\sqrt{\Delta t}$, so the variance grows linearly in elapsed time whatever the l on horizontal position, and the library's `RbpfConfig` keeps that zero. With GNSS-rate fixes on MEMS data a zero-noise cloud collapses and the filter diverges, so `strapdown-sim` and its configuration file default to 1 m/√s on each axis, the value the repository's `conf/` recipes -use. Pass `0 0` to run the paper's model -- on the 600 s synthetic trajectory above it diverges -until the health monitor stops the run (`Error: OutOfRange { what: "speed", ... max: 500.0 }`), -while the default `1 1` and `0.5 0.5` both finish. +use. Pass `0 0` to run the paper's model. On the stationary 600 s synthetic trajectory above all +three settings finish: scored against `syn --no-noise` truth with the +[Quick Start](../quick-start.md#7-score-the-runs)'s script, `0 0` gives a horizontal RMS of +1.516 m, the default `1 1` 0.921 m and `0.5 0.5` 0.704 m (`0 0` with roughening off as well +also finishes, at 1.715 m). The divergence the default guards against is the one the library +documents for GNSS-rate fixes on recorded MEMS data; a stationary synthetic run does not show it. `--horizontal-process-noise-std-m` takes exactly two values, north then east, separated by a comma or a space: `--horizontal-process-noise-std-m 0.5,0.5` and diff --git a/book/src/user-guide/simulations.md b/book/src/user-guide/simulations.md index d5586b20..6e8a2bef 100644 --- a/book/src/user-guide/simulations.md +++ b/book/src/user-guide/simulations.md @@ -98,10 +98,10 @@ The rules, all from `sim/src/common.rs`: So `dr -i batch -o batch` is refused rather than overwriting the recordings. ```console -$ strapdown-sim dr -i batch -o out_file/run.csv +$ strapdown-sim --log-level warn dr -i batch -o out_file/run.csv $ ls out_file run_a.csv run_b.csv -$ strapdown-sim dr -i batch/a.csv -o batch/a.csv +$ strapdown-sim --log-level warn dr -i batch/a.csv -o batch/a.csv Error: "Refusing to write results to 'batch/a.csv': that is the input file. Pass a different --output path." ``` @@ -159,12 +159,15 @@ the `_pos` in the name. It is a run-level divergence check, separate from the measurements. ```console -$ strapdown-sim cl -i cruise.csv -o hl/out.csv --health-speed-mps-max 10 -Error: OutOfRange { what: "speed", value: 49.999902224914834, min: 0.0, max: 10.0 } +$ strapdown-sim --log-level warn cl -i cruise.csv -o hl/out.csv --health-speed-mps-max 10 +[ERROR] - Health fail after propagate at 2025-01-01 00:00:00.020 UTC (#0): speed = 49.79808422315322 is outside the valid range [0, 10] +[ERROR] - Error running closed-loop simulation: speed = 49.79808422315322 is outside the valid range [0, 10] +Error: OutOfRange { what: "speed", value: 49.79808422315322, min: 0.0, max: 10.0 } ``` (`cruise.csv` is a 50 m/s synthetic trajectory; see -[Synthetic Trajectories](./synthetic.md#a-moving-trajectory).) +[Synthetic Trajectories](./synthetic.md#a-moving-trajectory). The speed is not exactly 50 m/s +because the initial velocity is taken from the first record's noisy `speed` and `bearing`.) Not every limit can apply to every mode, and each flag's `--help` says which modes apply it: diff --git a/book/src/user-guide/synthetic.md b/book/src/user-guide/synthetic.md index cdd0b3f0..1af347e1 100644 --- a/book/src/user-guide/synthetic.md +++ b/book/src/user-guide/synthetic.md @@ -68,10 +68,16 @@ values with `=`: `--longitude-deg=-75`. | `--imu-grade ` | `consumer` | IMU noise and bias levels; see [IMU grades](#imu-grades) | | `--gnss-horizontal-noise-m ` | `2.5` | one-sigma white noise on the GNSS horizontal position, metres | | `--gnss-vertical-noise-m ` | `5` | one-sigma white noise on the GNSS altitude, metres | -| `--baro-noise-std-pa ` | `50` | one-sigma white noise on the `pressure` column, pascals | +| `--gnss-velocity-noise-mps ` | `0.5` | one-sigma white noise on the north and east velocity behind `speed` and `bearing`, m/s per axis | +| `--baro-noise-std-pa ` | about `26.86` | one-sigma white noise on the barometric pressure, pascals. `relativeAltitude` is computed from the noisy pressure, at about 0.083 m per pascal near sea level | | `--mag-noise-std-ut ` | `0.5` | one-sigma white noise on each magnetometer axis, microtesla | | `--mag-hard-iron-std-ut ` | `0` | standard deviation of a hard-iron offset drawn once per run and held, microtesla per axis | +The barometric default is not a round number because it is derived: it is the pressure noise +whose altitude equivalent at sea level is the 2.24 m ($\sqrt 5$ m) one-sigma the filters assume +for a barometer (`measurements::BAROMETRIC_ALTITUDE_NOISE_M`), so a default filter's $R$ +describes a default synthetic barometer. + Hard iron is off by default on purpose: a constant body-frame field biases the computed heading in a way no filter here can observe, so with it on the yaw error measures the offset rather than the filter. @@ -108,11 +114,13 @@ definitions are on [Input Data Format](./data-format.md). What `syn` puts in the | `time` | RFC 3339 UTC timestamps starting at `2025-01-01T00:00:00Z`, one per sample | | `acc_*`, `gyro_*` | perfect IMU plus the grade's constant bias and white noise, body frame | | `latitude`, `longitude`, `altitude` | truth plus white GNSS noise, **on every row** | -| `speed`, `bearing` | the truth's ground speed and track, **without noise** | -| `horizontalAccuracy`, `speedAccuracy`, `bearingAccuracy` | the `--gnss-horizontal-noise-m` value | +| `speed`, `bearing` | ground speed (m/s) and track (degrees) of the true north/east velocity plus `--gnss-velocity-noise-mps` white noise on each axis | +| `horizontalAccuracy` | the `--gnss-horizontal-noise-m` value | | `verticalAccuracy` | the `--gnss-vertical-noise-m` value | -| `relativeAltitude` | the *noisy* GNSS altitude minus the initial altitude | -| `pressure` | standard-atmosphere pressure at the true altitude plus `--baro-noise-std-pa` noise, Pa | +| `speedAccuracy` | the `--gnss-velocity-noise-mps` value | +| `bearingAccuracy` | $\operatorname{atan2}(\sigma_v, \text{speed})$ in degrees: the angle the velocity noise subtends at the row's speed, which tends to 90° as the platform stops | +| `pressure` | isothermal-atmosphere pressure at the true altitude plus `--baro-noise-std-pa` noise, **hPa** (the unit a Sensor Logger export uses) | +| `relativeAltitude` | the height of the noisy pressure relative to the **first** noisy pressure, through the same isothermal atmosphere, m. It is `0` on the first row | | `mag_*` | the World Magnetic Model field at the true position and the record's date, rotated into the body frame, plus noise and any hard iron, µT | | `grav_*` | local gravity rotated into the body frame, m/s² | | `qw`, `qx`, `qy`, `qz`, `roll`, `pitch`, `yaw` | the true attitude, as a quaternion and as Euler angles in radians | @@ -124,12 +132,15 @@ Three properties of these records matter when you use them: - **GNSS is on every row**, so with the default `--sched passthrough` a filter receives GNSS at the full sample rate -- 10 Hz by default. For a realistic 1 Hz receiver, run the filter with `--sched fixed --interval-s 1`, as the accuracy suite's `syn_cruise_1hz` scenario does. -- **The barometric channel is the GNSS altitude.** `relativeAltitude` is derived from the noisy - GNSS altitude column, not from `pressure`, so the barometer measurement the filters build from - it carries the same noise sample as the GNSS fix, and `--baro-noise-std-pa` changes only the - `pressure` column, which no estimator reads. -- **The velocity aiding is exact.** `speed` and `bearing` carry no noise, while - `speedAccuracy` advertises the horizontal position noise as their one-sigma. +- **The barometer is an independent sensor with a constant bias.** `relativeAltitude` comes + from the noisy pressure, not from the GNSS altitude, so its noise is independent of the GNSS + fix and set by `--baro-noise-std-pa`. Because it is referenced to the first noisy reading, as + a phone's is, that reading's error becomes a constant offset in every later row: the + barometric bias the Kalman filters estimate. +- **The velocity aiding is noisy, and says by how much.** `speed` and `bearing` carry + `--gnss-velocity-noise-mps` of noise per axis, and `speedAccuracy` reports that one-sigma, + which is what the filters weight the velocity fix with. On the stationary default the noise is + all there is, so `speed` is a small positive number and `bearing` is random. ### Truth with `--no-noise` @@ -183,9 +194,10 @@ The same flags and seed produce a byte-identical file: two runs of - every random draw comes from a generator seeded by `--seed`; - the start time is fixed at `2025-01-01T00:00:00Z` rather than taken from the clock, which also fixes the date the magnetic model is evaluated at; -- the magnetometer noise and the hard-iron offset draw from their own streams, so changing - `--mag-noise-std-ut` changes only the three `mag_*` columns and leaves the IMU, GNSS and - barometer noise untouched. +- the magnetometer noise, the hard-iron offset and the GNSS velocity noise draw from their own + streams, so changing `--mag-noise-std-ut` changes only the three `mag_*` columns, and changing + `--gnss-velocity-noise-mps` changes only `speed`, `bearing`, `speedAccuracy` and + `bearingAccuracy`. The IMU, GNSS position and barometer noise are untouched by either. Combined with the exact truth from `--no-noise`, that makes a synthetic trajectory the one input on which an error can be attributed to the estimator rather than to an unknown reference. @@ -208,7 +220,8 @@ seed = 42 no_noise = false gnss_horizontal_noise_m = 3.0 # default 2.5 gnss_vertical_noise_m = 5.0 # default 5 -baro_noise_std_pa = 30.0 # default 50 +gnss_velocity_noise_mps = 0.5 # default 0.5 +baro_noise_std_pa = 30.0 # default about 26.86 mag_noise_std_ut = 0.5 mag_hard_iron_std_ut = 0.0 @@ -233,5 +246,5 @@ $ strapdown-sim --config syn.toml `output` and `duration_s` have no defaults: a `[synthetic]` section without them is refused with ``missing field `duration_s` ``. Leave out `[synthetic.initial_state]` entirely and the vehicle starts at rest at 0, 0, 0; include it and `latitude_deg`, `longitude_deg` and `altitude_m` must -all be given. The other top-level keys of a configuration file (`input`, `[closed_loop]` and so -on) are ignored in synthetic mode. +all be given. The other keys of a configuration file (`input`, `[closed_loop]` and so on) are +ignored in synthetic mode, except `[logging]`, which sets the log level and file as in any mode. From 0963ce91fda34b663502cd450d128502a091a79c Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 21:56:22 -0400 Subject: [PATCH 24/26] Hold dr to its wall-clock budget, not to the health bounds Earlier on this branch, dr began applying every health limit the filters are held to. A plain strapdown-sim dr on a ten-minute syn trajectory then stopped at the default 500 m/s speed bound, exited 1 and wrote nothing. The book had to pass --health-speed-mps-max 1000 everywhere dr appeared, and the README's quick start would have failed for anyone who ran it. The health bounds exist to stop a diverged filter from burning the rest of its budget. An unaided MEMS arc is expected to leave them, and the arc is the result. dr now passes only the execution limits. A non-finite state still stops it, and dead_reckoning_with_limits still takes health limits for library callers who want them. The flags' help, the config docs and the book no longer claim dr applies them, and the speed-limit workarounds are gone. Co-Authored-By: Claude Opus 5.5 --- book/src/development/architecture.md | 7 ++-- book/src/examples/tutorial-particle-filter.md | 5 +-- book/src/quick-start.md | 9 ++-- book/src/user-guide/configuration.md | 4 +- book/src/user-guide/data-format.md | 5 +-- book/src/user-guide/dead-reckoning.md | 22 ++++------ book/src/user-guide/simulations.md | 6 +-- core/src/sim.rs | 3 +- sim/src/main.rs | 41 ++++++++----------- 9 files changed, 43 insertions(+), 59 deletions(-) diff --git a/book/src/development/architecture.md b/book/src/development/architecture.md index 8223f9a0..2fe59165 100644 --- a/book/src/development/architecture.md +++ b/book/src/development/architecture.md @@ -90,9 +90,10 @@ The variations are: - **Dead reckoning** (`dr`) skips the event stream: `sim::dead_reckoning_with_limits(records, is_enu, health_limits, execution_limits)` - mechanizes the IMU rows directly, checks the position, speed and wall-clock limits after each - step, and reports no covariance. `sim::dead_reckoning(records, is_enu)` is the same with no - limits. + mechanizes the IMU rows directly, checks whichever limits it is given after each step, and + reports no covariance. `strapdown-sim` passes only the wall-clock limits, because an unaided + arc is expected to leave the health bounds. `sim::dead_reckoning(records, is_enu)` is the + same with no limits. - **Closed loop** (`cl`) builds one of the Kalman filters and calls `run_closed_loop`; with geophysical maps it calls `run_closed_loop_with_geo`, which also reports the map biases. - **Particle filter** (`pf`) consumes the same `EventStream` through an event loop in diff --git a/book/src/examples/tutorial-particle-filter.md b/book/src/examples/tutorial-particle-filter.md index 64b5ddb3..5c954fb4 100644 --- a/book/src/examples/tutorial-particle-filter.md +++ b/book/src/examples/tutorial-particle-filter.md @@ -238,10 +238,7 @@ The gated comparison across scenarios, including the RBPF's own row, is on (one fix a minute) in which it diverges where the EKF does not. For the unaided baseline, run -`strapdown-sim dr -i drive.csv -o dr_out.csv --health-speed-mps-max 1000`. The raised speed -limit is needed because `dr` applies the health limits, and the unaided velocity error passes the -default 500 m/s before the ten minutes are up; without it the run stops with an `OutOfRange` -error on `speed` and writes nothing. Scored the same way, it reports no consistency metrics (a +`strapdown-sim dr -i drive.csv -o dr_out.csv`. Scored the same way, it reports no consistency metrics (a dead-reckoning run has no covariance) and a horizontal error that reaches 108.8 km by the end, a horizontal RMSE of 41.3 km, on this consumer-grade IMU. diff --git a/book/src/quick-start.md b/book/src/quick-start.md index 27f5e05f..4c6f5be1 100644 --- a/book/src/quick-start.md +++ b/book/src/quick-start.md @@ -130,14 +130,13 @@ elements plus the barometric bias. See [Kalman Filters](./filters/kalman.md). ## 4. Dead reckoning ```bash -strapdown-sim --log-level warn dr -i synthetic.csv -o dr.csv --health-speed-mps-max 1000 +strapdown-sim --log-level warn dr -i synthetic.csv -o dr.csv ``` `dr` propagates the IMU from the first record with no aiding at all, so its error grows without -bound. It applies the same health limits as the filters, and on this file the unaided velocity -passes the default 500 m/s ceiling about 580 s in: without `--health-speed-mps-max 1000` the run -stops with `Error: OutOfRange { what: "speed", value: 500.12376354277296, min: 0.0, max: 500.0 }`, -exits 1 and writes no file. (`ol` is a different subcommand, reserved for an open-loop mode that is **not +bound: on this file the unaided velocity error passes 500 m/s before the ten minutes are up. +That divergence is the result, so `dr` applies the wall-clock budgets but none of the health +bounds the filters are held to. (`ol` is a different subcommand, reserved for an open-loop mode that is **not implemented**: it writes no output. Use `dr` for dead reckoning.) ## 5. The particle filter diff --git a/book/src/user-guide/configuration.md b/book/src/user-guide/configuration.md index f1de0b0c..8f1718cf 100644 --- a/book/src/user-guide/configuration.md +++ b/book/src/user-guide/configuration.md @@ -135,8 +135,8 @@ The input and output path rules -- directory versus file, CSV only, refusing to `mode = "dr"` is rejected as an unknown variant. Every mode reads `[aiding]`, `[execution_limits]`, `[health_limits]` and `[logging]`, with two exceptions. Dead reckoning has -no aiding, so it ignores `[aiding]`; it applies the execution limits and the position and speed -health limits, as `dr` does on the command line, but not `cov_diag_max` or the NIS pair. Synthetic +no aiding, so it ignores `[aiding]`; it applies the execution limits but not `[health_limits]`, +as `dr` does on the command line. Synthetic mode reads only `[synthetic]` and `[logging]`. ## `[aiding]` diff --git a/book/src/user-guide/data-format.md b/book/src/user-guide/data-format.md index 8ab26b40..460621e6 100644 --- a/book/src/user-guide/data-format.md +++ b/book/src/user-guide/data-format.md @@ -120,12 +120,9 @@ strapdown-sim dr -i recording.csv -o results/ --enu # Anything generated by `syn` -- NED, so no flag strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 -strapdown-sim dr -i synthetic.csv -o results/ --health-speed-mps-max 1000 +strapdown-sim dr -i synthetic.csv -o results/ ``` -(The raised speed limit is for this ten-minute unaided run, whose drift passes the default -500 m/s; see [Dead Reckoning](./dead-reckoning.md).) - The flag applies to `dr`, `ol`, `cl` and `pf` alike, and the same setting is available as `is_enu = true` at the top level of a config file. `syn --enu` generates ENU data, so `syn --enu` and `dr --enu` pair up the way plain `syn` and plain `dr` do. diff --git a/book/src/user-guide/dead-reckoning.md b/book/src/user-guide/dead-reckoning.md index d1d6209b..dfe1cea4 100644 --- a/book/src/user-guide/dead-reckoning.md +++ b/book/src/user-guide/dead-reckoning.md @@ -6,7 +6,7 @@ output is the free-inertial solution, with all of its drift. ```bash strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 -strapdown-sim dr -i synthetic.csv -o results/dr.csv --health-speed-mps-max 1000 +strapdown-sim dr -i synthetic.csv -o results/dr.csv ``` ```console @@ -16,11 +16,7 @@ strapdown-sim dr -i synthetic.csv -o results/dr.csv --health-speed-mps-max 1000 [INFO] - Results written to results/dr.csv ``` -(Timestamps trimmed from the log lines.) `--health-speed-mps-max 1000` is there because `dr` -applies the health limits: on this ten-minute file the unaided velocity passes the default -500 m/s about 580 s in, and without the flag the run stops with -`Error: OutOfRange { what: "speed", value: 500.12376354277296, min: 0.0, max: 500.0 }` and writes -no file. The flags are only the shared ones -- `-i`, `-o`, +(Timestamps trimmed from the log lines.) The flags are only the shared ones -- `-i`, `-o`, `--enu` and the limit flags -- described in [Running Simulations](./simulations.md). `dr` takes no seed, because nothing in it is random. @@ -51,13 +47,13 @@ Three practical consequences of having no filter: and fails on it. Blanking a single `acc_x` cell in a synthetic file stops the run with `Error: NonFinite { what: "filter state" }`, where `cl` on the same file logs a tolerated 0.2 s gap and finishes. -- **The position and speed limits apply.** `dr` checks the wall-clock budgets - (`--max-wall-clock-*`, `--max-no-progress-s`) and the latitude, longitude, altitude and speed - bounds (`--health-lat-*`, `--health-lon-*`, `--health-alt-*`, `--health-speed-mps-max`) after - every step, as the filters do, and a run that leaves them stops with an error and writes no - file. The default speed bound is 500 m/s, which a long unaided MEMS arc can pass; raise it - when the drift itself is what you want to see. `--health-cov-diag-max` and `--nis-pos-*` do - not apply: there is no covariance and no measurement update. +- **Only the wall-clock limits apply.** `dr` checks the wall-clock budgets + (`--max-wall-clock-*`, `--max-no-progress-s`), but none of the health bounds + (`--health-*`, `--nis-pos-*`). Those exist to stop a diverged *filter* from wasting the rest of + its budget. An unaided MEMS arc is expected to leave them -- on the ten-minute file above its + velocity error passes 500 m/s -- and that arc is the result. A non-finite state still stops + the run. Library callers who want the bounds can pass them to + `sim::dead_reckoning_with_limits`. - **The frame check still runs.** Declaring the wrong frame is the one mistake that would make dead reckoning fail at 2 g rather than drift, so it is refused before propagation exactly as in the other modes; see [Input Data Format](./data-format.md#frame-convention). diff --git a/book/src/user-guide/simulations.md b/book/src/user-guide/simulations.md index 6e8a2bef..dda87ac5 100644 --- a/book/src/user-guide/simulations.md +++ b/book/src/user-guide/simulations.md @@ -171,10 +171,8 @@ because the initial velocity is taken from the first record's noisy `speed` and Not every limit can apply to every mode, and each flag's `--help` says which modes apply it: -- **`dr` applies the execution limits and the position and speed bounds**, checked after every - propagation step. It carries no covariance and makes no measurement update, so - `--health-cov-diag-max` and the NIS pair do not apply to it. An unaided MEMS arc can pass the - default 500 m/s speed bound on a long recording; raise `--health-speed-mps-max` for those. +- **`dr` applies only the execution limits.** The health bounds exist to stop a diverged filter; + an unaided MEMS arc is expected to leave them, and that arc is what `dr` is for. - **`pf` applies every limit except the NIS pair**: its loop checks the state and covariance bounds but computes no NIS, so `--nis-pos-max` and `--nis-pos-consec-fail` have no effect on it. - **`ol` runs nothing**, so none of them applies. diff --git a/core/src/sim.rs b/core/src/sim.rs index 025687d0..b086ccac 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -4758,7 +4758,8 @@ pub mod health { /// this crate. Narrow this to the scenario's real speed range to make it an /// effective gate; unaided `dead_reckoning` never calls [`HealthMonitor`], so a run /// that deliberately drifts past this bound (see #299) is unaffected. - /// `dead_reckoning_with_limits`, which `strapdown-sim dr` runs, does apply it. + /// `strapdown-sim dr` does not apply it either: it passes only execution limits to + /// `dead_reckoning_with_limits`, which applies health limits when a caller gives them. #[serde(default = "default_health_speed_mps_max")] pub speed_mps_max: f64, /// Largest variance allowed on the covariance diagonal before the run is failed diff --git a/sim/src/main.rs b/sim/src/main.rs index 1539953c..1a45855b 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -336,37 +336,37 @@ struct SimArgs { max_no_progress_s: f64, /// Minimum latitude the estimate may reach, in degrees, before the run is failed. Applied - /// by dr, cl and pf + /// by cl and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LAT_MIN_RAD.to_degrees())] health_lat_min_deg: f64, /// Maximum latitude the estimate may reach, in degrees, before the run is failed. Applied - /// by dr, cl and pf + /// by cl and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LAT_MAX_RAD.to_degrees())] health_lat_max_deg: f64, /// Minimum longitude the estimate may reach, in degrees, before the run is failed. Applied - /// by dr, cl and pf + /// by cl and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LON_MIN_RAD.to_degrees())] health_lon_min_deg: f64, /// Maximum longitude the estimate may reach, in degrees, before the run is failed. Applied - /// by dr, cl and pf + /// by cl and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_LON_MAX_RAD.to_degrees())] health_lon_max_deg: f64, - /// Minimum altitude in metres above the ellipsoid before the run is failed. Applied by dr, - /// cl and pf + /// Minimum altitude in metres above the ellipsoid before the run is failed. Applied by cl + /// and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_ALT_MIN_M)] health_alt_min_m: f64, - /// Maximum altitude in metres above the ellipsoid before the run is failed. Applied by dr, - /// cl and pf + /// Maximum altitude in metres above the ellipsoid before the run is failed. Applied by cl + /// and pf; dr is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_ALT_MAX_M)] health_alt_max_m: f64, - /// Max velocity vector magnitude in m/s before the run is failed. Applied by dr, cl and pf; - /// an unaided dr arc on MEMS data can exceed the default, so raise it for long runs + /// Max velocity vector magnitude in m/s before the run is failed. Applied by cl and pf; dr + /// is expected to diverge and is not bounded #[arg(long, default_value_t = strapdown::sim::DEFAULT_HEALTH_SPEED_MPS_MAX)] health_speed_mps_max: f64, @@ -1021,7 +1021,6 @@ fn process_file( run_dead_reckoning_file( &records, config.is_enu, - &config.health_limits, &config.execution_limits, &output_file, config.generate_plot, @@ -1708,29 +1707,25 @@ fn run_dead_reckoning(args: &SimArgs, parallel: bool, plot: bool) -> Result<(), info!("Processing file: {}", input_file.display()); let records = load_records(input_file)?; let output_file = resolve_output_path(&args.output, input_file, &csv_files)?; - run_dead_reckoning_file( - &records, - args.enu, - &limits.health, - &limits.execution, - &output_file, - plot, - ) + run_dead_reckoning_file(&records, args.enu, &limits.execution, &output_file, plot) }) } /// Dead-reckon one file's records and write the result. /// /// Shared by `dr` and a `mode = "dead-reckoning"` scenario file. Both apply the wall-clock -/// budgets and the position and speed health bounds; the covariance and NIS limits have -/// nothing to test without a filter. They used to apply none of them. +/// budgets, which they used to ignore, but not the health bounds. The health bounds exist to +/// stop a *filter* that has diverged from burning the rest of its budget; an unaided arc on +/// MEMS data is expected to leave every one of them, and that arc is the result. Applying the +/// default 500 m/s speed bound made a plain `dr` on a ten-minute synthetic trajectory exit 1 +/// with no output. [`dead_reckoning_with_limits`] still accepts health limits for library +/// callers who want them. /// /// # Errors /// From [`dead_reckoning_with_limits`], or when the result cannot be written. fn run_dead_reckoning_file( records: &[TestDataRecord], is_enu: bool, - health: &HealthLimits, execution: &ExecutionLimits, output_file: &Path, plot: bool, @@ -1739,7 +1734,7 @@ fn run_dead_reckoning_file( "Running dead reckoning simulation on {} records", records.len() ); - let results = dead_reckoning_with_limits(records, is_enu, Some(health), Some(execution))?; + let results = dead_reckoning_with_limits(records, is_enu, None, Some(execution))?; info!("Generated {} navigation results", results.len()); NavigationResult::to_csv(&results, output_file)?; info!("Results written to {}", output_file.display()); From f64c825fa791584c62ae56fcf6ca5d9663a0aa87 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 22:03:04 -0400 Subject: [PATCH 25/26] Leave mdBook's generated 404 page out of the offline link check The first run of the new link check passed 4,180 links and failed on one: the that mdBook writes into 404.html from site-url. That link is root-relative, which lychee cannot resolve against local files without a root directory. It is generated, correct on the deployed site, and not a link any page author wrote. Co-Authored-By: Claude Opus 5.5 --- .github/workflows/deploy-book.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/deploy-book.yml b/.github/workflows/deploy-book.yml index acdc5da0..3eba5bd4 100644 --- a/.github/workflows/deploy-book.yml +++ b/.github/workflows/deploy-book.yml @@ -72,11 +72,13 @@ jobs: cp -r target/doc/. book/book/api/ # Offline: checks every relative link between book pages and into the API docs, - # without depending on external sites being up. + # without depending on external sites being up. 404.html is mdBook's generated error + # page; its `` (from `site-url`) is root-relative, which an + # offline check of local files cannot resolve. - name: Check links uses: lycheeverse/lychee-action@v2 with: - args: --offline --no-progress --exclude-path book/book/api 'book/book/**/*.html' + args: --offline --no-progress --exclude-path book/book/api --exclude-path book/book/404.html 'book/book/**/*.html' fail: true - name: Upload artifact From d0991b4436940c123ec00d0f141f944f10d2f117 Mon Sep 17 00:00:00 2001 From: James Brodovsky Date: Thu, 1 Oct 2026 22:04:46 -0400 Subject: [PATCH 26/26] Define the crate's ENU in the glossary as it actually is The glossary gave the textbook East-North-Up, but this crate's 'ENU' keeps the north-then-east horizontal order and flips only the vertical axis, as StrapdownState::to_enu does and the Coordinate Frames page says. A reader who took the glossary at its word would swap the horizontal components. Co-Authored-By: Claude Opus 5.5 --- book/src/resources/glossary.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/book/src/resources/glossary.md b/book/src/resources/glossary.md index cc6112f5..c1ba3df3 100644 --- a/book/src/resources/glossary.md +++ b/book/src/resources/glossary.md @@ -15,9 +15,12 @@ - **EKF** (extended Kalman filter): A Kalman filter for a nonlinear system that propagates the full state through the nonlinear model and the covariance through its Jacobians. See [EKF](../filters/ekf.md). -- **ENU / NED**: The two local-level frame conventions: East-North-Up and North-East-Down. NED - is this crate's default; ENU is opt-in with `--enu` or `is_enu`. Altitude is positive up in - both; vertical velocity is positive down in NED and up in ENU. See [Coordinate +- **ENU / NED**: The two local-level frame conventions this crate supports. NED + (North-East-Down) is the default. "ENU" is opt-in with `--enu` or `is_enu`, and here it is + **not** the textbook East-North-Up: it keeps the north-then-east order of the horizontal + axes (`velocity_north`, `velocity_east` in both) and flips only the vertical axis to point + up, which is what `StrapdownState::to_enu` does. Altitude is positive up in both; vertical + velocity is positive down in NED and up in "ENU". See [Coordinate Frames](../user-guide/coordinate-frames.md). - **ESKF** (error-state Kalman filter): A Kalman filter that estimates the *error* in a separately propagated nominal solution, folds the estimate back in after each update, and