diff --git a/Cargo.lock b/Cargo.lock index 2e9cb0d..a04ba2f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2008,6 +2008,8 @@ dependencies = [ "plotters", "rayon", "strapdown-core", + "strapdown-geonav", + "tempfile", ] [[package]] diff --git a/README.md b/README.md index 47df31d..d0ec8f3 100644 --- a/README.md +++ b/README.md @@ -38,4 +38,10 @@ The toolbox is designed for research, teaching, and development purposes and aim 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 or EKF) 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. -Both `strapdown-sim` and `geonav-sim` include 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. \ No newline at end of file +`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. + +**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 diff --git a/geonav/Cargo.toml b/geonav/Cargo.toml index 7ab8802..8d35a8f 100644 --- a/geonav/Cargo.toml +++ b/geonav/Cargo.toml @@ -24,6 +24,6 @@ env_logger = "0.11" name = "geonav" path = "src/lib.rs" -[[bin]] -name = "geonav-sim" -path = "src/main.rs" +# Note: The geonav-sim binary has been consolidated into strapdown-sim. +# Use: cargo build --package strapdown-sim --features geonav +# Run: strapdown-sim cl --geo --gravity-resolution ... diff --git a/geonav/README.md b/geonav/README.md index f88634f..f8de19b 100644 --- a/geonav/README.md +++ b/geonav/README.md @@ -1,168 +1,134 @@ -# Geonav-Sim Usage Guide +# Strapdown Geonav - Geophysical Navigation Library -This guide demonstrates how to use the `geonav-sim` program for geophysical navigation simulations. +This crate provides geophysical navigation capabilities for strapdown inertial navigation systems, enabling gravity and magnetic anomaly aiding for improved navigation accuracy, particularly in GNSS-denied environments. ## Overview -`geonav-sim` extends the basic strapdown navigation simulation by incorporating geophysical measurements (gravity and magnetic anomalies) to aid navigation, particularly in GNSS-denied environments. +`strapdown-geonav` is a library crate that extends the basic strapdown navigation simulation by incorporating geophysical measurements (gravity and magnetic anomalies) to aid navigation. -## Basic Usage +**Note**: The standalone `geonav-sim` binary has been consolidated into `strapdown-sim`. Use `strapdown-sim` with the `--features geonav` flag to access geophysical navigation capabilities. + +## Usage + +### As a Simulation Tool + +Build and run with geophysical navigation support: ```bash -# Basic gravity-aided navigation simulation -geonav-sim -i data/input.csv -o results/output.csv --geo-type gravity - -# Magnetic-aided navigation simulation with higher resolution -geonav-sim -i data/input.csv -o results/output.csv \ - --geo-type magnetic \ - --geo-resolution two-minutes \ - --geo-noise-std 50.0 - -# Custom map file specification -geonav-sim -i data/input.csv -o results/output.csv \ - --geo-type gravity \ - --map-file data/custom_gravity_map.nc +# Build strapdown-sim with geonav feature +cargo build --release --package strapdown-sim --features geonav + +# Run geophysical navigation simulation +strapdown-sim cl --input data.csv --output out/ \ + --geo \ + --gravity-resolution one-minute \ + --filter ukf + +# Magnetic navigation +strapdown-sim cl --input data.csv --output out/ \ + --geo \ + --magnetic-resolution two-minutes \ + --magnetic-noise-std 150.0 ``` -## Input File Requirements +### As a Library -### CSV Data File -The input CSV must contain standard IMU/GNSS data with the following columns: -- `time` - ISO UTC timestamp (YYYY-MM-DD HH:mm:ss.ssss+HH:MM) -- `latitude`, `longitude`, `altitude` - Position measurements -- `speed`, `bearing` - Velocity measurements -- `acc_x`, `acc_y`, `acc_z` - Accelerometer data (m/s²) -- `gyro_x`, `gyro_y`, `gyro_z` - Gyroscope data (rad/s) -- `grav_x`, `grav_y`, `grav_z` - Gravity measurements (m/s²) [for gravity-aided navigation] -- `mag_x`, `mag_y`, `mag_z` - Magnetometer data (μT) [for magnetic-aided navigation] +Add to your `Cargo.toml`: -### Geophysical Map Files -Maps should be in NetCDF format with specific naming conventions: +```toml +[dependencies] +strapdown-geonav = { path = "../geonav" } +``` -- **Gravity maps**: `{input_name}_gravity.nc` -- **Magnetic maps**: `{input_name}_magnetic.nc` +Use in your code: + +```rust +use geonav::{GeoMap, GeophysicalMeasurementType, GravityResolution}; +use geonav::{build_event_stream, geo_closed_loop_ukf}; + +// 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 +let results = geo_closed_loop_ukf(&mut ukf, events)?; +``` -For example, if your input CSV is `flight_data.csv`, the program will look for: -- `flight_data_gravity.nc` (for gravity-aided navigation) -- `flight_data_magnetic.nc` (for magnetic-aided navigation) +## Command Line Options -#### Map File Structure -Maps should contain: -- `lat` variable: latitude coordinates (degrees) -- `lon` variable: longitude coordinates (degrees) -- `z` variable: anomaly data (mGal for gravity, nT for magnetic) +When using `strapdown-sim --features geonav`: -## Command Line Options +### Enabling Geophysical Navigation +- `--geo`: Enable geophysical navigation mode (required) -### Geophysical Measurement Configuration -- `--geo-type`: Choose between `gravity` or `magnetic` measurements -- `--geo-resolution`: Map resolution (from `one-degree` to `one-second`) -- `--geo-noise-std`: Measurement noise standard deviation -- `--map-file`: Custom map file path (overrides auto-detection) +### Gravity Configuration +- `--gravity-resolution`: Map resolution (one-degree to one-minute) +- `--gravity-bias`: Measurement bias in mGal +- `--gravity-noise-std`: Noise standard deviation in mGal (default: 100) +- `--gravity-map-file`: Custom map file path -### GNSS Degradation Configuration -The program inherits all GNSS degradation options from `strapdown-sim`: +### Magnetic Configuration +- `--magnetic-resolution`: Map resolution (one-degree to two-minutes) +- `--magnetic-bias`: Measurement bias in nT +- `--magnetic-noise-std`: Noise standard deviation in nT (default: 150) +- `--magnetic-map-file`: Custom map file path -#### Scheduler Options (controls GNSS availability) -- `--sched passthrough`: Use all available GNSS measurements -- `--sched fixed --interval-s 10`: GNSS fixes every 10 seconds -- `--sched duty --on-s 30 --off-s 60`: 30s ON, 60s OFF cycles +### Common Options +- `--geo-frequency-s`: Geophysical measurement frequency in seconds -#### Fault Models (corrupts GNSS measurements) -- `--fault none`: No GNSS corruption -- `--fault degraded`: AR(1) correlated errors -- `--fault slowbias`: Slow-drifting bias -- `--fault hijack`: Position spoofing attack +## Geophysical Map Files -## Example Scenarios +Maps should be in NetCDF format: -### 1. GNSS-Denied Navigation with Gravity Aiding -```bash -# Simulate complete GNSS denial with gravity measurements -geonav-sim -i urban_canyon.csv -o gravity_aided.csv \ - --geo-type gravity \ - --geo-noise-std 50.0 \ - --sched fixed --interval-s 999999 # Effectively no GNSS -``` +- **Gravity maps**: `{input_name}_gravity.nc` or specified via `--gravity-map-file` +- **Magnetic maps**: `{input_name}_magnetic.nc` or specified via `--magnetic-map-file` -### 2. Intermittent GNSS with Magnetic Aiding -```bash -# GNSS available 20% of the time, magnetic aiding throughout -geonav-sim -i flight_data.csv -o magnetic_aided.csv \ - --geo-type magnetic \ - --geo-resolution five-minutes \ - --sched duty --on-s 10 --off-s 40 -``` +### Map File Structure +Maps should contain: +- `lat` variable: latitude coordinates (degrees) +- `lon` variable: longitude coordinates (degrees) +- `z` variable: anomaly data (mGal for gravity, nT for magnetic) + +## Example Scenarios -### 3. GNSS Spoofing with Geophysical Validation +### GNSS-Denied Navigation with Gravity Aiding ```bash -# Simulate spoofing attack with gravity measurements for validation -geonav-sim -i test_route.csv -o spoofing_test.csv \ - --geo-type gravity \ - --fault hijack --hijack-offset-n-m 100 --hijack-start-s 300 +strapdown-sim cl --input urban_canyon.csv --output out/ \ + --geo --gravity-resolution one-minute \ + --dropout-start-s 0 --dropout-duration-s 999999 ``` -### 4. High-Precision Survey with Multiple Aiding +### Intermittent GNSS with Magnetic Aiding ```bash -# High-resolution gravity aiding with minimal GNSS degradation -geonav-sim -i survey_data.csv -o high_precision.csv \ - --geo-type gravity \ - --geo-resolution one-minute \ - --geo-noise-std 10.0 \ - --fault degraded --sigma-pos-m 1.0 +strapdown-sim cl --input flight_data.csv --output out/ \ + --geo --magnetic-resolution five-minutes \ + --sched duty --on-s 10 --off-s 40 ``` -## Output +## API Documentation -The program generates a CSV file with navigation results including: -- Estimated position, velocity, and attitude -- Covariance diagonal elements -- Input measurements and derived quantities -- Geophysical anomaly values used for aiding +See the Rust API documentation for detailed information on: +- `GeoMap` - Geophysical map loading and interpolation +- `GravityMeasurement` - Gravity anomaly measurement model +- `MagneticAnomalyMeasurement` - Magnetic anomaly measurement model +- `build_event_stream` - Event stream construction with geophysical measurements +- `geo_closed_loop_ukf` / `geo_closed_loop_ekf` - Simulation functions -## Performance Considerations +## Performance Notes -- **Map Resolution**: Higher resolution maps provide better accuracy but require more memory and computation -- **Noise Levels**: Typical values: +- **Map Resolution**: Higher resolution maps provide better accuracy but require more memory +- **Typical Noise Levels**: - Gravity: 10-100 mGal standard deviation - Magnetic: 50-200 nT standard deviation - **Coverage**: Ensure geophysical maps cover the entire trajectory area - -## Troubleshooting - -### Common Issues -1. **Map file not found**: Ensure proper naming convention or use `--map-file` -2. **Out of bounds errors**: Trajectory extends beyond map coverage -3. **Memory issues**: Very high-resolution maps may require significant RAM - -### Error Messages -- `"Map file not found"`: Check file naming and paths -- `"Latitude out of bounds"`: Trajectory extends beyond map area -- `"Failed to read config"`: GNSS config file format issues - -## Integration with Analysis Tools - -Output CSV files are compatible with standard navigation analysis tools and can be processed with: -- Python pandas/numpy for analysis -- MATLAB navigation toolboxes -- R for statistical analysis -- Excel for basic visualization - -Example Python analysis: -```python -import pandas as pd -import matplotlib.pyplot as plt - -# Load results -results = pd.read_csv('output.csv') - -# Plot trajectory -plt.figure(figsize=(10, 8)) -plt.scatter(results['longitude'], results['latitude'], - c=results['timestamp'], cmap='viridis') -plt.xlabel('Longitude (degrees)') -plt.ylabel('Latitude (degrees)') -plt.title('Geophysical Navigation Trajectory') -plt.colorbar(label='Time') -plt.show() -``` \ No newline at end of file diff --git a/geonav/src/main.rs b/geonav/src/main.rs deleted file mode 100644 index 8328fc9..0000000 --- a/geonav/src/main.rs +++ /dev/null @@ -1,836 +0,0 @@ -//! GEONAV-SIM: A geophysical navigation simulation tool for strapdown inertial navigation systems. -//! -//! This program extends the basic strapdown simulation by incorporating geophysical measurements such as -//! gravity and magnetic anomalies for enhanced navigation accuracy. It loads geophysical maps (NetCDF format) -//! and simulates how these measurements can aid inertial navigation systems, particularly in GNSS-denied environments. -//! -//! The program operates in closed-loop mode, incorporating both GNSS measurements (when available) and geophysical -//! measurements from loaded maps. It can simulate various GNSS degradation scenarios while maintaining navigation -//! accuracy through geophysical aiding. -//! -//! You can run simulations either by: -//! 1. Loading all parameters from a configuration file (TOML/JSON/YAML) -//! 2. Specifying parameters via command-line flags - -use clap::{Args, Parser, Subcommand}; -use log::{error, info}; -use std::error::Error; -use std::io; -use std::path::{Path, PathBuf}; -use std::rc::Rc; - -use geonav::{ - GeoMap, GeophysicalMeasurementType, GravityResolution, MagneticResolution, build_event_stream, - geo_closed_loop_ekf, geo_closed_loop_ukf, -}; -use strapdown::NavigationFilter; -use strapdown::kalman::{ExtendedKalmanFilter, InitialState}; -use strapdown::sim::{ - DEFAULT_PROCESS_NOISE, FaultArgs, FilterType, GeoResolution, GeonavSimulationConfig, - NavigationResult, SchedulerArgs, TestDataRecord, build_fault, build_scheduler, initialize_ukf, -}; - -const LONG_ABOUT: &str = "GEONAV-SIM: A geophysical navigation simulation tool for strapdown inertial navigation systems. - -This program extends the basic strapdown simulation by incorporating geophysical measurements such as -gravity and magnetic anomalies for enhanced navigation accuracy. It loads geophysical maps (NetCDF format) -and simulates how these measurements can aid inertial navigation systems, particularly in GNSS-denied environments. - -The program operates in closed-loop mode, incorporating both GNSS measurements (when available) and geophysical -measurements from loaded maps. It can simulate various GNSS degradation scenarios while maintaining navigation -accuracy through geophysical aiding. - -You can run simulations either by: - 1. Loading all parameters from a configuration file (TOML/JSON/YAML) - 2. Specifying parameters via command-line flags - -Input data format is identical to strapdown-sim, with additional geophysical map files: -* Input CSV: Standard IMU/GNSS data as per strapdown-sim specification -* Gravity maps: *_gravity.nc files containing gravity anomaly data -* Magnetic maps: *_magnetic.nc files containing magnetic anomaly data - -The program automatically detects and loads the appropriate map file based on the measurement type specified -and the input file directory."; - -/// Command line arguments -#[derive(Parser)] -#[command(author, version, about = "A geophysical navigation simulation tool for strapdown inertial navigation systems.", long_about = LONG_ABOUT)] -struct Cli { - /// Run simulation from a configuration file (TOML/JSON/YAML) - /// This option overrides any subcommand arguments - #[arg(short, long, global = true)] - config: Option, - - /// Command to execute (ignored if --config is provided) - #[command(subcommand)] - command: Option, - - /// Log level (off, error, warn, info, debug, trace) - #[arg(long, default_value = "info", global = true)] - log_level: String, - - /// Log file path (if not specified, logs to stderr) - #[arg(long, global = true)] - log_file: Option, - - /// Run simulations in parallel when processing multiple files - #[arg(long, global = true)] - parallel: bool, - - /// Generate performance plot comparing navigation output to GPS measurements - #[arg(long, global = true)] - plot: bool, -} - -/// Top-level commands -#[derive(Subcommand, Clone)] -enum Command { - #[command( - name = "cl", - about = "Run geophysical navigation simulation in closed-loop mode", - long_about = "Run geophysical INS simulation in a closed-loop (feedback) mode. In this mode, GNSS measurements and geophysical measurements are incorporated to correct for IMU drift 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." - )] - ClosedLoop(Box), - - #[command(name = "conf", about = "Generate a template configuration file")] - CreateConfig, -} - -/// Common simulation arguments for input/output -#[derive(Args, Clone, Debug)] -struct SimArgs { - /// Input CSV file path or directory containing CSV files - /// If a directory is provided, all CSV files in it will be processed - #[arg(short, long, value_parser)] - input: PathBuf, - - /// Output CSV file path or directory - /// When processing multiple files, output filenames will be generated as: {output_stem}_{input_stem}.csv - #[arg(short, long, value_parser)] - output: PathBuf, -} - -/// Geophysical measurement arguments -#[derive(Args, Clone, Debug)] -struct GeophysicalArgs { - // Gravity measurement parameters (all optional) - /// Gravity map resolution - #[arg(long, value_enum)] - gravity_resolution: Option, - - /// Gravity measurement bias (mGal) - #[arg(long)] - gravity_bias: Option, - - /// Gravity measurement noise std dev (mGal) - #[arg(long, default_value_t = 100.0)] - gravity_noise_std: f64, - - /// Gravity map file path - #[arg(long)] - gravity_map_file: Option, - - // Magnetic measurement parameters (all optional) - /// Magnetic map resolution - #[arg(long, value_enum)] - magnetic_resolution: Option, - - /// Magnetic measurement bias (nT) - #[arg(long)] - magnetic_bias: Option, - - /// Magnetic measurement noise std dev (nT) - #[arg(long, default_value_t = 150.0)] - magnetic_noise_std: f64, - - /// Magnetic map file path - #[arg(long)] - magnetic_map_file: Option, - - // Common parameters - /// Geophysical measurement frequency (seconds) - #[arg(long)] - geo_frequency_s: Option, -} - -/// Closed-loop simulation arguments -#[derive(Args, Clone, Debug)] -struct ClosedLoopSimArgs { - /// Common simulation input/output arguments - #[command(flatten)] - sim: SimArgs, - - /// Filter type to use for closed-loop navigation - #[arg(long, value_enum, default_value_t = FilterType::Ukf)] - filter: FilterType, - - /// RNG seed for stochastic processes - #[arg(long, default_value_t = 42)] - seed: u64, - - /// Geophysical measurement configuration - #[command(flatten)] - geo: GeophysicalArgs, - - /// GNSS scheduler settings (dropouts / reduced rate) - #[command(flatten)] - scheduler: SchedulerArgs, - - /// Fault model settings (corrupt measurement content) - #[command(flatten)] - fault: FaultArgs, -} - -/// Initialize the logger with the specified configuration -fn init_logger(log_level: &str, log_file: Option<&PathBuf>) -> Result<(), Box> { - use std::io::Write; - - let level = log_level.parse::().unwrap_or_else(|_| { - eprintln!("Invalid log level '{}', defaulting to 'info'", log_level); - log::LevelFilter::Info - }); - - let mut builder = env_logger::Builder::new(); - builder.filter_level(level); - builder.format(|buf, record| { - writeln!( - buf, - "{} [{}] - {}", - chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.3f"), - record.level(), - record.args() - ) - }); - - if let Some(log_path) = log_file { - let target = Box::new( - std::fs::OpenOptions::new() - .create(true) - .append(true) - .open(log_path)?, - ); - builder.target(env_logger::Target::Pipe(target)); - } - - builder.try_init()?; - Ok(()) -} - -/// Validate input path exists and is either a file or directory -fn validate_input_path(input: &Path) -> Result<(), Box> { - if !input.exists() { - return Err(format!("Input path '{}' does not exist.", input.display()).into()); - } - if !input.is_file() && !input.is_dir() { - return Err(format!( - "Input path '{}' is neither a file nor a directory.", - input.display() - ) - .into()); - } - Ok(()) -} - -/// Get all CSV files from a path (either single file or all CSVs in directory) -fn get_csv_files(input: &Path) -> Result, Box> { - if input.is_file() { - if input.extension().and_then(|s| s.to_str()) != Some("csv") { - return Err(format!("Input file '{}' is not a CSV file.", input.display()).into()); - } - Ok(vec![input.to_path_buf()]) - } else if input.is_dir() { - let mut csv_files: Vec = std::fs::read_dir(input)? - .filter_map(|entry| entry.ok()) - .map(|entry| entry.path()) - .filter(|path| { - path.is_file() && path.extension().and_then(|s| s.to_str()) == Some("csv") - }) - .collect(); - - if csv_files.is_empty() { - return Err(format!("No CSV files found in directory '{}'.", input.display()).into()); - } - - // Sort for consistent ordering - csv_files.sort(); - Ok(csv_files) - } else { - Err(format!( - "Input path '{}' is neither a file nor a directory.", - input.display() - ) - .into()) - } -} - -/// Validate output path and create parent directories if needed -fn validate_output_path(output: &Path) -> Result<(), Box> { - // Output is a directory, create it if it doesn't exist - if !output.exists() { - std::fs::create_dir_all(output)?; - } - Ok(()) -} - -/// Convert GeoResolution to GravityResolution -fn convert_resolution_gravity(resolution: GeoResolution) -> GravityResolution { - match resolution { - GeoResolution::OneDegree => GravityResolution::OneDegree, - GeoResolution::ThirtyMinutes => GravityResolution::ThirtyMinutes, - GeoResolution::TwentyMinutes => GravityResolution::TwentyMinutes, - GeoResolution::FifteenMinutes => GravityResolution::FifteenMinutes, - GeoResolution::TenMinutes => GravityResolution::TenMinutes, - GeoResolution::SixMinutes => GravityResolution::SixMinutes, - GeoResolution::FiveMinutes => GravityResolution::FiveMinutes, - GeoResolution::FourMinutes => GravityResolution::FourMinutes, - GeoResolution::ThreeMinutes => GravityResolution::ThreeMinutes, - GeoResolution::TwoMinutes => GravityResolution::TwoMinutes, - GeoResolution::OneMinute => GravityResolution::OneMinute, - // For gravity, highest resolution available is 1 minute - _ => GravityResolution::OneMinute, - } -} - -/// Convert GeoResolution to MagneticResolution -fn convert_resolution_magnetic(resolution: GeoResolution) -> MagneticResolution { - match resolution { - GeoResolution::OneDegree => MagneticResolution::OneDegree, - GeoResolution::ThirtyMinutes => MagneticResolution::ThirtyMinutes, - GeoResolution::TwentyMinutes => MagneticResolution::TwentyMinutes, - GeoResolution::FifteenMinutes => MagneticResolution::FifteenMinutes, - GeoResolution::TenMinutes => MagneticResolution::TenMinutes, - GeoResolution::SixMinutes => MagneticResolution::SixMinutes, - GeoResolution::FiveMinutes => MagneticResolution::FiveMinutes, - GeoResolution::FourMinutes => MagneticResolution::FourMinutes, - GeoResolution::ThreeMinutes => MagneticResolution::ThreeMinutes, - GeoResolution::TwoMinutes => MagneticResolution::TwoMinutes, - // For magnetic, highest resolution available is 2 minutes - _ => MagneticResolution::TwoMinutes, - } -} - -/// Auto-detect gravity map file based on input directory -fn find_gravity_map(input_path: &Path) -> Result> { - let input_dir = input_path - .parent() - .ok_or("Cannot determine input directory")?; - - let input_stem = input_path - .file_stem() - .ok_or("Cannot determine input file stem")? - .to_string_lossy(); - - let map_file = input_dir.join(format!("{}_gravity.nc", input_stem)); - - if map_file.exists() { - Ok(map_file) - } else { - Err(format!("Gravity map file not found: {}", map_file.display()).into()) - } -} - -/// Auto-detect magnetic map file based on input directory -fn find_magnetic_map(input_path: &Path) -> Result> { - let input_dir = input_path - .parent() - .ok_or("Cannot determine input directory")?; - - let input_stem = input_path - .file_stem() - .ok_or("Cannot determine input file stem")? - .to_string_lossy(); - - let map_file = input_dir.join(format!("{}_magnetic.nc", input_stem)); - - if map_file.exists() { - Ok(map_file) - } else { - Err(format!("Magnetic map file not found: {}", map_file.display()).into()) - } -} - -/// Convert GeophysicalArgs to GeophysicalConfig -fn geophysical_args_to_config(args: &GeophysicalArgs) -> strapdown::sim::GeophysicalConfig { - strapdown::sim::GeophysicalConfig { - gravity_resolution: args.gravity_resolution, - gravity_bias: args.gravity_bias, - gravity_noise_std: Some(args.gravity_noise_std), - gravity_map_file: args - .gravity_map_file - .as_ref() - .map(|p| p.to_string_lossy().to_string()), - magnetic_resolution: args.magnetic_resolution, - magnetic_bias: args.magnetic_bias, - magnetic_noise_std: Some(args.magnetic_noise_std), - magnetic_map_file: args - .magnetic_map_file - .as_ref() - .map(|p| p.to_string_lossy().to_string()), - geo_frequency_s: args.geo_frequency_s, - } -} - -/// Process a single CSV file with geophysical navigation -fn process_file( - input_file: &Path, - output: &Path, - config: &GeonavSimulationConfig, -) -> Result<(), Box> { - info!("Processing file: {}", input_file.display()); - - // Load sensor data - let records = TestDataRecord::from_csv(input_file)?; - info!( - "Read {} records from {}", - records.len(), - input_file.display() - ); - - // Load gravity map if configured - let gravity_map = if let Some(res) = config.geophysical.gravity_resolution { - let map_path = match &config.geophysical.gravity_map_file { - Some(path) => PathBuf::from(path), - None => find_gravity_map(input_file)?, - }; - - info!("Loading gravity map from: {}", map_path.display()); - let measurement_type = 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", - map.get_lats().len(), - map.get_lons().len() - ); - Some(map) - } else { - None - }; - - // Load magnetic map if configured - let magnetic_map = if let Some(res) = config.geophysical.magnetic_resolution { - let map_path = match &config.geophysical.magnetic_map_file { - Some(path) => PathBuf::from(path), - None => find_magnetic_map(input_file)?, - }; - - info!("Loading magnetic map from: {}", map_path.display()); - let measurement_type = - 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", - map.get_lats().len(), - map.get_lons().len() - ); - Some(map) - } else { - None - }; - - // Ensure at least one map is loaded - if gravity_map.is_none() && magnetic_map.is_none() { - return Err("No geophysical maps configured. At least one of gravity_resolution or magnetic_resolution must be set.".into()); - } - - // Build event stream with geophysical measurements - let events = build_event_stream( - &records, - &config.gnss_degradation, - gravity_map.clone(), - if gravity_map.is_some() { - Some(config.geophysical.get_gravity_noise_std()) - } else { - None - }, - magnetic_map.clone(), - if magnetic_map.is_some() { - Some(config.geophysical.get_magnetic_noise_std()) - } else { - None - }, - config.geophysical.geo_frequency_s, - ); - info!("Built event stream with {} events", events.events.len()); - - // Determine number of geophysical states - let num_geo_states = gravity_map.is_some() as usize + magnetic_map.is_some() as usize; - - // Run simulation based on filter type - let results = match config.filter { - FilterType::Ukf => { - info!("Initializing UKF..."); - let mut process_noise: Vec = DEFAULT_PROCESS_NOISE.into(); - // Extend for geophysical states - process_noise.extend(vec![1e-9; num_geo_states]); - - // Order is critical: gravity first, then magnetic - // This must match the state vector layout and measurement processing order - // State vector: [base states (9)..., gravity_bias?, magnetic_bias?] - let mut geo_biases = Vec::new(); - let mut geo_noise_stds = Vec::new(); - - if gravity_map.is_some() { - geo_biases.push(config.geophysical.get_gravity_bias()); - geo_noise_stds.push(config.geophysical.get_gravity_noise_std()); - } - if magnetic_map.is_some() { - geo_biases.push(config.geophysical.get_magnetic_bias()); - geo_noise_stds.push(config.geophysical.get_magnetic_noise_std()); - } - - let mut ukf = initialize_ukf( - records[0].clone(), - None, - None, - None, - Some(geo_biases), - Some(geo_noise_stds), - Some(process_noise), - ); - info!( - "Initialized UKF with state dimension {} (base: 9, geo: {})", - ukf.get_estimate().len(), - num_geo_states - ); - - info!("Running UKF geophysical navigation simulation..."); - geo_closed_loop_ukf(&mut ukf, events) - } - FilterType::Ekf => { - info!("Initializing EKF..."); - - // Construct initial state from first record - let initial_state = InitialState { - latitude: records[0].latitude, - longitude: records[0].longitude, - altitude: records[0].altitude, - northward_velocity: records[0].speed * records[0].bearing.to_radians().cos(), - eastward_velocity: records[0].speed * records[0].bearing.to_radians().sin(), - vertical_velocity: 0.0, - roll: 0.0, - pitch: 0.0, - yaw: records[0].bearing.to_radians(), - in_degrees: true, - is_enu: true, - }; - - // IMU biases (accelerometer + gyroscope) - let imu_biases = vec![0.0; 6]; - - // Initial covariance diagonal (15-state: pos, vel, att, biases) - let mut covariance_diagonal = vec![ - 1e-6, 1e-6, 1.0, // Position uncertainty - 0.1, 0.1, 0.1, // Velocity uncertainty - 1e-4, 1e-4, 1e-4, // Attitude uncertainty - 1e-6, 1e-6, 1e-6, // Accel bias uncertainty - 1e-8, 1e-8, 1e-8, // Gyro bias uncertainty - ]; - // Add geophysical bias uncertainties - covariance_diagonal.extend(vec![1.0; num_geo_states]); - - // Process noise (15-state + geophysical) - use nalgebra::DMatrix; - let mut process_noise_vec = vec![ - 1e-9, 1e-9, 1e-6, // Position process noise - 1e-6, 1e-6, 1e-6, // Velocity process noise - 1e-9, 1e-9, 1e-9, // Attitude process noise - 1e-9, 1e-9, 1e-9, // Accel bias process noise - 1e-9, 1e-9, 1e-9, // Gyro bias process noise - ]; - // Add geophysical process noise - process_noise_vec.extend(vec![1e-9; num_geo_states]); - let process_noise = - DMatrix::from_diagonal(&nalgebra::DVector::from_vec(process_noise_vec)); - - let mut ekf = ExtendedKalmanFilter::new( - initial_state, - imu_biases, - covariance_diagonal, - process_noise, - true, // Use 15-state with biases - ); - - info!( - "Initialized EKF with state dimension {} (base: 15, geo: {})", - ekf.get_estimate().len(), - num_geo_states - ); - - info!("Running EKF geophysical navigation simulation..."); - geo_closed_loop_ekf(&mut ekf, events) - } - FilterType::Eskf => { - error!("ESKF is not yet implemented for geophysical navigation"); - return Err("ESKF is not yet implemented for geophysical navigation".into()); - } - }; - - // Write results - let output_file = output.join(input_file.file_name().ok_or_else(|| { - std::io::Error::new( - std::io::ErrorKind::InvalidInput, - format!("Input file path '{}' has no filename", input_file.display()), - ) - })?); - - 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 geophysical navigation simulation: {}", e); - Err(e.into()) - } - } -} - -/// Execute simulation from a configuration file -fn run_from_config( - config_path: &Path, - cli_parallel: bool, - cli_plot: bool, -) -> Result<(), Box> { - info!("Loading configuration from {}", config_path.display()); - - let mut config = GeonavSimulationConfig::from_file(config_path)?; - - // Override parallel setting if CLI flag is set - if cli_parallel { - config.parallel = true; - } - - // Override plot setting if CLI flag is set - if cli_plot { - config.generate_plot = true; - } - - info!("Configuration loaded successfully"); - info!("Filter: {:?}", config.filter); - info!("Input: {}", config.input); - info!("Output: {}", config.output); - info!("Parallel: {}", config.parallel); - info!("Generate plot: {}", config.generate_plot); - - // Validate paths - let input = Path::new(&config.input); - let output = Path::new(&config.output); - 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 sequentially (parallel not yet implemented for geonav) - let mut failures = 0usize; - for input_file in &csv_files { - if let Err(e) = process_file(input_file, output, &config) { - if !is_multiple { - return Err(e); - } - failures += 1; - error!("Error processing {}: {}", input_file.display(), e); - } - } - if failures > 0 { - error!("{} file(s) failed to process", failures); - } - - Ok(()) -} - -/// Execute closed-loop geophysical navigation simulation -fn run_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { - validate_input_path(&args.sim.input)?; - validate_output_path(&args.sim.output)?; - - let filter_name = match args.filter { - FilterType::Ukf => "Unscented Kalman Filter (UKF)", - FilterType::Ekf => "Extended Kalman Filter (EKF)", - FilterType::Eskf => "Error-State Kalman Filter (ESKF)", - }; - info!( - "Running geophysical navigation in closed-loop mode with {}", - filter_name - ); - - // 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()); - } - - // Build configuration from CLI args - let config = GeonavSimulationConfig { - input: args.sim.input.to_string_lossy().to_string(), - output: args.sim.output.to_string_lossy().to_string(), - filter: args.filter, - seed: args.seed, - parallel: false, - generate_plot: false, - logging: Default::default(), - geophysical: geophysical_args_to_config(&args.geo), - gnss_degradation: strapdown::messages::GnssDegradationConfig { - scheduler: build_scheduler(&args.scheduler), - fault: build_fault(&args.fault), - seed: args.seed, - }, - }; - - // Process each CSV file - for input_file in &csv_files { - match process_file(input_file, &args.sim.output, &config) { - Ok(()) => { - // Success - } - Err(e) => { - error!( - "Error running geophysical navigation on {}: {}", - input_file.display(), - e - ); - if !is_multiple { - return Err(e); - } - error!( - "Error processing {}: {}. Continuing with remaining files...", - input_file.display(), - e - ); - } - } - } - - Ok(()) -} - -/// Read a line from stdin, trimming whitespace and checking for quit command -fn read_user_input() -> Option { - let mut input = String::new(); - io::stdin() - .read_line(&mut input) - .expect("Failed to read line"); - 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()) - } -} - -/// Prompt for configuration name with validation -fn prompt_config_name() -> String { - loop { - println!( - "Please name your configuration file with extension (.toml, .json, .yaml) or 'q' to quit:" - ); - if let Some(input) = read_user_input() { - return input; - } - println!("Error: Configuration path cannot be empty. Please try again.\n"); - } -} - -/// Prompt for configuration file path with validation -fn prompt_config_path() -> String { - loop { - println!("Please specify the output configuration file path (or 'q' to quit):"); - if let Some(input) = read_user_input() { - return input; - } - println!("Error: Configuration path cannot be empty. Please try again.\n"); - } -} - -/// Interactive configuration file creation wizard -fn create_config_file() -> Result<(), Box> { - println!("\n=== Geonav Simulation Configuration Wizard ===\n"); - println!( - "This wizard will help you create a configuration file for geophysical navigation simulations." - ); - println!( - "For now, we'll create a basic template. You can edit it to customize your simulation.\n" - ); - - let config_name = prompt_config_name(); - let save_path = prompt_config_path(); - - println!( - "\nCreating configuration file at: {}/{}\n", - save_path, config_name - ); - - // Create a default configuration - let config = GeonavSimulationConfig::default(); - - // Validate output location exists and write to file - 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 edit the file to customize your simulation settings."); - println!("Then run the simulation with:"); - println!(" geonav-sim --config {}", config_output_path.display()); - - Ok(()) -} - -fn main() -> Result<(), Box> { - let cli = Cli::parse(); - - // If --config is provided, load config - if let Some(ref config_path) = cli.config { - // Load config first to get logging preferences - let config = GeonavSimulationConfig::from_file(config_path)?; - - // Determine log level - let log_level = config.logging.level.as_str(); - - // Create PathBuf from config file string if needed - let config_log_file = config.logging.file.as_ref().map(PathBuf::from); - let log_file = cli.log_file.as_ref().or(config_log_file.as_ref()); - - // Initialize logger with resolved settings - init_logger(log_level, log_file)?; - - 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())?; - - // Execute based on subcommand - match cli.command { - Some(Command::ClosedLoop(args)) => run_closed_loop_cli(&args), - Some(Command::CreateConfig) => create_config_file(), - None => { - eprintln!("Error: No command provided. Use -h or --help for usage information."); - std::process::exit(1); - } - } -} diff --git a/sim/Cargo.toml b/sim/Cargo.toml index d3bfd91..9654fb8 100644 --- a/sim/Cargo.toml +++ b/sim/Cargo.toml @@ -9,10 +9,12 @@ license = "MIT" [features] default = ["plotting"] plotting = ["dep:plotters"] +geonav = ["dep:strapdown-geonav"] [dependencies] clap = { version = "4.5.4", features = ["derive"] } strapdown-core = { path = "../core", features = ["clap"] } +strapdown-geonav = { path = "../geonav", optional = true } log = "0.4" env_logger = "0.11" chrono = "0.4" @@ -20,6 +22,9 @@ nalgebra = "0.34.1" rayon = "1.10" plotters = { version = "0.3.7", optional = true } +[dev-dependencies] +tempfile = "3.15" + [[bin]] name = "strapdown-sim" path = "src/main.rs" diff --git a/sim/src/common.rs b/sim/src/common.rs new file mode 100644 index 0000000..9e56817 --- /dev/null +++ b/sim/src/common.rs @@ -0,0 +1,413 @@ +//! Common utility functions for simulation applications. +//! +//! This module contains shared utilities for CLI applications including: +//! - Logger initialization +//! - Path validation and file discovery +//! - User input prompts + +use log::error; +use rayon::prelude::*; +use std::error::Error; +use std::io; +use std::path::{Path, PathBuf}; +use std::sync::Mutex; + +/// Initialize the logger with the specified configuration. +/// +/// # Arguments +/// * `log_level` - Log level string (off, error, warn, info, debug, trace) +/// * `log_file` - Optional path to log file (logs to stderr if None) +/// +/// # Errors +/// Returns an error if the log file cannot be opened or logger initialization fails. +pub fn init_logger(log_level: &str, log_file: Option<&PathBuf>) -> Result<(), Box> { + use std::io::Write; + + let level = log_level.parse::().unwrap_or_else(|_| { + eprintln!("Invalid log level '{}', defaulting to 'info'", log_level); + log::LevelFilter::Info + }); + + let mut builder = env_logger::Builder::new(); + builder.filter_level(level); + builder.format(|buf, record| { + writeln!( + buf, + "{} [{}] - {}", + chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.3f"), + record.level(), + record.args() + ) + }); + + if let Some(log_path) = log_file { + let target = Box::new( + std::fs::OpenOptions::new() + .create(true) + .append(true) + .open(log_path)?, + ); + builder.target(env_logger::Target::Pipe(target)); + } + + builder.try_init()?; + Ok(()) +} + +/// Validate input path exists and is either a file or directory. +/// +/// # Arguments +/// * `input` - Path to validate +/// +/// # Errors +/// Returns an error if the path does not exist or is neither a file nor directory. +pub fn validate_input_path(input: &Path) -> Result<(), Box> { + if !input.exists() { + return Err(format!("Input path '{}' does not exist.", input.display()).into()); + } + if !input.is_file() && !input.is_dir() { + return Err(format!( + "Input path '{}' is neither a file nor a directory.", + input.display() + ) + .into()); + } + Ok(()) +} + +/// Get all CSV files from a path (either single file or all CSVs in directory). +/// +/// # Arguments +/// * `input` - Path to a CSV file or directory containing CSV files +/// +/// # Returns +/// A sorted vector of PathBuf for each CSV file found. +/// +/// # Errors +/// Returns an error if: +/// - The input file is not a CSV +/// - No CSV files are found in the directory +/// - The path is neither a file nor directory +pub fn get_csv_files(input: &Path) -> Result, Box> { + if input.is_file() { + if input.extension().and_then(|s| s.to_str()) != Some("csv") { + return Err(format!("Input file '{}' is not a CSV file.", input.display()).into()); + } + Ok(vec![input.to_path_buf()]) + } else if input.is_dir() { + let mut csv_files: Vec = std::fs::read_dir(input)? + .filter_map(|entry| entry.ok()) + .map(|entry| entry.path()) + .filter(|path| { + path.is_file() && path.extension().and_then(|s| s.to_str()) == Some("csv") + }) + .collect(); + + if csv_files.is_empty() { + return Err(format!("No CSV files found in directory '{}'.", input.display()).into()); + } + + // Sort for consistent ordering + csv_files.sort(); + Ok(csv_files) + } else { + Err(format!( + "Input path '{}' is neither a file nor a directory.", + input.display() + ) + .into()) + } +} + +/// Validate output path and create parent directories if needed. +/// +/// If the output path does not exist, it will be created as a directory. +/// +/// # Arguments +/// * `output` - Path to validate/create +/// +/// # Errors +/// Returns an error if directory creation fails. +pub fn validate_output_path(output: &Path) -> Result<(), Box> { + if !output.exists() { + std::fs::create_dir_all(output)?; + } + Ok(()) +} + +/// Process multiple files with optional parallel execution. +/// +/// # Arguments +/// * `csv_files` - Vector of CSV files to process +/// * `parallel` - Whether to process files in parallel +/// * `processor` - Function to process each file, returns Result +/// +/// # Errors +/// Returns an error if any files fail to process (after all attempts). +pub fn process_files( + csv_files: &[PathBuf], + parallel: bool, + processor: F, +) -> Result<(), Box> +where + F: Fn(&Path) -> Result<(), Box> + Send + Sync, +{ + let is_multiple = csv_files.len() > 1; + + if parallel && is_multiple { + // Parallel processing + let errors = Mutex::new(Vec::new()); + + csv_files.par_iter().for_each(|input_file| { + if let Err(e) = processor(input_file) { + error!("Error processing {}: {}", input_file.display(), e); + errors + .lock() + .expect("Failed to acquire lock on error collection") + .push((input_file.clone(), e.to_string())); + } + }); + + let errors = errors + .into_inner() + .expect("Failed to extract errors from mutex"); + 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) = processor(input_file) { + if !is_multiple { + return Err(e); + } + failures += 1; + error!("Error processing {}: {}", input_file.display(), e); + } + } + if failures > 0 { + error!("{} file(s) failed to process", failures); + } + } + + Ok(()) +} + +// ============================================================================ +// User Input Utilities +// ============================================================================ + +/// 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 +/// +/// # Panics +/// Exits the process if user enters 'q' or 'Q'. +pub fn read_user_input() -> Option { + let mut input = String::new(); + io::stdin() + .read_line(&mut input) + .expect("Failed to read line"); + 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()) + } +} + +/// Prompt for configuration name with validation. +/// +/// Continues prompting until a non-empty name is provided. +/// +/// # Returns +/// The configuration filename entered by the user. +pub fn prompt_config_name() -> String { + loop { + println!( + "Please name your configuration file with extension (.toml, .json, .yaml) or 'q' to quit:" + ); + if let Some(input) = read_user_input() { + return input; + } + println!("Error: Configuration path cannot be empty. Please try again.\n"); + } +} + +/// Prompt for configuration file path with validation. +/// +/// Continues prompting until a non-empty path is provided. +/// +/// # Returns +/// The configuration file path entered by the user. +pub fn prompt_config_path() -> String { + loop { + println!("Please specify the output configuration file path (or 'q' to quit):"); + if let Some(input) = read_user_input() { + return input; + } + println!("Error: Configuration path cannot be empty. Please try again.\n"); + } +} + +/// Prompt for input CSV file or directory path with validation. +/// +/// # Returns +/// The input path entered by the user. +pub fn prompt_input_path() -> String { + loop { + println!( + "Please specify the input location, either a single CSV file or a directory containing them. ('q' to quit):" + ); + if let Some(input) = read_user_input() { + return input; + } + println!("Error: Input path cannot be empty. Please try again.\n"); + } +} + +/// Prompt for output CSV file path with validation. +/// +/// # Returns +/// The output path entered by the user. +pub fn prompt_output_path() -> String { + loop { + println!("Please specify the output location to save output data. ('q' to quit):"); + if let Some(input) = read_user_input() { + return input; + } + println!("Error: Output path cannot be empty. Please try again.\n"); + } +} + +/// Helper function to prompt for f64 with default value and range validation. +/// +/// # Arguments +/// * `prompt_text` - Text to display to the user +/// * `default` - Default value if user presses Enter +/// * `min_val` - Minimum acceptable value +/// * `max_val` - Maximum acceptable value +/// +/// # Returns +/// The validated f64 value. +pub fn prompt_f64_with_default(prompt_text: &str, default: f64, min_val: f64, max_val: f64) -> f64 { + loop { + println!( + "{} (press Enter for {}, or 'q' to quit):", + prompt_text, default + ); + match read_user_input() { + None => return default, + Some(input) => match input.parse::() { + Ok(val) if val >= min_val && val <= max_val => return val, + Ok(_) => println!( + "Error: Value must be between {} and {}.\n", + min_val, max_val + ), + Err(_) => println!("Error: Please enter a valid number.\n"), + }, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use std::fs::File; + use tempfile::tempdir; + + #[test] + fn test_validate_input_path_file() { + let dir = tempdir().unwrap(); + let file_path = dir.path().join("test.csv"); + File::create(&file_path).unwrap(); + + assert!(validate_input_path(&file_path).is_ok()); + } + + #[test] + fn test_validate_input_path_directory() { + let dir = tempdir().unwrap(); + assert!(validate_input_path(dir.path()).is_ok()); + } + + #[test] + fn test_validate_input_path_nonexistent() { + let result = validate_input_path(Path::new("/nonexistent/path")); + assert!(result.is_err()); + } + + #[test] + fn test_get_csv_files_single_file() { + let dir = tempdir().unwrap(); + let file_path = dir.path().join("test.csv"); + File::create(&file_path).unwrap(); + + let result = get_csv_files(&file_path).unwrap(); + assert_eq!(result.len(), 1); + assert_eq!(result[0], file_path); + } + + #[test] + fn test_get_csv_files_directory() { + let dir = tempdir().unwrap(); + + // Create multiple CSV files + File::create(dir.path().join("a.csv")).unwrap(); + File::create(dir.path().join("b.csv")).unwrap(); + File::create(dir.path().join("c.txt")).unwrap(); // Non-CSV should be ignored + + let result = get_csv_files(dir.path()).unwrap(); + assert_eq!(result.len(), 2); + // Should be sorted + assert!( + result[0].file_name().unwrap().to_str().unwrap() + < result[1].file_name().unwrap().to_str().unwrap() + ); + } + + #[test] + fn test_get_csv_files_non_csv() { + let dir = tempdir().unwrap(); + let file_path = dir.path().join("test.txt"); + File::create(&file_path).unwrap(); + + let result = get_csv_files(&file_path); + assert!(result.is_err()); + } + + #[test] + fn test_get_csv_files_empty_directory() { + let dir = tempdir().unwrap(); + let result = get_csv_files(dir.path()); + assert!(result.is_err()); + } + + #[test] + fn test_validate_output_path_creates_directory() { + let dir = tempdir().unwrap(); + let new_dir = dir.path().join("new_output_dir"); + + assert!(!new_dir.exists()); + validate_output_path(&new_dir).unwrap(); + assert!(new_dir.exists()); + } + + #[test] + fn test_validate_output_path_existing() { + let dir = tempdir().unwrap(); + assert!(validate_output_path(dir.path()).is_ok()); + } +} diff --git a/sim/src/main.rs b/sim/src/main.rs index a1d547b..3e697c3 100644 --- a/sim/src/main.rs +++ b/sim/src/main.rs @@ -18,17 +18,37 @@ //! //! For dataset format details, see the documentation or use --help with specific subcommands. +mod common; #[cfg(feature = "plotting")] mod plotting; use clap::{Args, Parser, Subcommand}; +use common::{ + get_csv_files, init_logger, prompt_config_name, prompt_config_path, prompt_f64_with_default, + prompt_input_path, prompt_output_path, read_user_input, validate_input_path, + validate_output_path, +}; use log::{error, info}; use rayon::prelude::*; use std::error::Error; -use std::io; use std::path::{Path, PathBuf}; use std::sync::Mutex; use strapdown::messages::{GnssScheduler, build_event_stream}; + +// Geophysical navigation imports (feature-gated) +#[cfg(feature = "geonav")] +use geonav::{ + GeoMap, GeophysicalMeasurementType, GravityResolution, MagneticResolution, + build_event_stream as geo_build_event_stream, geo_closed_loop_ekf, geo_closed_loop_ukf, +}; +#[cfg(feature = "geonav")] +use std::rc::Rc; +#[cfg(feature = "geonav")] +use strapdown::NavigationFilter; +#[cfg(feature = "geonav")] +use strapdown::kalman::{ExtendedKalmanFilter, InitialState}; +#[cfg(feature = "geonav")] +use strapdown::sim::{DEFAULT_PROCESS_NOISE, GeoResolution}; use strapdown::sim::{ FaultArgs, FilterType, NavigationResult, ParticleFilterType, SchedulerArgs, SimulationConfig, SimulationMode, TestDataRecord, build_fault, build_scheduler, dead_reckoning, initialize_ekf, @@ -133,6 +153,56 @@ struct SimArgs { output: PathBuf, } +/// Geophysical measurement arguments (feature-gated) +#[cfg(feature = "geonav")] +#[derive(Args, Clone, Debug)] +struct GeophysicalArgs { + /// Enable geophysical navigation + #[arg(long)] + geo: bool, + + /// Gravity map resolution + #[arg(long, value_enum, requires = "geo")] + gravity_resolution: Option, + + /// Gravity measurement bias (mGal) + #[arg(long, requires = "geo")] + gravity_bias: Option, + + /// Gravity measurement noise std dev (mGal) + #[arg(long, default_value_t = 100.0, requires = "geo")] + gravity_noise_std: f64, + + /// Gravity map file path + #[arg(long, requires = "geo")] + gravity_map_file: Option, + + /// Magnetic map resolution + #[arg(long, value_enum, requires = "geo")] + magnetic_resolution: Option, + + /// Magnetic measurement bias (nT) + #[arg(long, requires = "geo")] + magnetic_bias: Option, + + /// Magnetic measurement noise std dev (nT) + #[arg(long, default_value_t = 150.0, requires = "geo")] + magnetic_noise_std: f64, + + /// Magnetic map file path + #[arg(long, requires = "geo")] + magnetic_map_file: Option, + + /// Geophysical measurement frequency (seconds) + #[arg(long, requires = "geo")] + geo_frequency_s: Option, +} + +/// Empty stub when geonav feature is disabled +#[cfg(not(feature = "geonav"))] +#[derive(Args, Clone, Debug, Default)] +struct GeophysicalArgs {} + /// Closed-loop simulation arguments #[derive(Args, Clone, Debug)] struct ClosedLoopSimArgs { @@ -155,6 +225,10 @@ struct ClosedLoopSimArgs { /// Fault model settings (corrupt measurement content) #[command(flatten)] fault: FaultArgs, + + /// Geophysical navigation options (optional, requires --features geonav) + #[command(flatten)] + geo: GeophysicalArgs, } /// Particle filter simulation arguments @@ -222,99 +296,6 @@ struct CreateConfigArgs { mode: SimulationMode, } -/// Initialize the logger with the specified configuration -fn init_logger(log_level: &str, log_file: Option<&PathBuf>) -> Result<(), Box> { - use std::io::Write; - - let level = log_level.parse::().unwrap_or_else(|_| { - eprintln!("Invalid log level '{}', defaulting to 'info'", log_level); - log::LevelFilter::Info - }); - - let mut builder = env_logger::Builder::new(); - builder.filter_level(level); - builder.format(|buf, record| { - writeln!( - buf, - "{} [{}] - {}", - chrono::Local::now().format("%Y-%m-%d %H:%M:%S%.3f"), - record.level(), - record.args() - ) - }); - - if let Some(log_path) = log_file { - let target = Box::new( - std::fs::OpenOptions::new() - .create(true) - .append(true) - .open(log_path)?, - ); - builder.target(env_logger::Target::Pipe(target)); - } - - builder.try_init()?; - Ok(()) -} - -/// Validate input path exists and is either a file or directory -fn validate_input_path(input: &Path) -> Result<(), Box> { - if !input.exists() { - return Err(format!("Input path '{}' does not exist.", input.display()).into()); - } - if !input.is_file() && !input.is_dir() { - return Err(format!( - "Input path '{}' is neither a file nor a directory.", - input.display() - ) - .into()); - } - Ok(()) -} - -/// Get all CSV files from a path (either single file or all CSVs in directory) -fn get_csv_files(input: &Path) -> Result, Box> { - if input.is_file() { - if input.extension().and_then(|s| s.to_str()) != Some("csv") { - return Err(format!("Input file '{}' is not a CSV file.", input.display()).into()); - } - Ok(vec![input.to_path_buf()]) - } else if input.is_dir() { - let mut csv_files: Vec = std::fs::read_dir(input)? - .filter_map(|entry| entry.ok()) - .map(|entry| entry.path()) - .filter(|path| { - path.is_file() && path.extension().and_then(|s| s.to_str()) == Some("csv") - }) - .collect(); - - if csv_files.is_empty() { - return Err(format!("No CSV files found in directory '{}'.", input.display()).into()); - } - - // Sort for consistent ordering - csv_files.sort(); - Ok(csv_files) - } else { - Err(format!( - "Input path '{}' is neither a file nor a directory.", - input.display() - ) - .into()) - } -} - -/// Validate output path and create parent directories if needed. -/// If output ends with `.csv`, treat as a single file output and ensure its parent exists. -/// Otherwise, treat as a directory and create it if needed. -fn validate_output_path(output: &Path) -> Result<(), Box> { - // Output is a directory, create it if it doesn't exist - if !output.exists() { - std::fs::create_dir_all(output)?; - } - Ok(()) -} - /// Process a single CSV file with the given configuration fn process_file( input_file: &Path, @@ -654,6 +635,12 @@ fn run_open_loop(args: &SimArgs) -> Result<(), Box> { /// Execute closed-loop simulation fn run_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { + // Check if geophysical navigation is enabled + #[cfg(feature = "geonav")] + if args.geo.geo { + return run_geo_closed_loop_cli(args); + } + validate_input_path(&args.sim.input)?; validate_output_path(&args.sim.output)?; @@ -727,11 +714,111 @@ fn run_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { Ok(()) } -/// Execute particle filter simulation -fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box> { +// ============================================================================ +// Geophysical Navigation Functions (feature-gated) +// ============================================================================ + +/// Convert GeoResolution to GravityResolution +#[cfg(feature = "geonav")] +fn convert_resolution_gravity(resolution: GeoResolution) -> GravityResolution { + match resolution { + GeoResolution::OneDegree => GravityResolution::OneDegree, + GeoResolution::ThirtyMinutes => GravityResolution::ThirtyMinutes, + GeoResolution::TwentyMinutes => GravityResolution::TwentyMinutes, + GeoResolution::FifteenMinutes => GravityResolution::FifteenMinutes, + GeoResolution::TenMinutes => GravityResolution::TenMinutes, + GeoResolution::SixMinutes => GravityResolution::SixMinutes, + GeoResolution::FiveMinutes => GravityResolution::FiveMinutes, + GeoResolution::FourMinutes => GravityResolution::FourMinutes, + GeoResolution::ThreeMinutes => GravityResolution::ThreeMinutes, + GeoResolution::TwoMinutes => GravityResolution::TwoMinutes, + GeoResolution::OneMinute => GravityResolution::OneMinute, + _ => GravityResolution::OneMinute, + } +} + +/// Convert GeoResolution to MagneticResolution +#[cfg(feature = "geonav")] +fn convert_resolution_magnetic(resolution: GeoResolution) -> MagneticResolution { + match resolution { + GeoResolution::OneDegree => MagneticResolution::OneDegree, + GeoResolution::ThirtyMinutes => MagneticResolution::ThirtyMinutes, + GeoResolution::TwentyMinutes => MagneticResolution::TwentyMinutes, + GeoResolution::FifteenMinutes => MagneticResolution::FifteenMinutes, + GeoResolution::TenMinutes => MagneticResolution::TenMinutes, + GeoResolution::SixMinutes => MagneticResolution::SixMinutes, + GeoResolution::FiveMinutes => MagneticResolution::FiveMinutes, + GeoResolution::FourMinutes => MagneticResolution::FourMinutes, + GeoResolution::ThreeMinutes => MagneticResolution::ThreeMinutes, + GeoResolution::TwoMinutes => MagneticResolution::TwoMinutes, + _ => MagneticResolution::TwoMinutes, + } +} + +/// Auto-detect gravity map file based on input directory +#[cfg(feature = "geonav")] +fn find_gravity_map(input_path: &Path) -> Result> { + let input_dir = input_path + .parent() + .ok_or("Cannot determine input directory")?; + + let input_stem = input_path + .file_stem() + .ok_or("Cannot determine input file stem")? + .to_string_lossy(); + + let map_file = input_dir.join(format!("{}_gravity.nc", input_stem)); + + if map_file.exists() { + Ok(map_file) + } else { + Err(format!("Gravity map file not found: {}", map_file.display()).into()) + } +} + +/// Auto-detect magnetic map file based on input directory +#[cfg(feature = "geonav")] +fn find_magnetic_map(input_path: &Path) -> Result> { + let input_dir = input_path + .parent() + .ok_or("Cannot determine input directory")?; + + let input_stem = input_path + .file_stem() + .ok_or("Cannot determine input file stem")? + .to_string_lossy(); + + let map_file = input_dir.join(format!("{}_magnetic.nc", input_stem)); + + if map_file.exists() { + Ok(map_file) + } else { + Err(format!("Magnetic map file not found: {}", map_file.display()).into()) + } +} + +/// Execute geophysical closed-loop simulation +#[cfg(feature = "geonav")] +fn run_geo_closed_loop_cli(args: &ClosedLoopSimArgs) -> Result<(), Box> { validate_input_path(&args.sim.input)?; validate_output_path(&args.sim.output)?; + let filter_name = match args.filter { + FilterType::Ukf => "Unscented Kalman Filter (UKF)", + FilterType::Ekf => "Extended Kalman Filter (EKF)", + FilterType::Eskf => "Error-State Kalman Filter (ESKF)", + }; + info!( + "Running geophysical navigation in closed-loop mode with {}", + filter_name + ); + + // Validate that at least one geophysical map is configured + if args.geo.gravity_resolution.is_none() && args.geo.magnetic_resolution.is_none() { + return Err("At least one of --gravity-resolution or --magnetic-resolution must be specified when using --geo".into()); + } + + // Get all CSV files to process let csv_files = get_csv_files(&args.sim.input)?; let is_multiple = csv_files.len() > 1; @@ -739,87 +826,250 @@ fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box path.clone(), + None => find_gravity_map(input_file)?, + }; - Ok(()) -} + info!("Loading gravity map from: {}", map_path.display()); + let measurement_type = + 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", + map.get_lats().len(), + map.get_lons().len() + ); + Some(map) + } else { + None + }; -/// Read a line from stdin, trimming whitespace and checking for quit command -/// Returns None if user enters 'q', otherwise returns the trimmed input -fn read_user_input() -> Option { - let mut input = String::new(); - io::stdin() - .read_line(&mut input) - .expect("Failed to read line"); - let input = input.trim(); - - if input.eq_ignore_ascii_case("q") { - std::process::exit(0); - } + // Load magnetic map if configured + let magnetic_map = if let Some(res) = args.geo.magnetic_resolution { + let map_path = match &args.geo.magnetic_map_file { + Some(path) => path.clone(), + None => find_magnetic_map(input_file)?, + }; - if input.is_empty() { - None - } else { - Some(input.to_string()) - } -} + info!("Loading magnetic map from: {}", map_path.display()); + let measurement_type = + 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", + map.get_lats().len(), + map.get_lons().len() + ); + Some(map) + } else { + None + }; -/// Prompt for configuration name with validation -fn prompt_config_name() -> String { - loop { - println!( - "Please name your configuration file with extension (.toml, .json, .yaml) or 'q' to quit:" + // Build GNSS degradation config from CLI args + let gnss_degradation = strapdown::messages::GnssDegradationConfig { + scheduler: build_scheduler(&args.scheduler), + fault: build_fault(&args.fault), + seed: args.seed, + }; + + // Build event stream with geophysical measurements + let events = geo_build_event_stream( + &records, + &gnss_degradation, + gravity_map.clone(), + if gravity_map.is_some() { + Some(args.geo.gravity_noise_std) + } else { + None + }, + magnetic_map.clone(), + if magnetic_map.is_some() { + Some(args.geo.magnetic_noise_std) + } else { + None + }, + args.geo.geo_frequency_s, ); - if let Some(input) = read_user_input() { - return input; - } - println!("Error: Configuration path cannot be empty. Please try again.\n"); - } -} -/// Prompt for configuration file path with validation -fn prompt_config_path() -> String { - loop { - println!("Please specify the output configuration file path (or 'q' to quit):"); - if let Some(input) = read_user_input() { - return input; + info!("Built event stream with {} events", events.events.len()); + + // Determine number of geophysical states + let num_geo_states = gravity_map.is_some() as usize + magnetic_map.is_some() as usize; + + // Run simulation based on filter type + let results = match args.filter { + FilterType::Ukf => { + info!("Initializing UKF..."); + let mut process_noise: Vec = DEFAULT_PROCESS_NOISE.into(); + process_noise.extend(vec![1e-9; num_geo_states]); + + let mut geo_biases = Vec::new(); + let mut geo_noise_stds = Vec::new(); + + if gravity_map.is_some() { + geo_biases.push(args.geo.gravity_bias.unwrap_or(0.0)); + geo_noise_stds.push(args.geo.gravity_noise_std); + } + if magnetic_map.is_some() { + geo_biases.push(args.geo.magnetic_bias.unwrap_or(0.0)); + geo_noise_stds.push(args.geo.magnetic_noise_std); + } + + let mut ukf = initialize_ukf( + records[0].clone(), + None, + None, + None, + Some(geo_biases), + Some(geo_noise_stds), + Some(process_noise), + ); + info!( + "Initialized UKF with state dimension {} (base: 9, geo: {})", + ukf.get_estimate().len(), + num_geo_states + ); + + info!("Running UKF geophysical navigation simulation..."); + geo_closed_loop_ukf(&mut ukf, events) + } + FilterType::Ekf => { + info!("Initializing EKF..."); + + let initial_state = InitialState { + latitude: records[0].latitude, + longitude: records[0].longitude, + altitude: records[0].altitude, + northward_velocity: records[0].speed * records[0].bearing.to_radians().cos(), + eastward_velocity: records[0].speed * records[0].bearing.to_radians().sin(), + vertical_velocity: 0.0, + roll: 0.0, + pitch: 0.0, + yaw: records[0].bearing.to_radians(), + in_degrees: true, + is_enu: true, + }; + + let imu_biases = vec![0.0; 6]; + + let mut covariance_diagonal = vec![ + 1e-6, 1e-6, 1.0, // Position uncertainty + 0.1, 0.1, 0.1, // Velocity uncertainty + 1e-4, 1e-4, 1e-4, // Attitude uncertainty + 1e-6, 1e-6, 1e-6, // Accel bias uncertainty + 1e-8, 1e-8, 1e-8, // Gyro bias uncertainty + ]; + covariance_diagonal.extend(vec![1.0; num_geo_states]); + + use nalgebra::DMatrix; + let mut process_noise_vec = vec![ + 1e-9, 1e-9, 1e-6, // Position process noise + 1e-6, 1e-6, 1e-6, // Velocity process noise + 1e-9, 1e-9, 1e-9, // Attitude process noise + 1e-9, 1e-9, 1e-9, // Accel bias process noise + 1e-9, 1e-9, 1e-9, // Gyro bias process noise + ]; + process_noise_vec.extend(vec![1e-9; num_geo_states]); + let process_noise = + DMatrix::from_diagonal(&nalgebra::DVector::from_vec(process_noise_vec)); + + let mut ekf = ExtendedKalmanFilter::new( + initial_state, + imu_biases, + covariance_diagonal, + process_noise, + true, + ); + + info!( + "Initialized EKF with state dimension {} (base: 15, geo: {})", + ekf.get_estimate().len(), + num_geo_states + ); + + info!("Running EKF geophysical navigation simulation..."); + geo_closed_loop_ekf(&mut ekf, events) + } + FilterType::Eskf => { + error!("ESKF is not yet implemented for geophysical navigation"); + return Err("ESKF is not yet implemented for geophysical navigation".into()); + } + }; + + // Write results + let output_file = args.sim.output.join(input_file.file_name().ok_or_else(|| { + std::io::Error::new( + std::io::ErrorKind::InvalidInput, + format!("Input file path '{}' has no filename", input_file.display()), + ) + })?); + + match results { + Ok(ref nav_results) => { + NavigationResult::to_csv(nav_results, &output_file)?; + info!("Results written to {}", output_file.display()); + } + Err(e) => { + error!( + "Error running geophysical navigation on {}: {}", + input_file.display(), + e + ); + if !is_multiple { + return Err(e.into()); + } + error!( + "Error processing {}: {}. Continuing with remaining files...", + input_file.display(), + e + ); + } } - println!("Error: Configuration path cannot be empty. Please try again.\n"); } + + Ok(()) } -/// Prompt for input CSV file or directory path with validation -fn prompt_input_path() -> String { - loop { - println!( - "Please specify the input location, either a single CSV file or a directory containing them. ('q' to quit):" - ); - if let Some(input) = read_user_input() { - return input; - } - println!("Error: Input path cannot be empty. Please try again.\n"); +/// Execute particle filter simulation +fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box> { + 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; + + if is_multiple { + info!("Processing {} CSV files from directory", csv_files.len()); } -} -/// Prompt for output CSV file path with validation -fn prompt_output_path() -> String { - loop { - println!("Please specify the output location to save output data. ('q' to quit):"); - if let Some(input) = read_user_input() { - return input; - } - println!("Error: Output path cannot be empty. Please try again.\n"); + for input_file in &csv_files { + info!("Processing file: {}", input_file.display()); + + // TODO: Implement particle filter processing here + // let records = TestDataRecord::from_csv(input_file)?; + // let output_file = generate_output_path(&args.sim.output, input_file, is_multiple); + // ... process and write results ... } + + info!("Particle filter mode is not yet fully implemented"); + println!("Particle filter mode is not yet fully implemented"); + // Note: Implementation would go here once particle filter is ready + + Ok(()) } /// Prompt for simulation mode with validation @@ -1056,27 +1306,6 @@ fn prompt_hijack_fault_model() -> strapdown::messages::GnssFaultModel { } } -/// Helper function to prompt for f64 with default value and range validation -fn prompt_f64_with_default(prompt_text: &str, default: f64, min_val: f64, max_val: f64) -> f64 { - loop { - println!( - "{} (press Enter for {}, or 'q' to quit):", - prompt_text, default - ); - match read_user_input() { - None => return default, - Some(input) => match input.parse::() { - Ok(val) if val >= min_val && val <= max_val => return val, - Ok(_) => println!( - "Error: Value must be between {} and {}.\n", - min_val, max_val - ), - Err(_) => println!("Error: Please enter a valid number.\n"), - }, - } - } -} - /// Prompt for parallel execution preference fn prompt_parallel() -> bool { loop {