diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 00000000..6a60eadb --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,30 @@ +--- +name: Bug report +about: Something produced a wrong result, an error, or a crash +labels: bug +--- + +**What happened** + + + +**How to reproduce** + + + +```bash + +``` + +**Environment** + +- strapdown-rs version or commit: +- Crate(s) involved: strapdown-core / strapdown-sim / strapdown-geonav / analysis +- Cargo features enabled: +- OS and `rustc --version`: + +**Output** + + diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 00000000..33328bbb --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,8 @@ +blank_issues_enabled: true +contact_links: + - name: User guide + url: https://jbrodovsky.github.io/strapdown-rs/ + about: Installation, the CLI, configuration files and the filters. + - name: API documentation + url: https://docs.rs/strapdown-core + about: Reference documentation for the strapdown-core library. diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md new file mode 100644 index 00000000..c7084a3a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/question.md @@ -0,0 +1,13 @@ +--- +name: Question / support +about: Ask how to use strapdown-rs, or whether it can do something +labels: question +--- + +**What are you trying to do?** + + + +**What have you tried?** + + diff --git a/.github/workflows/draft-pdf.yml b/.github/workflows/draft-pdf.yml index 71fea43f..5d2334f8 100644 --- a/.github/workflows/draft-pdf.yml +++ b/.github/workflows/draft-pdf.yml @@ -1,9 +1,10 @@ name: Draft PDF on: push: - branches: [ "joss" ] + branches: [ "main" ] + paths: [ "papers/joss/**", ".github/workflows/draft-pdf.yml" ] pull_request: - branches: [ "joss" ] + paths: [ "papers/joss/**", ".github/workflows/draft-pdf.yml" ] jobs: paper: diff --git a/CITATION.cff b/CITATION.cff new file mode 100644 index 00000000..ff031e1f --- /dev/null +++ b/CITATION.cff @@ -0,0 +1,20 @@ +cff-version: 1.2.0 +message: "If you use this software, please cite it as below." +title: "strapdown-rs: Strapdown inertial navigation and GNSS-degradation simulation in Rust" +type: software +authors: + - family-names: Brodovsky + given-names: James + orcid: "https://orcid.org/0000-0002-1371-9044" + affiliation: Temple University +version: 1.0.0 +license: MIT +repository-code: "https://github.com/jbrodovsky/strapdown-rs" +url: "https://jbrodovsky.github.io/strapdown-rs/" +keywords: + - inertial navigation + - strapdown INS + - GNSS + - Kalman filter + - particle filter + - Rust diff --git a/CLAUDE.md b/CLAUDE.md index e2592cf0..a21d5236 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -269,8 +269,10 @@ The Free Core implementation must achieve the following capabilities: loop and are not measurement updates - Time update once per measurement epoch (transition and noise accumulate between); one measurement update for every other sensor (`C = H T`, weights under `C P Cᵀ + R`) - - `horizontal_process_noise_std_m` defaults to the paper's zero (eq. 19), which **diverges** - with GNSS-rate fixes on MEMS data; the `conf/` recipes set 1 m/√s + - `horizontal_process_noise_std_m`: `RbpfConfig` (the library) defaults to the paper's zero + (eq. 19), which **diverges** with GNSS-rate fixes on MEMS data. `strapdown-sim` and its + `[particle_filter]` section default to 1 m/√s (`sim::DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M`), + as the `conf/` recipes set - Reports the Kalman filters' layout, `[9 nav, b_a, b_g, map biases]`, with zero bias rows; `core/src/rbpf.rs`'s module docs list the departures from the paper - **Process noise**: `sim::DEFAULT_PROCESS_NOISE_DENSITY` is a **spectral density** -- a diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 00000000..6797d8ab --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,15 @@ +# Code of Conduct + +This project adopts the [Contributor Covenant, version 2.1](https://www.contributor-covenant.org/version/2/1/code_of_conduct/) +as its code of conduct. It applies to everyone taking part in the project's spaces: the issue +tracker, pull requests, discussions, and any other channel used to work on `strapdown-rs`. + +In short: be respectful and constructive, assume good faith, and critique work rather than +people. Harassment, personal attacks and discriminatory language are not acceptable. + +## Reporting + +Report unacceptable behaviour privately to the maintainer, James Brodovsky, at +[jbrodovsky@temple.edu](mailto:jbrodovsky@temple.edu). Reports are handled confidentially, and +the maintainer will respond with the enforcement steps described in the Contributor Covenant's +[enforcement guidelines](https://www.contributor-covenant.org/version/2/1/code_of_conduct/#enforcement-guidelines). diff --git a/README.md b/README.md index 1473f42c..481ed024 100644 --- a/README.md +++ b/README.md @@ -1,100 +1,217 @@ -# Strapdown-rs - A simple strapdown INS implementation +# strapdown-rs + +[![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) +[![License: MIT](https://img.shields.io/badge/license-MIT-blue.svg)](LICENSE) + +**Strapdown inertial navigation and GNSS-degradation simulation in Rust.** + +`strapdown-rs` is a library and command-line toolkit for research on inertial navigation +systems (INS), particularly when GNSS is unavailable or untrustworthy. It provides: + +- local-level strapdown mechanization (Groves, *Principles of GNSS, Inertial, and Multisensor + Integrated Navigation Systems*, 2nd ed., §5.4); +- four loosely coupled navigation filters behind one trait; +- a simulator that replays recorded or synthetic IMU/GNSS data while removing, thinning or + corrupting the GNSS stream according to a seeded scenario file. + +**📖 [User guide](https://jbrodovsky.github.io/strapdown-rs/)** · +**📚 [API documentation](https://docs.rs/strapdown-core)** · +**▶️ [Runnable examples](#runnable-examples)** · +**🔧 [Example scenarios](examples/configs/)** + +## Statement of need + +Navigation algorithms are commonly prototyped in MATLAB or Python. There they are fast only +once rewritten in vectorized form, which is especially awkward for particle filters. Real +GNSS-denied data is expensive, regulated and impossible to repeat exactly. + +`strapdown-rs` addresses both problems: +- Filters are written plainly and compiled, so the same code serves research scripts, + experiments and applications. +- Any trajectory can be run under controlled, reproducible GNSS outages and faults, so + algorithms are compared under identical conditions. + +It is aimed at graduate students, researchers and engineers working on INS/GNSS integration +and alternative positioning, navigation and timing (PNT). + +## What it does + +- **Filters.** All four implement the `NavigationFilter` trait: + - error-state Kalman filter (ESKF, the default): 15 error states, plus a barometric bias + state unless `--no-estimate-baro-bias`; + - extended Kalman filter (EKF); + - unscented Kalman filter (UKF); + - Rao-Blackwellized particle filter after Canciani & Raquet (2017). +- **Aiding.** GNSS position and velocity, barometric altitude with bias estimation, + magnetometer heading (World Magnetic Model declination), and ZUPT/ZARU. Chi-squared + innovation gating with recovery. +- **Initialization.** Coarse alignment, IMU calibration and lever-arm compensation via the + `InsEngine` builder. +- **GNSS degradation.** + - Schedulers: pass-through, fixed interval, duty-cycle outages. + - Faults: correlated AR(1) noise, slow bias drift, position hijacking, combinations. + - Configured from TOML, YAML or JSON files, or from CLI flags. +- **Synthetic trajectories** with IMU error models by sensor grade (`strapdown-sim syn`). +- **Metrics.** RMSE, CEP, NEES and NIS. +- **Output.** The CLI writes CSV. The library also writes HDF5, NetCDF and MCAP. +- **Experimental:** gravity and magnetic anomaly map aiding (`strapdown-geonav`). + +It is loosely coupled only. There are no pseudorange or carrier-phase models, and no +raw-signal processing. -HTML: +## Repository layout -Markdown: [![status](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4/status.svg)](https://joss.theoj.org/papers/5079592cc860d1435482a4a7764edcd4) +The repository is a two-language monorepo. The Rust crates form a Cargo workspace. The +Python package is a [uv](https://docs.astral.sh/uv/) workspace declared by the root +`pyproject.toml`. Neither is a member of the other. -[![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) +| Path | Language | What it is | +|---|---|---| +| [`core/`](core/) | Rust | `strapdown-core`: mechanization, filters, measurement models, scenario engine | +| [`sim/`](sim/) | Rust | `strapdown-sim`: the command-line simulator | +| [`geonav/`](geonav/) | Rust | `strapdown-geonav`: experimental gravity and magnetic anomaly aiding | +| [`analysis/`](analysis/) | Python | the `analyze` CLI: preprocessing, measurement statistics, result comparison | +| [`examples/`](examples/) | — | scenario configuration files | +| [`conf/`](conf/) | — | the experiment recipes behind the published results | -Strapdown-rs is a straightforward strapdown inertial navigation system (INS) implementation in Rust. It is designed to be simple and easy to understand, making it a great starting point for those interested in learning about or implementing strapdown INS algorithms. It is currently under active development. +## Installation -**📖 [Full Documentation](https://jbrodovsky.github.io/strapdown-rs/)** | **📚 [API Documentation](https://docs.rs/strapdown-core)** | **▶️ [Runnable Examples](#runnable-examples)** | **🔧 [Example Configurations](examples/configs/)** +`rust-toolchain.toml` fetches the pinned Rust toolchain (1.91) on the first `cargo` command. +What else you need depends on what you build. -The primary contributions of this project are: +**Library only.** `strapdown-core` with default features needs nothing beyond Rust: -1. A source library of strapdown INS algorithms and utilities (`strapdown-core`: /core). -2. A program for simulating INS performance in various GNSS conditions (`strapdown-sim`: /sim). -3. A dataset of smartphone-based MEMS IMU and GNSS data (`strapdown-data`: /data). +```bash +cargo add strapdown-core --git https://github.com/jbrodovsky/strapdown-rs +``` -Additionally experimental research is being conducted on improving the accuracy and robustness of strapdown INS algorithms under degraded and denied GNSS conditions by providing an alternative PNT solution in the form of geophysical anomaly data (`strapdown-geonav`: /geonav). This is an area of active research. +The library crate is imported as `strapdown`: -## Repository layout +```rust +use strapdown::StrapdownState; +``` -The repository is a two-language monorepo. The Rust crates are a Cargo workspace; the Python -package is a [uv](https://docs.astral.sh/uv/) workspace declared by the root `pyproject.toml`. -Neither is a member of the other. +**Simulator.** -| Path | Language | What it is | -|---|---|---| -| [`core/`](core/) | Rust | `strapdown-core` -- the mechanization, the filters and the simulation framework | -| [`sim/`](sim/) | Rust | `strapdown-sim` -- the command-line simulator | -| [`geonav/`](geonav/) | Rust | `strapdown-geonav` -- experimental gravity and magnetic anomaly aiding | -| [`analysis/`](analysis/) | Python | the `analyze` CLI: preprocessing, measurement characterisation and performance analysis | +```bash +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim +``` -## Installation +Its default `plotting` feature, and the HDF5 and NetCDF features, compile C libraries from +vendored sources. Those builds need a C/C++ compiler and **cmake 3.26 or newer**, and +`HDF5_DIR` must be unset. See the [installation guide](https://jbrodovsky.github.io/strapdown-rs/installation/installation.html). + +The crates are also published to crates.io. Version 1.0.0 is released there together with the +JOSS paper; until then, install from git as above. -To use `strapdown-rs`, you can add it as a dependency in your `Cargo.toml` file: `cargo add strapdown-rs`. You can install the whole package or just the core library. Similarly you can install the simulation binary `cargo install strapdown-sim`. +**Developing.** From a clone: -To work on the repository itself, build both halves: +```bash +cargo build --workspace --release +``` ```bash -cargo build --workspace --release # the Rust crates; the toolchain fetches itself -uv sync # the analysis package, from the workspace root +uv sync ``` -`just setup` runs both. Nothing beyond Rust and uv is needed for the Rust crates; the -`analyze` CLI's map-drawing subcommands additionally want the GMT C library installed, though -every other subcommand -- preprocessing and measurement characterisation included -- runs -without it. +`just setup` runs both. The `analyze` map-drawing subcommands also need the GMT C library; +every other subcommand runs without it. + +## Quick start + +```bash +# A 10-minute synthetic trajectory: noisy IMU, GNSS and barometer records (add --no-noise for the truth) +strapdown-sim syn -o synthetic.csv --duration-s 600 --seed 42 + +# Closed loop (ESKF) with a duty-cycled outage: 50 s denied, then 100 s available, repeating +strapdown-sim cl -i synthetic.csv -o eskf.csv --seed 42 --sched duty --on-s 100 --off-s 50 + +# Dead reckoning: no aiding at all +strapdown-sim dr -i synthetic.csv -o dr.csv +``` + +Use `--filter ukf` or `--filter ekf` to select another filter. `strapdown-sim pf` runs the +particle filter. `strapdown-sim --help` lists every option. Logging is controlled +with `--log-level` and `--log-file`; see the +[logging guide](https://jbrodovsky.github.io/strapdown-rs/user-guide/logging.html). + +Geophysical aiding is experimental and behind a feature: + +```bash +cargo install --git https://github.com/jbrodovsky/strapdown-rs strapdown-sim --features geonav +``` + +```bash +strapdown-sim cl -i data.csv -o out.csv --geo --gravity-resolution one-minute +``` ## Runnable examples -Two worked examples live in [`core/examples/`](core/examples/). Both are self-contained -- -they generate their own trajectory, so there is no dataset to fetch first. +Two worked examples live in [`core/examples/`](core/examples/). Both generate their own +trajectory, so there is no dataset to fetch first. ```bash # Minimal InsEngine usage: propagate IMU, fuse GNSS, read the solution. cargo run -p strapdown-core --example basic_ins +``` -# Dead-reckoning through a GNSS outage, and recovery when the signal returns. +```bash +# Coasting through a GNSS outage, and recovery when the signal returns. cargo run -p strapdown-core --example gnss_outage ``` -`basic_ins` is the one to read first: it is the propagate/update/read loop that every -application is built around, on a trajectory simple enough to check by hand. It is also the -place the specific-force sign convention is spelled out, which is the single most common way -to get a first integration wrong -- an accelerometer at rest reads **-9.81 m/s² on the down -axis** in NED, because it senses the ground pushing up. +**Read `basic_ins` first.** It is the propagate/update/read loop that every application is +built around, on a trajectory simple enough to check by hand. It also spells out the +specific-force sign convention, the most common way to get a first integration wrong: an +accelerometer at rest reads **−9.81 m/s² on the down axis** in NED, because it senses the +ground pushing up. + +**Then `gnss_outage`.** It shows the error staying flat while aided, growing while coasting, +and collapsing on recovery. It then explains why the coasting error is several times +*smaller* than double-integrating the accelerometer bias would predict: the filter absorbed +most of the bias into a fraction of a degree of pitch, which position and velocity aiding +cannot tell apart from bias. + +## Running the tests -`gnss_outage` is the one to read second. It shows the error staying flat while aided, growing -while coasting, and collapsing on recovery -- and then explains why the coasting error is -several times *smaller* than double-integrating the accelerometer bias would predict. The -answer is that the filter absorbed most of the bias into a fraction of a degree of pitch, -because position and velocity aiding cannot tell those two apart. +```bash +# Every Rust test, all features (as CI runs it) +cargo test --workspace --all-features +``` -For scenario-driven runs of the same ideas through the CLI rather than in code, see -[`examples/configs/`](examples/configs/). +```bash +# The minimal-feature configuration CI also tests +cargo test-min +``` -## Summary +```bash +# The Python analysis package +uv run pytest -q +``` -`strapdown-rs` is a Rust-based software library for implementing strapdown inertial navigation systems (INS). It provides core functionality for processing inertial measurement unit (IMU) data to estimate position, velocity, and orientation using a strapdown mechanization model that is typical of modern systems particularly in the low size, weight, and power (low SWaP) domain (cell phones, drones, robotics, UAVs, UUVs, etc.). Additionally, it provides some basic simulation capabilities for simulating INS scenarios (e.g. dead reckoning, closed-loop INS, intermitent GPS, GPS degradation, etc.). +The accuracy regression suite (`core/tests/perf_baseline.rs`) runs every filter over +recorded and synthetic scenarios. It fails if a gated metric moves beyond tolerance. Its +results are published as the +[performance baselines](https://jbrodovsky.github.io/strapdown-rs/development/performance.html) +page. CI runs on Linux, macOS and Windows. -`strapdown-rs` prioritizes correctness, numerical stability, and performance. It is built with extensibility in mind, allowing researchers and engineers to implement additional filtering, sensor fusion, or aiding algorithms on top of the base INS framework. This library is not intended to be a full-featured INS solution, notably it does not have code for processing raw IMU or GPS signals and only implements a loosely-couple INS. +## Contributing and support -The toolbox is designed for research, teaching, and development purposes and aims to serve the broader robotics, aerospace, and autonomous systems communities. The intent is to provide a high-performance, memory-safe, and cross-platform implementation of strapdown INS algorithms that can be easily integrated into existing systems. The simulation is intended to be used for testing and verifying the correctness of the INS algorithms, by providing a simple simulation that allows users to generate a "ground truth" trajectory. +- Bugs and feature requests: [open an issue](https://github.com/jbrodovsky/strapdown-rs/issues). +- Questions about using the toolkit: open an issue with the *question* template. +- Contributions: see [CONTRIBUTING.md](CONTRIBUTING.md) for the setup, the lint gate and the + pull-request checklist. -## Functionality +This project follows the [Contributor Covenant](CODE_OF_CONDUCT.md). -`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. +## Citing -The simulation program provides a simple command line interface for running various configurations of the INS. In can run in open-loop (dead reckoning) mode or closed-loop mode (loosely coupled; the 15-state ESKF by default, with the UKF and EKF selectable). 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 datsets for further navigation processing or research. +If you use `strapdown-rs` in research, please cite it using [CITATION.cff](CITATION.cff) +(GitHub's "Cite this repository" button). The JOSS paper is under review; its DOI will be +added on acceptance. -`strapdown-sim` includes built-in logging capabilities using the Rust `log` crate with `env_logger`. You can control log output via command-line options (`--log-level` and `--log-file`) for monitoring simulation progress, debugging issues, and recording detailed execution information. See [LOGGING.md](LOGGING.md) for detailed usage instructions. +## License -**Geophysical Navigation**: To enable geophysical navigation capabilities (gravity/magnetic anomaly aiding), build with `--features geonav` and use the `--geo` flag: -```bash -cargo build --release --package strapdown-sim --features geonav -strapdown-sim cl --input data.csv --output out/ --geo --gravity-resolution one-minute -``` \ No newline at end of file +MIT. See [LICENSE](LICENSE). diff --git a/analysis/pyproject.toml b/analysis/pyproject.toml index 4d3bdd23..c7d02021 100644 --- a/analysis/pyproject.toml +++ b/analysis/pyproject.toml @@ -1,7 +1,7 @@ [project] name = "analysis" version = "0.1.0" -description = "Add your description here" +description = "Preprocessing, measurement characterisation and result analysis for strapdown-rs experiments" readme = "README.md" authors = [ { name = "James Brodovsky", email = "james.brodovsky@gmail.com" } diff --git a/book/src/filters/rbpf.md b/book/src/filters/rbpf.md index 045701a0..132c163a 100644 --- a/book/src/filters/rbpf.md +++ b/book/src/filters/rbpf.md @@ -49,7 +49,7 @@ following: | key | meaning | default | | --- | --- | --- | | `velocity_process_noise_std_mps`, `attitude_process_noise_std_rad` | VRW and ARW, per √s (eq. 20) | 1e-3, 0.01 | -| `horizontal_process_noise_std_m` | `[north, east]` random walk on the sampled position, m/√s | `[0, 0]`, the paper's eq. 19 | +| `horizontal_process_noise_std_m` | `[north, east]` random walk on the sampled position, m/√s | `[1, 1]`; the paper's eq. 19 is `[0, 0]` (see below) | | `baro_loop_time_constant_s` | the loop's gains place its three poles at `-1/τ` | 10 | | `baro_error_std_m`, `baro_error_time_constant_s` | the barometer-aiding error as a Gauss-Markov process (`σ_b`, `τ_b`) | 8.3 m, 3600 s | | `vertical_accel_error_init_std_mps2` | prior on `δâ` | 0.1 | @@ -63,7 +63,7 @@ starts from its stationary distribution, and `c` takes the rest of `*_bias_init_ paper's magnetic temporal variation is `magnetic_variation_std = 5` with `magnetic_variation_time_constant_s = 300`. -## Horizontal process noise: why the recipes set 1 m/√s +## Horizontal process noise: why the default is 1 m/√s The paper's zero (eq. 19) makes every epoch's time update a noiseless observation of the velocity and tilt errors. That moves their uncertainty out of the shared covariance and into diff --git a/core/README.md b/core/README.md index 68acc77c..d0509b77 100644 --- a/core/README.md +++ b/core/README.md @@ -1,6 +1,10 @@ -# Strapdown - A simple strapdown INS implementation +# strapdown-core -Strapdown-rs is a straightforward strapdown inertial navigation system (INS) implementation in Rust. It is designed to be simple and easy to understand, making it a great starting point for those interested in learning about strapdown INS algorithms. It is currently under active development. +The library half of [strapdown-rs](https://github.com/jbrodovsky/strapdown-rs): local-level +strapdown inertial navigation (Groves, 2nd ed., §5.4), four loosely coupled navigation filters +behind one `NavigationFilter` trait (ESKF, EKF, UKF and a Rao-Blackwellized particle filter), +measurement models, and the event-stream engine that simulates GNSS outages and faults. The +[user guide](https://jbrodovsky.github.io/strapdown-rs/) covers the concepts and the CLI. ## Installation @@ -41,9 +45,9 @@ strapdown-core = { version = "1.0", features = ["hdf5", "netcdf"] } ## Summary -`strapdown-rs` is a Rust-based software library for implementing strapdown inertial navigation systems (INS). It provides core functionality for processing inertial measurement unit (IMU) data to estimate position, velocity, and orientation using a strapdown mechanization model that is typical of modern systems particularly in the low size, weight, and power (low SWaP) domain (cell phones, drones, robotics, UAVs, UUVs, etc.). Additionally, it provides some basic simulation capabilities for simulating INS scenarios (e.g. dead reckoning, closed-loop INS, intermitent GPS, GPS degradation, etc.). +`strapdown-rs` is a Rust-based software library for implementing strapdown inertial navigation systems (INS). It provides core functionality for processing inertial measurement unit (IMU) data to estimate position, velocity, and orientation using a strapdown mechanization model that is typical of modern systems particularly in the low size, weight, and power (low SWaP) domain (cell phones, drones, robotics, UAVs, UUVs, etc.). Additionally, it provides some basic simulation capabilities for simulating INS scenarios (e.g. dead reckoning, closed-loop INS, intermittent GPS, GPS degradation, etc.). -`strapdown-rs` prioritizes correctness, numerical stability, and performance. It is built with extensibility in mind, allowing researchers and engineers to implement additional filtering, sensor fusion, or aiding algorithms on top of the base INS framework. This library is not intended to be a full-featured INS solution, notably it does not have code for processing raw IMU or GPS signals and only implements a loosely-couple INS. +`strapdown-rs` prioritizes correctness, numerical stability, and performance. It is built with extensibility in mind, allowing researchers and engineers to implement additional filtering, sensor fusion, or aiding algorithms on top of the base INS framework. This library is not intended to be a full-featured INS solution, notably it does not have code for processing raw IMU or GPS signals and only implements a loosely coupled INS. The toolbox is designed for research, teaching, and development purposes and aims to serve the broader robotics, aerospace, and autonomous systems communities. The intent is to provide a high-performance, memory-safe, and cross-platform implementation of strapdown INS algorithms that can be easily integrated into existing systems. The simulation is intended to be used for testing and verifying the correctness of the INS algorithms, by providing a simple simulation that allows users to generate a "ground truth" trajectory. @@ -51,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. In can run in open-loop (dead reckoning) mode or closed-loop (full state loosely couple UKF) mode. 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 datsets 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 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. `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. diff --git a/core/src/rbpf.rs b/core/src/rbpf.rs index 26a01b6c..52d5d42d 100644 --- a/core/src/rbpf.rs +++ b/core/src/rbpf.rs @@ -74,8 +74,8 @@ //! misstate a Gauss-Markov state's variance, and this does not. //! 6. **Horizontal position process noise is configurable** //! ([`RbpfConfig::horizontal_process_noise_std_m`]). Its default is the paper's zero -//! (eq. 19), which diverges with GNSS-rate fixes on MEMS data; the configurations under -//! `conf/` use 1 m per root-second. +//! (eq. 19), which diverges with GNSS-rate fixes on MEMS data; `strapdown-sim` and the +//! configurations under `conf/` use 1 m per root-second. use crate::StrapdownError; use crate::gating::{ @@ -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. + /// root-second held it to 3.8 m. 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, /// Time constant of the barometer loop, seconds. The gains place all three closed-loop /// poles at `-1 / tau`: `k₁ = 3/tau`, `k₂ = 3/tau² + 2g/R`, `k₃ = 1/tau³`. diff --git a/core/src/sim.rs b/core/src/sim.rs index 31f98b87..8f6f0d19 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -328,6 +328,16 @@ pub const DEFAULT_PROCESS_NOISE_DENSITY: [f64; 15] = [ 1e-8, // gyro bias z, (rad/s)^2/s ]; +/// Default [`ParticleFilterConfig::horizontal_process_noise_std_m`], m per sqrt(s), applied +/// to both the north and east channels. +/// +/// The simulator departs here from the library's [`crate::rbpf::RbpfConfig`], whose default is +/// Canciani & Raquet's zero (eq. 19). That zero diverges with GNSS-rate fixes on MEMS data -- +/// `strapdown-sim pf` with no flags left a 10-minute synthetic trajectory at 500 m/s -- so a +/// simulator default that only works on navigation-grade data would fail the first thing a new +/// user tries. One metre per root-second is what every recipe under `conf/` uses. +pub const DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M: f64 = 1.0; + /// Default [`ExecutionLimits::max_wall_clock_ratio`]: a run may burn at most a quarter of a /// second of wall-clock time per second of trajectory it simulates. pub const DEFAULT_MAX_WALL_CLOCK_RATIO: f64 = 0.25; @@ -5337,7 +5347,8 @@ pub struct ParticleFilterConfig { /// [`crate::rbpf::RbpfConfig::attitude_process_noise_std_rad`]. pub attitude_process_noise_std_rad: f64, /// Random walk on the sampled horizontal position error, [`north`, `east`] in m per - /// sqrt(s). The filter's default is Canciani & Raquet's zero (eq. 19), which diverges with + /// sqrt(s). Defaults to [`DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M`] on both channels, + /// not the library filter's zero (Canciani & Raquet's eq. 19), which diverges with /// GNSS-rate fixes on MEMS data; see /// [`crate::rbpf::RbpfConfig::horizontal_process_noise_std_m`]. pub horizontal_process_noise_std_m: Vec, @@ -5560,7 +5571,8 @@ impl Default for ParticleFilterConfig { fn default() -> Self { // The barometer-loop and resampling defaults are the filter's own, read from it rather // than restated, so a config file that omits them runs the filter `RbpfConfig` - // documents. + // documents. The horizontal process noise is the one deliberate exception; see + // `DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M`. let filter = crate::rbpf::RbpfConfig::default(); Self { num_particles: default_num_particles(), @@ -5569,10 +5581,7 @@ impl Default for ParticleFilterConfig { attitude_init_std_rad: default_attitude_init_std_rad(), velocity_process_noise_std_mps: default_velocity_process_noise_std_mps(), attitude_process_noise_std_rad: default_attitude_process_noise_std_rad(), - horizontal_process_noise_std_m: filter - .horizontal_process_noise_std_m - .as_slice() - .to_vec(), + horizontal_process_noise_std_m: vec![DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M; 2], baro_loop_time_constant_s: filter.baro_loop_time_constant_s, baro_error_std_m: filter.baro_error_std_m, baro_error_time_constant_s: filter.baro_error_time_constant_s, diff --git a/papers/README.md b/papers/README.md index 3bf567cc..4f29f8d0 100644 --- a/papers/README.md +++ b/papers/README.md @@ -1,8 +1,11 @@ # Papers This directory holds the [JOSS](https://joss.theoj.org/) submission for -`strapdown-rs`. It is built by the `Draft PDF` GitHub Actions workflow -(`.github/workflows/draft-pdf.yml`) on pushes to the `joss` branch. +`strapdown-rs`, under review at +[openjournals/joss-reviews#11377](https://github.com/openjournals/joss-reviews/issues/11377). +The review reads the paper from `main`. The `Draft PDF` GitHub Actions workflow +(`.github/workflows/draft-pdf.yml`) builds it on every pull request and push to `main` that +touches `papers/joss/`, and uploads the PDF as a workflow artifact. - `joss/paper.md` — the submission manuscript - `joss/paper.bib` — its bibliography diff --git a/papers/joss/paper.bib b/papers/joss/paper.bib index 83058617..e7464265 100644 --- a/papers/joss/paper.bib +++ b/papers/joss/paper.bib @@ -1,23 +1,23 @@ - @book{groves, - edition = {2}, - title = {Principles of {GNSS}, Inertial, and Multisensor Integrated Navigation Systems}, - isbn = {978-1-60807-005-3}, - pagetotal = {800}, - publisher = {Artech House}, - author = {Groves, Paul}, - keywords = {{TAN}, inertial navigation, kf, dissertation}, - file = {Groves - Principles of GNSS, Inertial, and Multisensor Inte.pdf:/home/james/snap/zotero-snap/common/Zotero/storage/7UE7B7A9/Groves - Principles of GNSS, Inertial, and Multisensor Inte.pdf:application/pdf}, + author = {Groves, Paul D.}, + title = {Principles of {GNSS}, Inertial, and Multisensor Integrated Navigation Systems}, + edition = {2}, + publisher = {Artech House}, + address = {Boston}, + year = {2013}, + isbn = {978-1-60807-005-3} } + @techreport{wgs84, - author = {{National Geospatial-Intelligence Agency}}, - title = {Department of Defense World Geodetic System 1984: Its Definition and Relationships with Local Geodetic Systems}, + author = {{National Geospatial-Intelligence Agency}}, + title = {Department of Defense World Geodetic System 1984: Its Definition and Relationships with Local Geodetic Systems}, institution = {National Geospatial-Intelligence Agency}, - year = {2014}, % Or the year of the specific version/publication you are referring to - number = {NGA.STND.0036_1.0.0_WGS84}, % Example of a standard number, adjust if a different one is appropriate - address = {Springfield, VA}, - note = {The specific version and publication date can vary. Check the NGA website for the most current or relevant document for your application.} + number = {NGA.STND.0036\_1.0.0\_WGS84}, + address = {Springfield, VA}, + year = {2014}, + url = {https://earth-info.nga.mil/} } + @misc{wmm, author = {{NOAA NCEI Geomagnetic Modeling Team and British Geological Survey}}, title = {World Magnetic Model 2025}, @@ -26,63 +26,71 @@ @misc{wmm doi = {10.25921/aqfd-sd83}, url = {https://www.ncei.noaa.gov/products/world-magnetic-model} } + @misc{nav-types, - author = {Nordmoen, Jørgen and Feuerstein, Marco}, - title = {nav-types}, - year = {2022}, - url = {https://github.com/nordmoen/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 = {Yizhou Fang}, - title = {gnss-ins-sim: GNSS/INS simulator for testing and prototyping navigation algorithms}, - year = {2019}, - url = {https://github.com/Aceinna/gnss-ins-sim} + author = {{Aceinna, Inc.}}, + title = {gnss-ins-sim: {GNSS}/{INS} simulator for testing and prototyping navigation algorithms}, + year = {2019}, + url = {https://github.com/Aceinna/gnss-ins-sim} } -@article{gtsam, - author = {Dellaert, Frank}, - title = {{GTSAM}: A factor graph based nonlinear estimation library}, - journal = {Georgia Institute of Technology}, - year = {2012}, - url = {https://gtsam.org} + +@techreport{gtsam, + author = {Dellaert, Frank}, + title = {Factor Graphs and {GTSAM}: A Hands-on Introduction}, + institution = {Georgia Institute of Technology}, + number = {GT-RIM-CP\&R-2012-002}, + year = {2012}, + url = {https://gtsam.org} } + @inproceedings{geneva2020openvins, author = {Geneva, Patrick and Eckenhoff, Kevin and Lee, Woosik and Yang, Yulin and Huang, Guoquan}, title = {{OpenVINS}: A Research Platform for Visual-Inertial Estimation}, booktitle = {Proceedings of the IEEE International Conference on Robotics and Automation (ICRA)}, - year = {2020}, + pages = {4666--4672}, address = {Paris, France}, - url = {https://github.com/rpng/open_vins} + year = {2020}, + doi = {10.1109/ICRA40945.2020.9196524} } -@misc{nalgebra, - author = {Crozet, Sébastien and Bopp, Emilia}, - title = {nalgebra}, - year = {2024}, - url = {https://github.com/nordmoen/nav-types} + +@article{canciani2017, + author = {Canciani, Aaron and Raquet, John}, + title = {Airborne Magnetic Anomaly Navigation}, + journal = {IEEE Transactions on Aerospace and Electronic Systems}, + volume = {53}, + number = {1}, + pages = {67--80}, + year = {2017}, + doi = {10.1109/TAES.2017.2649238} } + @inproceedings{anom-ukf, - author = {Brodovsky, James and Dames, Philip}, + author = {Brodovsky, James and Dames, Philip}, + title = {Navigation in {GNSS}-denied environments using {MEMS}-grade sensors and geophysical anomalies: A {UKF} approach}, booktitle = {Proceedings of the 2026 International Technical Meeting of the Institute of Navigation}, - location = {Anaheim, California}, - pages = {155--164}, - editor = {}, - title = {Navigation in {GNSS}-denied environments using {MEMS}-grade sensors and geophysical anomalies: A {UKF} approach}, - month = {January}, - year = {2026}, - keywords = {conference}, + address = {Anaheim, California}, publisher = {Institute of Navigation}, - doi = {https://doi.org/10.33012/2026.20508}, + pages = {155--164}, + month = jan, + year = {2026}, + doi = {10.33012/2026.20508} } @inproceedings{anom-pf, - author = {Brodovsky, James and Dames, Philip}, - booktitle = {Proceedings of the ION Pacific PNT Meeting}, - editor = {}, - title = {Navigation in {GNSS}-denied Environments using {MEMS}-grade Sensors and Geophysical Anomalies: A Particle Filter Approach}, - volume = {}, - month = {April}, - year = {2026}, - keywords = {conference}, + author = {Brodovsky, James and Dames, Philip}, + title = {Navigation in {GNSS}-Denied Environments Using {MEMS}-Grade Sensors and Geophysical Anomalies: A Particle Filter Approach}, + booktitle = {Proceedings of the ION 2026 Pacific PNT Meeting}, + address = {Honolulu, Hawaii}, publisher = {Institute of Navigation}, - location = {Honolulu, Hawaii}, - doi = {} -} \ No newline at end of file + pages = {399--410}, + month = apr, + year = {2026}, + doi = {10.33012/2026.20618} +} diff --git a/papers/joss/paper.md b/papers/joss/paper.md index 2f1e3c2a..e284b48a 100644 --- a/papers/joss/paper.md +++ b/papers/joss/paper.md @@ -1,108 +1,174 @@ --- -title: 'strapdown-rs: A Simple Strapdown INS Implementation in Rust' +title: 'strapdown-rs: Strapdown inertial navigation and GNSS-degradation simulation in Rust' tags: - - strapdown-ins - inertial-navigation + - strapdown-ins + - gnss + - kalman-filter + - particle-filter - rust - - robotics - - aerospace authors: - name: James Brodovsky orcid: 0000-0002-1371-9044 - equal-contrib: true corresponding: true affiliation: 1 affiliations: - name: Temple University, United States index: 1 -date: 5 June 2025 +date: 1 October 2026 bibliography: paper.bib --- ## Summary -Inertial navigation systems (INSs) are critical for many applications in robotics, aerospace, and autonomous systems. They provide real-time estimates of position, velocity, and orientation using data from an inertial measurement unit (IMU) as well as other aiding sensors like GPS. Strapdown implementations of INSs are becoming increasingly common due to the proliferation of modern IMUs that do not require gyroscopic stabilization such as micro-electromagnetic systems (MEMS), fiber optic gyroscopes (FOGs), and ring laser gyroscopes (RLGs). These IMUs are popular for their lower size, weight, and power (SWaP) characteristics compared to historical spinning-mass gyroscopes and pendulous accelerometers, making them suitable for a wide range of applications including drones, robotics, and mobile devices. FOGs and RLGs in particular are becoming increasingly popular in the aerospace, maritime, and defense sectors due to their improved performance and lower cost. That said, such INSs are still highly reliant on GNSS position fixes for long-term accuracy, as even high-quality IMUs will drift over time without external corrections. - -`strapdown-rs` is a Rust-based software library and simulation toolkit for implementing and testing strapdown inertial navigation systems (INS). It provides core functionality for processing inertial measurement unit (IMU) data to estimate position, velocity, and orientation using a strapdown mechanization model that is typical of modern systems, particularly in the low SWaP domain (cell phones, drones, robotics, UAVs, UUVs, etc.). Additionally, it provides a comprehensive simulation framework for safely testing INS performance under degraded and denied GNSS conditions—scenarios increasingly relevant for autonomous systems operating in contested or signal-denied environments. - -`strapdown-rs` prioritizes correctness, numerical stability, and performance. It is built with extensibility in mind, allowing researchers and engineers to implement additional filtering, sensor fusion, or aiding algorithms on top of the base INS framework. This library is not intended to be a full-featured INS solution. Notably it does not have code for processing raw IMU or GPS signals from hardware and currently only implements a loosely-coupled INS. - -The toolbox is designed for research, teaching, and development purposes and aims to serve the broader robotics, aerospace, and autonomous systems communities. The intent is to provide a high-performance, memory-safe, and modern cross-platform implementation of strapdown INS algorithms that can be easily integrated into existing systems or research workflows. The simulation framework enables users to generate synthetic navigation scenarios with controllable GNSS degradation, allowing for systematic testing and verification of INS algorithms under various failure modes. - -## Statement of Need - -Existing inertial navigation system (INS) implementations are often fragmented across proprietary MATLAB code, legacy C/C++ systems, and Python prototypes—each with significant drawbacks for open, reproducible, and high-performance research. MATLAB and Python are widely used for prototyping but are unsuitable for production or real-time systems, while C/C++ implementations are difficult to maintain and extend, lack modern tooling, and are rarely open source. This landscape forces researchers to translate algorithms between languages or pay for expensive commercial solutions, increasing complexity and the risk of errors, and makes it challenging to build reusable, extensible, and high-performance INS libraries. The motivating example for the development of this library is the notion of "vectorized" calculations. - -Research in navigation algorithms, particularly alternative Positioning, Navigation, and Timing (PNT) methods, often necessitates the evaluation of other filter architectures such as particle filters or factor graph methods. Implementing these algorithms in an object-oriented manner in Python or MATLAB is straightforward, but the performance constraints make it difficult to evaluate them under realistic conditions. A common workaround is to implement the mathematics in a vectorized manner using NumPy or MATLAB's array operations. This can provide significant performance improvements, but it also introduces complexity, duplicates functionality, and can make the code less transparent and harder to debug. Additionally, vectorized implementations may not be suitable for real-time applications. There is a clear need for a modern, open, and high-performance toolkit that bridges the gap between rapid prototyping and compiled performance with a production environment in mind, enabling both research and production use without sacrificing performance, safety, or reproducibility. `strapdown-rs` addresses this gap by providing an INS library and simulation framework that is both accessible and suitable for high-performance applications. - -The second need motivating the development of this library is to produce reliable and reproducible simulation of GNSS-denied scenarios. Such scenarios are a critical need for the navigation community, as collecting real-world data under signal denial typically requires expensive and logistically challenging field tests with specialized jamming hardware. Such tests are not only costly and difficult to repeat, but also raise regulatory and safety concerns, limiting their accessibility for most researchers. A robust simulation framework enables systematic evaluation of navigation algorithms under a wide range of degraded or denied GNSS conditions, supporting fair comparison, rapid iteration, and transparent reporting of results. By providing configurable, open-source tools for simulating GNSS outages, degradation, and spoofing, `strapdown-rs` empowers researchers to develop and validate robust navigation solutions without the barriers of hardware-based field-testing. - -## State of the Field - -Several open-source tools exist for inertial navigation research, but each carries significant limitations for the use cases addressed by `strapdown-rs`. The `gnss-ins-sim` Python package [@gnss-ins-sim] offers a simulation environment that produces similar outputs but is limited to Python's performance constraints and does not support GNSS degradation fault modeling. GTSAM [@gtsam] provides a powerful factor-graph framework in C++ and Python, but is a more general-purpose computer vision library rather than a dedicated INS toolkit, requiring substantial configuration overhead for straightforward strapdown navigation tasks. OpenVINS [@geneva2020openvins] is a high-quality visual-inertial odometry system, but is tightly coupled to ROS and oriented toward camera-IMU fusion rather than GNSS-aided INS research. Numerous MATLAB implementations exist in academic repositories, but these are proprietary, non-portable, and unsuitable for deployment in performance-sensitive applications. - -No existing open-source tool in a single package provides: a standalone library for strapdown mechanization suitable for production, a configurable GNSS degradation simulation framework with reproducible, seeded scenarios, and a configurable INS implementation. Contributing these capabilities to an existing project was not viable because the language choice, which is central to the safety and performance goals of this work, is incompatible with Python- or C++-based frameworks. `strapdown-rs` fills this gap as a self-contained, high-performance, reproducible toolkit aimed specifically at GNSS-denied navigation research. - -## Overview of Functionality - -`strapdown-rs` provides both core navigation algorithms and simulation capabilities through two main components: the `strapdown-core` library and the `strapdown-sim` binary. The library contains four primary modules: `earth`, `strapdown`, `filter`, and `sim` (for simulation utilities). The binary provides a command-line interface for running navigation simulations with configurable GNSS degradation scenarios. - -### Core Library Modules - -The `earth` module contains constants and functions related to the Earth's shape and other geophysical features (gravity and magnetic field). The Earth is modeled as an ellipsoid with a semi-major axis and a semi-minor axis [@wgs84]. The Earth's gravity is modeled as a function of the latitude and altitude using the Somigliana method. The Earth's magnetic field is modeled using a dipole model [@wmm]. This module relies on the nav-types crate [@nav-types] 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. - -The `strapdown` module provides the forward mechanization equations for strapdown inertial navigation systems. It provides a set of structs for modeling both IMU data and the base nine-element strapdown state (latitude, longitude, and altitude; velocities north, east, and down; and attitude). It includes an implementation for the local-level frame forward mechanization, which is a common approach for strapdown INS and follows the equations from Chapter 5.4 of [@groves]. This module serves as the foundation for both dead reckoning and filtered navigation solutions. - -The `filter` module contains the core functionality for implementing strapdown INS algorithms, primarily a loosely-coupled integration architecture according to Chapter 14.1.2 of [@groves]. This module contains implementations of various inertial navigation filters, including an Unscented Kalman Filter (UKF) and particle filter. These filters are used to estimate the state of a strapdown inertial navigation system based on IMU measurements and other sensor data. The filters use the strapdown equations (provided by the strapdown module) to propagate the state in the local level frame. The module also provides measurement models for GPS position and velocity updates. - -The `sim` module provides utilities for simulation and testing, including data structures for test data records (compatible with the Sensor Logger app format), navigation results, and simulation functions for both dead reckoning and closed-loop navigation. Critically, this module includes the event stream framework that enables simulation of GNSS degradation scenarios. - -The `messages` module implements an event-driven architecture for simulating GNSS degradation. It allows users to convert data contained in a tabular format (e.g. CSV files) into a sequence of timestamped events that can be processed by the navigation filters, mimicking real-time sensor data streams and allowing for reproducible testing of degraded input. This module provides: - -- **GNSS Schedulers**: Control measurement availability through passthrough (all measurements), fixed-interval sampling, or duty-cycle patterns (on/off periods) -- **Fault Models**: Simulate measurement corruption through various mechanisms including degraded measurements (AR(1) noise processes), slow bias drift (position/velocity drift with rotation), and hijacking/spoofing (position offset injection) -- **Event Streams**: Transform raw sensor data into sequences of IMU prediction steps and measurement updates, with fault injection applied according to the configured degradation model - -### Simulation Binary - -The `strapdown-sim` binary provides a command-line interface for running navigation simulations and generating synthetic datasets. It can be operated in three modes. In synthetic data generation mode it can generate realistic trajetories from perfect kinematics based on an IMU classification. In open-loop mode (dead reckoning) the system propagates the state using the strapdown equations and IMU measurements without any corrections from aiding sensors. This mode is only recommended when analyzing high-quality IMUs, as MEMS-grade sensors will accumulate significant drift within seconds to minutes. In closed-loop mode (GNSS-aided INS) the system uses a 15-state UKF (9 navigation states + 6 IMU bias states) to estimate the state and correct it using GPS measurements. This mode supports extensive GNSS degradation simulation capabilities that can be used to evaluate the robustness of navigation algorithms under various failure modes such as: - -- **Signal dropouts**: Simulated via duty-cycle schedulers with configurable on/off periods -- **Reduced update rates**: Implemented through fixed-interval schedulers -- **Measurement degradation**: AR(1) colored noise processes applied to position and velocity measurements with configurable correlation and noise levels -- **Slow bias/drift**: Gradual position and velocity bias accumulation, optionally with rotation to simulate realistic drift patterns -- **Spoofing/hijacking**: Position offset injection over specified time windows to simulate spoofing attacks - -These degradation scenarios can be configured individually or combined, and can be specified either through command-line arguments or configuration files (JSON, YAML, or TOML). This enables systematic testing of INS performance under controlled degradation conditions—critical for developing robust navigation systems for contested environments. - -## Software Design - -The central design decision in `strapdown-rs` is the choice of Rust as the implementation language. Rust provides memory safety guarantees without garbage collection, eliminating entire classes of bugs—buffer overflows, use-after-free, and data races—that are common in navigation C/C++ codebases and difficult to detect in safety-critical systems. This comes at no runtime performance cost relative to C/C++, making `strapdown-rs` suitable for future real-time deployment, unlike Python-based alternatives. - -The workspace is split into two crates: `strapdown-core` (library) and `strapdown-sim` (CLI binary). This separation ensures that researchers who want only the navigation algorithms—without the simulation scaffolding—can depend on the core library independently. Within `strapdown-core`, trait-based abstractions (`NavigationFilter`, `MeasurementModel`) decouple the filter implementations from specific sensor models, allowing new filters or measurement sources to be added without modifying existing code. - -The UKF was chosen over the more common Extended Kalman Filter (EKF) because the local-level frame strapdown equations are nonlinear, and the unscented transform avoids first-order linearization errors without requiring Jacobian derivations. The 9-state core strapdown state is extended to 15 states in the UKF to estimate IMU accelerometer and gyroscope biases, which are the dominant error sources for MEMS-grade sensors. The event-driven GNSS simulation architecture deliberately separates measurement scheduling, fault injection, and filter update logic into independent components, enabling systematic scenario construction and reproducible experiments. - -## Key Technical Contributions - -The primary technical contributions of `strapdown-rs` to the open-source navigation community are: - -1. A well-documented, numerically stable implementation of the local-level frame strapdown equations following [@groves], providing a reusable foundation for navigation algorithm development. -1. A reference implementation of a 15-state UKF-based INS with GPS position and velocity aiding, suitable for MEMS-grade IMUs commonly found in robotics and low-SWaP applications. -1. A comprehensive toolkit for safely simulating INS performance under various GNSS degradation scenarios without requiring real-world testing in denied environments. The event-driven architecture separates concerns between measurement scheduling, fault injection, and navigation filtering, enabling researchers to systematically evaluate algorithm robustness. - -These contributions make `strapdown-rs` particularly valuable for researchers developing alternative or complementary navigation algorithms for autonomous systems that must operate in contested or signal-denied environments, such as urban canyons, indoor spaces, or GPS-jammed areas. - -## Research Impact Statement - -`strapdown-rs` has been developed in direct support of ongoing navigation research at Temple University. The particle filter implementation and GNSS degradation simulation framework have been used in research exploring geophysical anomaly based navigation in a UKF [@anom-ukf] and Rao-Blackwellized particle filter [@anom-pf] approaches to geophysical anomaly-aided navigation, with associated data processing pipelines and analysis built on top of the `strapdown-core` library. This work has produced processed datasets and reproducible experimental configurations that are archived alongside the software, providing the community-readiness signals that support external replication. - -The software's design for reproducibility—seeded random number generation, configuration-file-driven scenarios, and deterministic strapdown propagation—directly addresses a gap identified by the navigation research community: the lack of a publicly available, language-agnostic, reproducible baseline for evaluating GNSS-denied INS algorithms. Researchers can clone the repository, obtain the pre-processed input datasets, and reproduce published simulation results without access to proprietary tools or hardware. - -As a Cargo-published crate, `strapdown-core` is available for direct dependency by other Rust projects. The clean trait-based interface for filters and measurement models is designed to lower the barrier for researchers wishing to implement and compare novel filtering algorithms—such as factor graph methods or learned observation models—against the provided UKF baseline, using identical strapdown mechanization and GNSS degradation scenarios. - -## AI Usage Disclosure - -Generative AI tools, specifically Claude (Anthropic), were used during the development of this software and paper. AI was used in a pair-programming capacity during implementation of several modules in `strapdown-core` and `strapdown-sim`, and as a copy editor for documentation strings and this paper. All AI-generated or AI-assisted code was reviewed by the author, validated against the reference equations in [@groves], and verified through the project's unit and integration test suite (`cargo test --workspace`). All AI-assisted prose was reviewed and edited by the author for technical accuracy. The core algorithmic content (strapdown mechanization equations, UKF sigma-point formulation, and particle filter resampling strategies) is based directly on established references and was verified by the author independently of any AI-generated suggestions. - -## References \ No newline at end of file +An inertial navigation system (INS) estimates position, velocity and attitude by integrating +the specific force and angular rate measured by an inertial measurement unit (IMU). Modern +IMUs, from the micro-electro-mechanical (MEMS) parts in phones and drones to fibre-optic and +ring-laser gyroscopes, are *strapdown*: fixed to the vehicle, with the navigation equations +doing the work a gimballed platform once did. Every IMU drifts, so an INS is normally aided by +GNSS. Losing GNSS to jamming, spoofing or obstruction is a central problem in positioning, +navigation and timing (PNT) research. + +`strapdown-rs` is a Rust library and command-line toolkit for that research. The +`strapdown-core` crate implements local-level strapdown mechanization and four interchangeable +loosely coupled navigation filters. The `strapdown-sim` binary replays recorded or synthetic +IMU/GNSS data through those filters. It withholds, thins or corrupts the GNSS stream according +to a seeded scenario described in a configuration file. An experimental `strapdown-geonav` +crate adds gravity- and magnetic-anomaly map matching as a GNSS alternative. + +## Statement of need + +Navigation algorithms are usually prototyped in MATLAB or Python. Prototypes in those +languages run fast only when the mathematics is rewritten in vectorized array form. That +rewrite is particularly awkward for particle filters, whose natural expression is a loop over +particles that each carry their own state. It obscures the algorithm, duplicates logic and is +hard to debug. C and C++ give speed, but without memory safety, which makes a library hard to +extend safely. `strapdown-rs` lets filters be written as plainly as the textbook presents them +and compiled. The same code can then be reused unchanged from a research script, a CLI +experiment or an application. + +The second need is reproducible GNSS-denied experiments. Collecting real data under denial +requires jamming hardware, test ranges and regulatory approval, so few researchers can, and no +field test can be repeated exactly. A simulation can take real or synthetic trajectories and +remove or corrupt GNSS in a controlled, seeded way. That lets algorithms be compared under +identical conditions, and lets others re-run the comparison. `strapdown-rs` targets graduate +students, researchers and engineers working on INS/GNSS integration and alternative PNT. + +## State of the field + +- `gnss-ins-sim` [@gnss-ins-sim] generates synthetic IMU and GNSS data in Python. It has no + GNSS fault models, and its filters are examples rather than a reusable library. +- GTSAM [@gtsam] is a general factor-graph library with IMU preintegration. Using it for a + conventional loosely coupled INS means building the navigation stack yourself, and it offers + no degradation scenarios. +- OpenVINS [@geneva2020openvins] is a mature visual-inertial estimator built on ROS. It is + designed around camera-IMU fusion rather than GNSS-aided navigation. +- Academic MATLAB implementations, typically following @groves, are widely shared. They + require a proprietary runtime and are seldom packaged for reuse. + +We built a new package rather than contributing to one of these because our two goals are +joint. We need a memory-safe compiled library and a seeded scenario engine that share one +filter interface. That combination does not fit inside a Python simulator, a C++ factor-graph +framework or a ROS stack without rewriting the host project. + +## Software design + +**One filter interface.** Every filter implements the object-safe `NavigationFilter` trait: +`predict` on an IMU sample, and `update` against any `MeasurementModel`. The CLI, the +`InsEngine` builder for library users, and the test suite all drive filters through this +trait. Four implementations share one mechanization, which follows @groves (Chapter 5.4): + +- an error-state Kalman filter (ESKF) with a multiplicative attitude error; +- a total-state extended Kalman filter (EKF); +- an unscented Kalman filter (UKF); +- the Rao-Blackwellized particle filter of @canciani2017. It samples only horizontal position + and shares one Kalman filter across all particles for the remaining states. + +The ESKF is the default. Its error state stays small and near zero, which keeps linearization +accurate and avoids attitude-parameterization singularities. It estimates IMU biases at modest +cost, and on the regression scenarios its accuracy is indistinguishable from the UKF's. The +EKF's analytic Jacobians are tested against finite differences of the mechanization, because +a wrong Jacobian does not crash a filter; it quietly degrades it. + +**Scenarios as an event stream.** Input records become a time-ordered stream of IMU +propagation steps and measurement events. Two independent stages act on that stream: +- *Scheduling* decides when GNSS is available: pass-through, fixed-interval, or duty-cycle + outages. +- *Fault injection* corrupts the fixes that are delivered: correlated AR(1) noise, slow bias + drift, position hijacking, or combinations of these. + +A filter therefore cannot distinguish a simulated outage from a real one, and new faults +require no filter changes. All randomness is seeded. + +**Correctness over convenience.** +- The library returns a `StrapdownError` instead of panicking, and lints deny `unwrap`, + `expect` and `panic` in library code. +- The navigation frame (NED by default, ENU on request) is checked against the gravity + direction measured in the data rather than assumed. +- Process noise is specified as a spectral density, so tuning does not change with sample + rate. + +## Functionality + +- Strapdown mechanization on the WGS84 ellipsoid [@wgs84] with Somigliana gravity, built on + `nav-types` [@nav-types]. +- Aiding by: + - GNSS position and velocity; + - barometric altitude, with bias estimation; + - magnetometer heading, corrected for declination by the World Magnetic Model [@wmm]; + - zero-velocity and zero-angular-rate updates, triggered by a stationarity detector. +- Chi-squared innovation gating with recovery. +- Coarse alignment, IMU calibration and lever-arm compensation. +- GNSS degradation scenarios configured from TOML, YAML or JSON files or from CLI flags. +- A synthetic trajectory generator with IMU error models by sensor grade. +- Accuracy and consistency metrics: RMSE, CEP, NEES and NIS. +- CSV output from the CLI, plus HDF5, NetCDF and MCAP writers in the library. +- Experimental geophysical navigation in `strapdown-geonav`. +- A Python `analysis` package for preprocessing and statistics. + +The documentation is an mdBook user guide plus API documentation. A lint makes +`missing_docs` an error, so every public item is documented. + +## Research impact statement + +`strapdown-rs` is the experimental platform behind two conference papers on navigating +MEMS-grade platforms without GNSS using gravity and magnetic anomaly maps. One uses a UKF +[@anom-ukf] and the other a particle filter [@anom-pf]. Both are built on the toolkit's +filters, its GNSS-denial scenarios and its geophysical measurement models. The scenario +recipes for these studies are versioned in the repository's `conf/` directory. + +The reproducibility claims are tested rather than asserted: +- About 820 Rust tests run in continuous integration on Linux, macOS and Windows, alongside + 57 Python tests. +- An accuracy regression suite runs every filter over recorded and synthetic scenarios: clean + GNSS, sparse fixes, a 60-second outage, degraded GNSS, and dead reckoning. +- The suite fails the build when a gated metric moves beyond its recorded tolerance. The + baseline tables in the documentation are generated from the same results. +- Synthetic trajectories make every scenario reproducible without the original recordings. + +The v1.0 release publishes `strapdown-core`, `strapdown-sim` and `strapdown-geonav` to +crates.io. Other Rust projects can then depend directly on the mechanization and filters, and +compare new estimators against the provided baselines under identical scenarios. + +## AI usage disclosure + +Generative AI was used substantially in this project. The repository history records commits +authored by Anthropic's Claude Code and by the GitHub Copilot coding agent alongside the +author's own. These tools implemented features, wrote tests and documentation, and +investigated defects. The author directed the work, reviewed each change before merging it, +and checked the algorithms against @groves and @canciani2017. + +Correctness rests on checks that do not depend on trusting the generated code: +- the unit and integration tests; +- finite-difference verification of the analytic Jacobians; +- the accuracy baseline gated in CI; +- lints that forbid panics in library code. + +This paper was drafted with AI assistance from the author's notes and the code, then revised +and verified by the author. + +## Acknowledgements + +This work received no external funding. + +## References diff --git a/sim/src/main.rs b/sim/src/main.rs index dd4f0173..c6486d11 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -624,9 +624,14 @@ struct ParticleFilterSimArgs { geo_bias: GeophysicalBiasArgs, /// Random walk on the sampled horizontal position error as `north,east` in m/sqrt(s). - /// Default: 0, Canciani & Raquet's eq. 19, which diverges with GNSS-rate fixes on MEMS - /// data; the recipes under `conf/` use `1,1`. - #[arg(long, value_delimiter = ',', num_args = 2, default_value = "0,0")] + /// 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. + #[arg( + long, + value_delimiter = ',', + num_args = 2, + default_values_t = [strapdown::sim::DEFAULT_PF_HORIZONTAL_PROCESS_NOISE_STD_M; 2] + )] horizontal_process_noise_std_m: Vec, /// Time constant of the barometer loop in the mechanization (s). Default: 10. @@ -4375,6 +4380,34 @@ magnetic_bias_process_noise_std = 3.0 ); } + /// `pf` with no flags and a `[particle_filter]` section that sets nothing run the same + /// horizontal process noise, and it is not the library filter's zero. + /// + /// With the zero, `strapdown-sim pf` diverged on a 10-minute synthetic trajectory and + /// failed its speed check at 500 m/s; so did both shipped example configs that run the + /// particle filter, because neither set the key. + #[test] + fn pf_defaults_to_a_horizontal_process_noise_that_survives_gnss_rate_fixes() { + let cli = Cli::try_parse_from(["strapdown-sim", "pf", "-i", "in.csv", "-o", "out.csv"]) + .expect("the flags above must parse"); + let Some(Command::ParticleFilter(args)) = cli.command else { + panic!("expected the `pf` subcommand"); + }; + let from_config = strapdown::sim::ParticleFilterConfig::default(); + assert_eq!( + args.horizontal_process_noise_std_m, + from_config.horizontal_process_noise_std_m + ); + assert!( + from_config + .horizontal_process_noise_std_m + .iter() + .all(|&std| std > 0.0), + "{:?}", + from_config.horizontal_process_noise_std_m + ); + } + /// `pf`'s flags and a config file resolve to the same map-bias priors. /// /// `pf` flattened `--gravity-bias` in with the other geophysical flags and then ignored it,