From 53157279b2c260b6add63fff2fc20453a1f41f54 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Apr 2026 10:33:34 -0500 Subject: [PATCH 1/7] Implement ESO gain optimization and markdown report generation - Add src/eso.rs: 2nd-order LESO bandwidth optimization via golden-section search - Add src/report.rs: Per-axis signal statistics and ESO results in markdown format - Add CLI flags: --eso, --eso-axis, --eso-b0, --report - Add ESO constants to src/constants.rs (omega_0 bounds, GSS tolerance) - Update lib.rs and main.rs to wire modules and implement ESO/report execution - Update OVERVIEW.md with ESO and report sections - Add INFORMATION/ESO_HOWTO.md user guide - Tested on Betaflight 2025.12 and EmuFlight 0.4.3 full flights --- OVERVIEW.md | 32 ++++++- src/constants.rs | 7 ++ src/eso.rs | 220 +++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 2 + src/main.rs | 147 ++++++++++++++++++++++++++++++ src/report.rs | 230 +++++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 637 insertions(+), 1 deletion(-) create mode 100644 src/eso.rs create mode 100644 src/report.rs diff --git a/OVERVIEW.md b/OVERVIEW.md index 6d64b2ea..f7e37919 100644 --- a/OVERVIEW.md +++ b/OVERVIEW.md @@ -9,6 +9,8 @@ - [Implementation Details](#implementation-details) - [Filter Response Curves](#filter-response-curves) - [Bode Plot Analysis (Optional)](#bode-plot-analysis-optional) + - [ESO Gain Optimization (Optional)](#eso-gain-optimization-optional) + - [Statistical Report Output (Optional)](#statistical-report-output-optional) - [Step-Response Comparison with Other Analysis Tools](#step-response-comparison-with-other-analysis-tools) - [Compared to PIDtoolbox/Matlab (PTstepcalc.m)](#compared-to-pidtoolboxmatlab-ptstepcalcm) - [Compared to PlasmaTree/Python (PID-Analyzer.py)](#compared-to-plasmatreepython-pid-analyzerpy) @@ -26,6 +28,8 @@ All analysis parameters, thresholds, plot dimensions, and algorithmic constants * Additional options include `--help` and `--version` for user assistance. * The `--output-dir` parameter now requires a directory path when specified. If omitted, plots are saved in the source folder (input file's directory). * Handles multiple input files and determines if a directory prefix should be added to output filenames to avoid collisions when processing files from different directories. + * **ESO flags:** `--eso` enables 2nd-order LESO bandwidth optimization; `--eso-axis ` selects axes (default: all); `--eso-b0 ` sets control effectiveness (default: 1.0). + * **Report flag:** `--report` writes a markdown statistical report (`_report.md`) alongside plot outputs. 2. **File Processing (`src/main.rs:process_file`):** * For each input CSV: @@ -157,7 +161,32 @@ All analysis parameters, thresholds, plot dimensions, and algorithmic constants * **Limitations:** Normal operational flight logs produce low coherence due to nonlinearities, closed-loop feedback, and nonstationary maneuvers. Results in such cases are unreliable and not recommended for tuning decisions. * **Warning:** A runtime warning is displayed when `--bode` is used to inform users of these requirements and recommend spectrum analysis for normal flights. -### Output and Tuning Recommendations +### ESO Gain Optimization (Optional) + +* **Purpose:** Offline system identification of 2nd-order LESO (Linear Extended State Observer) bandwidth (omega_0) from recorded flight data. Finds observer gains that minimise tracking error against measured gyro rate. +* **Activation:** Disabled by default; enable with `--eso`. Optionally restrict axes with `--eso-axis roll,pitch,yaw` and set control effectiveness with `--eso-b0 `. +* **Algorithm (`src/eso.rs`):** + * Extracts filtered gyro (omega) and PID sum (P+I+D+F) per axis as measured output and control input respectively. + * Simulates a discrete Euler-forward 2nd-order LESO at each candidate omega_0: + * `e = omega_meas[k] - omega_hat` + * `omega_hat += Ts * (f_hat + b0 * u[k] + beta1 * e)` + * `f_hat += Ts * (beta2 * e)` + * Bandwidth parameterisation (Gao 2003): `beta1 = 2*omega_0`, `beta2 = omega_0^2`. + * Minimises MSE(omega_hat, omega_meas) via golden-section search over `[ESO_OMEGA0_MIN, min(sample_rate/3, ESO_OMEGA0_MAX)]`. +* **Stability constraint:** omega_0 < sample_rate / 3 (enforced automatically). +* **Output:** Prints optimal omega_0, beta1, beta2, and MSE per axis to console. Results are also included in the markdown report if `--report` is also given. +* **Limitations:** `b0=1.0` (default) is dimensionless. For absolute accuracy co-tune b0 using known frame inertia. The cost function is MSE on the closed-loop observer output; unimodality is assumed over the search range. + +### Statistical Report Output (Optional) + +* **Purpose:** Produces a markdown file summarising per-axis signal statistics and optional ESO results alongside plot outputs. +* **Activation:** Enable with `--report`. File is written as `_report.md` in the output directory. +* **Content (`src/report.rs`):** + * **Metadata:** Sample rate, total row count, flight duration, selected firmware/configuration keys. + * **Per-axis tables:** Mean, std dev, min, max, RMS, sample count for gyro (filtered), setpoint, PID sum, P-term, I-term, and D-term. + * **ESO table (when `--eso` is also active):** omega_0, beta1, beta2, b0, MSE, and sample count per axis. + + #### Generated PNG Plots @@ -175,6 +204,7 @@ When `--step` flag is not used, all plots below are generated: - **`*_Gyro_PSD_Spectrogram_comparative.png`** — Gyro spectrogram (PSD vs. time) using Short-Time Fourier Transform - **`*_Throttle_Freq_Heatmap_comparative.png`** — System noise characteristics across throttle levels and frequencies - **`*_PID_Activity_stacked.png`** — P, I, D term activity over time for each axis (Roll, Pitch, Yaw). Displays all three PID components on the same time-domain plot with unified Y-axis scaling for visual comparison. Each term shows min/avg/max statistics in the legend. Useful for visualizing PID contribution balance during flight and identifying control issues (persistent P-term offset, I-term wind direction, D-term phase lag). +- **`*_report.md`** — Markdown statistical report (requires `--report`). Per-axis signal statistics table and, when combined with `--eso`, the optimised LESO gains. #### P:D Ratio Recommendations diff --git a/src/constants.rs b/src/constants.rs index 6f1f041d..9f18cc26 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -259,4 +259,11 @@ pub const PSD_EPSILON: f64 = 1e-12; // Guard against division by zero for PSD va pub const MAGNITUDE_PLOT_MARGIN_DB: f64 = 10.0; // Padding above/below magnitude data for plot range pub const PHASE_PLOT_MARGIN_DEG: f64 = 30.0; // Padding above/below phase data for plot range +// ESO (Extended State Observer) optimization constants +pub const ESO_OMEGA0_MIN: f64 = 50.0; // Lower bound for observer bandwidth search (rad/s) +pub const ESO_OMEGA0_MAX: f64 = 500.0; // Upper bound for observer bandwidth search (rad/s) +pub const ESO_GSS_TOLERANCE: f64 = 0.01; // Golden-section search convergence tolerance (rad/s) +pub const ESO_GSS_MAX_ITER: usize = 100; // Maximum iterations for golden-section search +pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimensionless) + // src/constants.rs diff --git a/src/eso.rs b/src/eso.rs new file mode 100644 index 00000000..5eee45b5 --- /dev/null +++ b/src/eso.rs @@ -0,0 +1,220 @@ +// src/eso.rs +// 2nd-order Linear Extended State Observer (LESO) for flight controller blackbox data. +// Implements discrete Euler-forward LESO simulation with golden-section search for the +// optimal observer bandwidth (omega_0) using PID sum as control input and filtered +// gyro as measured output. + +use std::error::Error; + +use crate::constants::{ + ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, +}; +use crate::data_input::log_data::LogRowData; + +/// Configuration for a single-axis ESO optimization run. +#[derive(Debug, Clone)] +pub struct EsoConfig { + /// Control effectiveness (scales PID sum to angular acceleration). Default: 1.0. + pub b0: f64, + /// Observer bandwidth search lower bound (rad/s). + pub omega0_min: f64, + /// Observer bandwidth search upper bound (rad/s). + pub omega0_max: f64, +} + +impl Default for EsoConfig { + fn default() -> Self { + Self { + b0: ESO_DEFAULT_B0, + omega0_min: ESO_OMEGA0_MIN, + omega0_max: ESO_OMEGA0_MAX, + } + } +} + +/// Result of a single-axis ESO bandwidth optimization. +#[derive(Debug, Clone)] +pub struct EsoResult { + /// Axis index (0=Roll, 1=Pitch, 2=Yaw). + #[allow(dead_code)] + pub axis: usize, + /// Optimal observer bandwidth in rad/s. + pub omega0_opt: f64, + /// Observer gain beta1 = 2 * omega0_opt. + pub beta1: f64, + /// Observer gain beta2 = omega0_opt^2. + pub beta2: f64, + /// Control effectiveness used. + pub b0: f64, + /// MSE tracking cost at the optimal omega0. + pub mse: f64, + /// Number of samples used in the optimization. + pub sample_count: usize, +} + +/// Compute 2nd-order bandwidth-parameterized LESO gains from omega_0. +/// Returns (beta1, beta2): beta1 = 2*omega0, beta2 = omega0^2. +fn leso2_gains(omega0: f64) -> (f64, f64) { + (2.0 * omega0, omega0 * omega0) +} + +/// Simulate 2nd-order discrete LESO (Euler forward) and return estimated rate sequence. +/// +/// Discrete update at each step k: +/// e = omega_meas[k] - omega_hat +/// omega_hat += Ts * (f_hat + b0 * u[k] + beta1 * e) +/// f_hat += Ts * (beta2 * e) +/// +/// # Arguments +/// * `omega_meas` - Measured angular rate (deg/s, filtered gyro). +/// * `u` - Control input (PID sum: P + I + D + F per axis). +/// * `ts` - Sample period in seconds (1 / sample_rate). +/// * `omega0` - Observer bandwidth (rad/s). +/// * `b0` - Control effectiveness. +fn simulate_leso2(omega_meas: &[f64], u: &[f64], ts: f64, omega0: f64, b0: f64) -> Vec { + let (beta1, beta2) = leso2_gains(omega0); + let n = omega_meas.len().min(u.len()); + + let mut omega_hat = omega_meas.first().copied().unwrap_or(0.0); + let mut f_hat = 0.0_f64; + + let mut estimated = Vec::with_capacity(n); + for k in 0..n { + estimated.push(omega_hat); + let e = omega_meas[k] - omega_hat; + omega_hat += ts * (f_hat + b0 * u[k] + beta1 * e); + f_hat += ts * (beta2 * e); + } + estimated +} + +/// Compute MSE between LESO-estimated and measured rate for a given omega_0. +fn mse_cost(omega_meas: &[f64], u: &[f64], ts: f64, omega0: f64, b0: f64) -> f64 { + let estimated = simulate_leso2(omega_meas, u, ts, omega0, b0); + let n = estimated.len().min(omega_meas.len()); + if n == 0 { + return f64::INFINITY; + } + let sum_sq: f64 = estimated + .iter() + .zip(omega_meas.iter()) + .map(|(e, m)| (e - m).powi(2)) + .sum(); + sum_sq / n as f64 +} + +/// Golden-section search for the minimum of a unimodal function on [lo, hi]. +/// Returns (best_x, f(best_x)). +fn golden_section_search f64>( + f: F, + mut lo: f64, + mut hi: f64, + tol: f64, + max_iter: usize, +) -> (f64, f64) { + // Inverse golden ratio: (sqrt(5) - 1) / 2 + const PHI_INV: f64 = 0.618_033_988_749_895; + let mut x1 = hi - PHI_INV * (hi - lo); + let mut x2 = lo + PHI_INV * (hi - lo); + let mut f1 = f(x1); + let mut f2 = f(x2); + + for _ in 0..max_iter { + if (hi - lo).abs() < tol { + break; + } + if f1 < f2 { + hi = x2; + x2 = x1; + f2 = f1; + x1 = hi - PHI_INV * (hi - lo); + f1 = f(x1); + } else { + lo = x1; + x1 = x2; + f1 = f2; + x2 = lo + PHI_INV * (hi - lo); + f2 = f(x2); + } + } + let best_x = (lo + hi) / 2.0; + (best_x, f(best_x)) +} + +/// Extract gyro measurements and PID sum for an axis from log data. +/// Rows with missing gyro are skipped; PID terms default to 0.0 if absent. +/// Returns (omega_meas, pid_sum) or None when fewer than 2 samples are available. +fn extract_axis_data(log_data: &[LogRowData], axis: usize) -> Option<(Vec, Vec)> { + let mut omega_meas = Vec::with_capacity(log_data.len()); + let mut pid_sum = Vec::with_capacity(log_data.len()); + + for row in log_data { + if let Some(gyro) = row.gyro[axis] { + let p = row.p_term[axis].unwrap_or(0.0); + let i_val = row.i_term[axis].unwrap_or(0.0); + let d = row.d_term[axis].unwrap_or(0.0); + let f_val = row.f_term[axis].unwrap_or(0.0); + omega_meas.push(gyro); + pid_sum.push(p + i_val + d + f_val); + } + } + + if omega_meas.len() < 2 { + return None; + } + Some((omega_meas, pid_sum)) +} + +/// Run ESO gain optimization for a single axis using golden-section search on omega_0. +/// +/// The search is constrained to omega_0 < sample_rate / 3 for discrete-time stability. +/// +/// # Arguments +/// * `log_data` - Parsed blackbox log rows. +/// * `sample_rate` - Loop rate in Hz. +/// * `axis` - Axis index (0=Roll, 1=Pitch, 2=Yaw). +/// * `config` - ESO configuration (b0 and omega_0 search bounds). +pub fn run_eso_optimization( + log_data: &[LogRowData], + sample_rate: f64, + axis: usize, + config: &EsoConfig, +) -> Result> { + let (omega_meas, pid_sum) = extract_axis_data(log_data, axis) + .ok_or("Insufficient data for ESO optimization (fewer than 2 usable samples)")?; + + // Enforce discrete-time stability: omega_0 < sample_rate / 3 + let omega0_max_stable = (sample_rate / 3.0).min(config.omega0_max); + if omega0_max_stable <= config.omega0_min { + return Err(format!( + "Sample rate {:.1} Hz too low for ESO search (need > {:.1} Hz)", + sample_rate, + config.omega0_min * 3.0 + ) + .into()); + } + + let ts = 1.0 / sample_rate; + let b0 = config.b0; + let cost_fn = |omega0: f64| mse_cost(&omega_meas, &pid_sum, ts, omega0, b0); + + let (omega0_opt, mse) = golden_section_search( + cost_fn, + config.omega0_min, + omega0_max_stable, + ESO_GSS_TOLERANCE, + ESO_GSS_MAX_ITER, + ); + + let (beta1, beta2) = leso2_gains(omega0_opt); + + Ok(EsoResult { + axis, + omega0_opt, + beta1, + beta2, + b0, + mse, + sample_count: omega_meas.len(), + }) +} diff --git a/src/lib.rs b/src/lib.rs index 6d466356..f241aa95 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -6,8 +6,10 @@ pub mod axis_names; pub mod constants; pub mod data_analysis; pub mod data_input; +pub mod eso; pub mod font_config; pub mod pid_context; pub mod plot_framework; pub mod plot_functions; +pub mod report; pub mod types; diff --git a/src/main.rs b/src/main.rs index ec5a4a0c..c7473c2d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,10 +5,12 @@ mod constants; mod data_analysis; mod data_input; mod debug_mode_lookup; +mod eso; mod font_config; mod pid_context; mod plot_framework; mod plot_functions; +mod report; mod types; use std::collections::HashSet; @@ -46,6 +48,10 @@ struct PlotConfig { pub motor_spectrums: bool, pub bode: bool, pub pid_activity: bool, + pub run_eso: bool, + pub run_report: bool, + pub eso_b0: f64, + pub eso_axes: [bool; 3], } impl Default for PlotConfig { @@ -66,6 +72,10 @@ impl Default for PlotConfig { motor_spectrums: true, bode: false, pid_activity: true, + run_eso: false, + run_report: false, + eso_b0: crate::constants::ESO_DEFAULT_B0, + eso_axes: [true; 3], } } } @@ -88,6 +98,10 @@ impl PlotConfig { motor_spectrums: false, bode: false, pid_activity: false, + run_eso: false, + run_report: false, + eso_b0: crate::constants::ESO_DEFAULT_B0, + eso_axes: [true; 3], } } } @@ -366,6 +380,12 @@ fn print_usage_and_exit(program_name: &str) { eprintln!( " --dps : Deg/s threshold for detailed step response plots (positive number)." ); + eprintln!(" --eso: Run 2nd-order LESO bandwidth optimization (omega_0) per axis."); + eprintln!( + " --eso-axis : Axes to optimize (comma-separated: roll,pitch,yaw). Default: all." + ); + eprintln!(" --eso-b0 : Control effectiveness b0 for ESO (default: 1.0)."); + eprintln!(" --report: Write a markdown statistical report (_report.md)."); eprintln!(); eprintln!("=== GENERAL ==="); eprintln!(); @@ -1084,6 +1104,61 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." } // CWD restoration happens automatically when _cwd_guard goes out of scope + + // --- ESO Gain Optimization --- + let mut eso_results: [Option; 3] = [None, None, None]; + if plot_config.run_eso { + println!("\n--- ESO Gain Optimization (2nd-order LESO) ---"); + if let Some(sr) = sample_rate { + let config = eso::EsoConfig { + b0: plot_config.eso_b0, + ..Default::default() + }; + for (axis_idx, (axis_enabled, eso_slot)) in plot_config + .eso_axes + .iter() + .zip(eso_results.iter_mut()) + .enumerate() + { + if *axis_enabled { + let axis_name = crate::axis_names::AXIS_NAMES + .get(axis_idx) + .unwrap_or(&"Unknown"); + print!(" {axis_name}: running ... "); + match eso::run_eso_optimization(&all_log_data, sr, axis_idx, &config) { + Ok(result) => { + println!( + "[OK] omega0={:.1} rad/s beta1={:.2} beta2={:.2} MSE={:.6}", + result.omega0_opt, result.beta1, result.beta2, result.mse + ); + *eso_slot = Some(result); + } + Err(e) => eprintln!("[WARN] Skipped: {e}"), + } + } + } + } else { + println!(" [WARN] Sample rate unknown — skipping ESO optimization."); + } + } + + // --- Markdown Statistical Report --- + if plot_config.run_report { + let report_filename = format!("{root_name_string}_report.md"); + let report_path = std::path::Path::new(&report_filename); + println!("\n--- Generating Markdown Report: {report_filename} ---"); + match report::generate_markdown_report( + &all_log_data, + sample_rate, + &header_metadata, + report_path, + &eso_results, + ) { + Ok(()) => println!(" [OK] Report written."), + Err(e) => eprintln!(" [ERROR] Report generation failed: {e}"), + } + } + println!("--- Finished processing file: {input_file_str} ---"); Ok(()) } @@ -1182,6 +1257,69 @@ fn main() -> Result<(), Box> { } else if arg == "--pid" { has_only_flags = true; pid_requested = true; + } else if arg == "--eso" { + plot_config.run_eso = true; + } else if arg == "--report" { + plot_config.run_report = true; + } else if arg == "--eso-b0" { + if i + 1 >= args.len() { + eprintln!("Error: --eso-b0 requires a numeric value."); + print_usage_and_exit(program_name); + } + match args[i + 1].parse::() { + Ok(val) if val > 0.0 && val.is_finite() => { + plot_config.eso_b0 = val; + i += 1; + } + _ => { + eprintln!( + "Error: --eso-b0 must be a positive finite number: {}", + args[i + 1] + ); + print_usage_and_exit(program_name); + } + } + } else if arg == "--eso-axis" { + if i + 1 >= args.len() { + eprintln!( + "Error: --eso-axis requires a comma-separated list of axes (roll,pitch,yaw)." + ); + print_usage_and_exit(program_name); + } + let val = args[i + 1].to_ascii_lowercase(); + let mut axes = [false; 3]; + let mut any_valid = false; + for part in val.split(',') { + match part.trim() { + "roll" | "0" => { + axes[0] = true; + any_valid = true; + } + "pitch" | "1" => { + axes[1] = true; + any_valid = true; + } + "yaw" | "2" => { + axes[2] = true; + any_valid = true; + } + "all" => { + axes = [true; 3]; + any_valid = true; + } + other => { + eprintln!( + "Error: Unknown axis '{}'. Use: roll, pitch, yaw, or all.", + other + ); + print_usage_and_exit(program_name); + } + } + } + if any_valid { + plot_config.eso_axes = axes; + } + i += 1; } else if arg.starts_with("--") { eprintln!("Error: Unknown option '{arg}'"); print_usage_and_exit(program_name); @@ -1193,7 +1331,16 @@ fn main() -> Result<(), Box> { // Apply "only" flags if any were specified (non-mutually exclusive: OR together) if has_only_flags { + // Preserve ESO/report settings through the plot-type reset + let saved_run_eso = plot_config.run_eso; + let saved_run_report = plot_config.run_report; + let saved_eso_b0 = plot_config.eso_b0; + let saved_eso_axes = plot_config.eso_axes; plot_config = PlotConfig::none(); + plot_config.run_eso = saved_run_eso; + plot_config.run_report = saved_run_report; + plot_config.eso_b0 = saved_eso_b0; + plot_config.eso_axes = saved_eso_axes; plot_config.step_response = step_requested; plot_config.motor_spectrums = motor_requested; plot_config.bode = bode_requested; diff --git a/src/report.rs b/src/report.rs new file mode 100644 index 00000000..1f56bb2e --- /dev/null +++ b/src/report.rs @@ -0,0 +1,230 @@ +// src/report.rs +// Markdown statistical report generation for flight controller blackbox data. +// Produces a per-axis summary: signal statistics and optional ESO optimization results. + +use std::error::Error; +use std::fmt::Write; +use std::fs; +use std::path::Path; + +use crate::axis_names::AXIS_NAMES; +use crate::data_input::log_data::LogRowData; +use crate::eso::EsoResult; + +/// Descriptive statistics for a time-series signal. +#[derive(Debug, Clone)] +pub struct SignalStats { + pub mean: f64, + pub std_dev: f64, + pub min: f64, + pub max: f64, + pub rms: f64, + pub count: usize, +} + +/// Compute descriptive statistics for a slice of f64 values. +/// Returns None if the slice is empty. +pub fn compute_signal_stats(data: &[f64]) -> Option { + let count = data.len(); + if count == 0 { + return None; + } + let mean = data.iter().sum::() / count as f64; + let var = data.iter().map(|x| (x - mean).powi(2)).sum::() / count as f64; + let std_dev = var.sqrt(); + let min = data.iter().cloned().fold(f64::INFINITY, f64::min); + let max = data.iter().cloned().fold(f64::NEG_INFINITY, f64::max); + let rms = (data.iter().map(|x| x * x).sum::() / count as f64).sqrt(); + Some(SignalStats { + mean, + std_dev, + min, + max, + rms, + count, + }) +} + +/// Per-axis signals extracted from log data for report generation. +struct AxisSignals { + gyro: Vec, + setpoint: Vec, + pid_sum: Vec, + pid_p: Vec, + pid_i: Vec, + pid_d: Vec, +} + +fn extract_axis_signals(log_data: &[LogRowData], axis: usize) -> AxisSignals { + let mut gyro = Vec::new(); + let mut setpoint = Vec::new(); + let mut pid_sum = Vec::new(); + let mut pid_p = Vec::new(); + let mut pid_i = Vec::new(); + let mut pid_d = Vec::new(); + + for row in log_data { + if let Some(g) = row.gyro[axis] { + let p = row.p_term[axis].unwrap_or(0.0); + let i_val = row.i_term[axis].unwrap_or(0.0); + let d = row.d_term[axis].unwrap_or(0.0); + let f_val = row.f_term[axis].unwrap_or(0.0); + gyro.push(g); + pid_p.push(p); + pid_i.push(i_val); + pid_d.push(d); + pid_sum.push(p + i_val + d + f_val); + } + if axis < 4 { + if let Some(sp) = row.setpoint[axis] { + setpoint.push(sp); + } + } + } + + AxisSignals { + gyro, + setpoint, + pid_sum, + pid_p, + pid_i, + pid_d, + } +} + +fn write_stats_row(md: &mut String, name: &str, stats: &SignalStats) -> std::fmt::Result { + writeln!( + md, + "| {} | {:.2} | {:.2} | {:.2} | {:.2} | {:.2} | {} |", + name, stats.mean, stats.std_dev, stats.min, stats.max, stats.rms, stats.count + ) +} + +/// Generate a markdown statistical report and write it to `output_path`. +/// +/// # Arguments +/// * `log_data` - Parsed blackbox log rows. +/// * `sample_rate` - Detected sample rate in Hz (None if unknown). +/// * `header_metadata` - Raw header key-value pairs from the log file. +/// * `output_path` - Destination file path for the `.md` report. +/// * `eso_results` - Per-axis ESO results (indexed 0=Roll, 1=Pitch, 2=Yaw). +pub fn generate_markdown_report( + log_data: &[LogRowData], + sample_rate: Option, + header_metadata: &[(String, String)], + output_path: &Path, + eso_results: &[Option; 3], +) -> Result<(), Box> { + let mut md = String::new(); + + writeln!(md, "# BlackBox Statistical Report")?; + writeln!(md)?; + writeln!(md, "## Metadata")?; + writeln!(md)?; + + match sample_rate { + Some(sr) => writeln!(md, "- **Sample Rate:** {:.1} Hz", sr)?, + None => writeln!(md, "- **Sample Rate:** Unknown")?, + } + writeln!(md, "- **Total Rows:** {}", log_data.len())?; + + if let (Some(first), Some(last)) = ( + log_data.first().and_then(|r| r.time_sec), + log_data.last().and_then(|r| r.time_sec), + ) { + writeln!( + md, + "- **Duration:** {:.2} s ({:.2} s to {:.2} s)", + last - first, + first, + last + )?; + } + writeln!(md)?; + + // Selected firmware / configuration keys + let interesting_keys = [ + "Firmware revision", + "Craft name", + "looptime", + "gyro_lpf_hz", + "dterm_lpf_hz", + "pid_process_denom", + "rollPID", + "pitchPID", + "yawPID", + ]; + let mut wrote_fw_header = false; + for (k, v) in header_metadata { + if interesting_keys.iter().any(|ik| k.eq_ignore_ascii_case(ik)) { + if !wrote_fw_header { + writeln!(md, "### Firmware / Configuration")?; + writeln!(md)?; + wrote_fw_header = true; + } + writeln!(md, "- **{}:** {}", k, v)?; + } + } + if wrote_fw_header { + writeln!(md)?; + } + + // Per-axis signal statistics + writeln!(md, "## Per-Axis Signal Statistics")?; + writeln!(md)?; + + for axis_idx in 0..AXIS_NAMES.len().min(3) { + let axis_name = AXIS_NAMES[axis_idx]; + let sigs = extract_axis_signals(log_data, axis_idx); + + writeln!(md, "### {}", axis_name)?; + writeln!(md)?; + writeln!(md, "| Signal | Mean | Std Dev | Min | Max | RMS | Count |")?; + writeln!(md, "|--------|------|---------|-----|-----|-----|-------|")?; + + if let Some(s) = compute_signal_stats(&sigs.gyro) { + write_stats_row(&mut md, "Gyro (filt)", &s)?; + } + if let Some(s) = compute_signal_stats(&sigs.setpoint) { + write_stats_row(&mut md, "Setpoint", &s)?; + } + if let Some(s) = compute_signal_stats(&sigs.pid_sum) { + write_stats_row(&mut md, "PID Sum", &s)?; + } + if let Some(s) = compute_signal_stats(&sigs.pid_p) { + write_stats_row(&mut md, "P-term", &s)?; + } + if let Some(s) = compute_signal_stats(&sigs.pid_i) { + write_stats_row(&mut md, "I-term", &s)?; + } + if let Some(s) = compute_signal_stats(&sigs.pid_d) { + write_stats_row(&mut md, "D-term", &s)?; + } + writeln!(md)?; + + // ESO optimization results for this axis + if let Some(eso) = &eso_results[axis_idx] { + writeln!(md, "**ESO Optimization Result (2nd-order LESO):**")?; + writeln!(md)?; + writeln!(md, "| Parameter | Value |")?; + writeln!(md, "|-----------|-------|")?; + writeln!(md, "| omega_0 (optimal) | {:.2} rad/s |", eso.omega0_opt)?; + writeln!(md, "| beta1 = 2*omega_0 | {:.2} |", eso.beta1)?; + writeln!(md, "| beta2 = omega_0^2 | {:.4} |", eso.beta2)?; + writeln!(md, "| b0 (control effectiveness) | {:.4} |", eso.b0)?; + writeln!(md, "| MSE (tracking error) | {:.6} |", eso.mse)?; + writeln!(md, "| Samples | {} |", eso.sample_count)?; + writeln!(md)?; + writeln!( + md, + "> Stability requirement: omega_0 < loop_rate / 3. \ + Use in an ADRC implementation. b0=1.0 (default) is dimensionless; \ + tune with physical inertia for absolute accuracy." + )?; + writeln!(md)?; + } + } + + fs::write(output_path, md)?; + Ok(()) +} From 41053944fc02a6ffe3c7ea6234ff70f951432afd Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 16 Apr 2026 15:55:58 -0500 Subject: [PATCH 2/7] eso: replace hand-rolled GSS with argmin, fix cost function, add time-domain plot - Add argmin 0.11 dependency; implement CostFunction trait for ESO bandwidth search - Replace monotone 1-step MSE cost with N-step-ahead open-loop prediction cost (unimodal: low omega0 = stale f_hat; high omega0 = noise amplification) Constants: ESO_N_AHEAD_STEPS=5, ESO_WARMUP_FRACTION=0.20 - EsoResult now carries omega_meas_trace, omega_hat_trace, f_hat_trace, timestamps - Add plot_eso.rs: stacked time-domain ESO output plot per axis (omega_meas blue thin over omega_hat orange thick, f_hat green scaled) - Wire ESO output plot into main.rs after optimization; generated as *_ESO_output_stacked.png - Update report.rs: MSE label clarified to N-step-ahead prediction - Add ESO plot colors to constants.rs (COLOR_ESO_MEAS/HAT/FHAT) - ESO_GSS_MAX_ITER type corrected to u64 for argmin compatibility --- Cargo.lock | 133 ++++++++++++++++++-- Cargo.toml | 1 + src/constants.rs | 9 +- src/eso.rs | 217 ++++++++++++++++++++++----------- src/main.rs | 10 ++ src/plot_functions/mod.rs | 1 + src/plot_functions/plot_eso.rs | 170 ++++++++++++++++++++++++++ src/report.rs | 2 +- 8 files changed, 466 insertions(+), 77 deletions(-) create mode 100644 src/plot_functions/plot_eso.rs diff --git a/Cargo.lock b/Cargo.lock index ba963de6..a3ffed28 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -7,13 +7,14 @@ name = "BlackBox_CSV_Render" version = "0.1.0" dependencies = [ "anyhow", + "argmin", "colorous", "csv", "ndarray", "ndarray-stats", "num-complex", "plotters", - "rand", + "rand 0.8.5", "realfft", "rusttype", "vergen", @@ -52,6 +53,37 @@ version = "1.0.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a23eb6b1614318a8071c9b2521f36b424b2c83db5eb3a0fead4a6c0809af6e61" +[[package]] +name = "argmin" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7ab7ca97779074715a402e5e8045fae27e7191acaec9b4c5653276316e9e404" +dependencies = [ + "anyhow", + "argmin-math", + "num-traits", + "paste", + "rand 0.9.4", + "rand_xoshiro", + "thiserror", + "web-time", +] + +[[package]] +name = "argmin-math" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba6958a87117ff5e1d0d7716856a4303752518012ec4a67a68446b6631a1a54d" +dependencies = [ + "anyhow", + "cfg-if", + "num-complex", + "num-integer", + "num-traits", + "rand 0.9.4", + "thiserror", +] + [[package]] name = "autocfg" version = "1.4.0" @@ -367,6 +399,18 @@ dependencies = [ "wasi", ] +[[package]] +name = "getrandom" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi", + "wasip2", +] + [[package]] name = "gif" version = "0.12.0" @@ -551,7 +595,7 @@ dependencies = [ "noisy_float", "num-integer", "num-traits", - "rand", + "rand 0.8.5", ] [[package]] @@ -626,6 +670,12 @@ dependencies = [ "ttf-parser 0.15.2", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pathfinder_geometry" version = "0.5.1" @@ -752,6 +802,12 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "r-efi" +version = "5.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" + [[package]] name = "rand" version = "0.8.5" @@ -759,8 +815,18 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" dependencies = [ "libc", - "rand_chacha", - "rand_core", + "rand_chacha 0.3.1", + "rand_core 0.6.4", +] + +[[package]] +name = "rand" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "44c5af06bb1b7d3216d91932aed5265164bf384dc89cd6ba05cf59a35f5f76ea" +dependencies = [ + "rand_chacha 0.9.0", + "rand_core 0.9.5", ] [[package]] @@ -770,7 +836,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" dependencies = [ "ppv-lite86", - "rand_core", + "rand_core 0.6.4", +] + +[[package]] +name = "rand_chacha" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d3022b5f1df60f26e1ffddd6c66e8aa15de382ae63b3a0c1bfc0e4d3e3f325cb" +dependencies = [ + "ppv-lite86", + "rand_core 0.9.5", ] [[package]] @@ -779,7 +855,25 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" dependencies = [ - "getrandom", + "getrandom 0.2.16", +] + +[[package]] +name = "rand_core" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "76afc826de14238e6e8c374ddcc1fa19e374fd8dd986b0d2af0d02377261d83c" +dependencies = [ + "getrandom 0.3.4", +] + +[[package]] +name = "rand_xoshiro" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f703f4665700daf5512dcca5f43afa6af89f09db47fb56be587f80636bda2d41" +dependencies = [ + "rand_core 0.9.5", ] [[package]] @@ -803,7 +897,7 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dd6f9d3d47bdd2ad6945c5015a226ec6155d0bcdfd8f7cd29f86b71f8de99d2b" dependencies = [ - "getrandom", + "getrandom 0.2.16", "libredox", "thiserror", ] @@ -1033,6 +1127,15 @@ version = "0.11.0+wasi-snapshot-preview1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" +[[package]] +name = "wasip2" +version = "1.0.2+wasi-0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9517f9239f02c069db75e65f174b3da828fe5f5b945c4dd26bd25d89c03ebcf5" +dependencies = [ + "wit-bindgen", +] + [[package]] name = "wasm-bindgen" version = "0.2.100" @@ -1101,6 +1204,16 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "web-time" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a6580f308b1fad9207618087a65c04e7a10bc77e02c8e84e9b00dd4b12fa0bb" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + [[package]] name = "weezl" version = "0.1.10" @@ -1343,6 +1456,12 @@ dependencies = [ "winapi", ] +[[package]] +name = "wit-bindgen" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7249219f66ced02969388cf2bb044a09756a083d0fab1e566056b04d9fbcaa5" + [[package]] name = "yeslogic-fontconfig-sys" version = "6.0.0" diff --git a/Cargo.toml b/Cargo.toml index 97a2c7c9..8e06309e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,6 +15,7 @@ ndarray = "0.15" ndarray-stats = "0.5" colorous = "1.0.16" rusttype = "0.9" +argmin = "0.11" [build-dependencies] anyhow = "1.0" diff --git a/src/constants.rs b/src/constants.rs index 9f18cc26..6583f0e4 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -263,7 +263,14 @@ pub const PHASE_PLOT_MARGIN_DEG: f64 = 30.0; // Padding above/below phase data f pub const ESO_OMEGA0_MIN: f64 = 50.0; // Lower bound for observer bandwidth search (rad/s) pub const ESO_OMEGA0_MAX: f64 = 500.0; // Upper bound for observer bandwidth search (rad/s) pub const ESO_GSS_TOLERANCE: f64 = 0.01; // Golden-section search convergence tolerance (rad/s) -pub const ESO_GSS_MAX_ITER: usize = 100; // Maximum iterations for golden-section search +pub const ESO_GSS_MAX_ITER: u64 = 100; // Maximum iterations for golden-section search pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimensionless) +pub const ESO_N_AHEAD_STEPS: usize = 5; // Steps ahead for open-loop prediction cost (unimodal objective) +pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer spin-up before cost evaluation + +// ESO output plot colors +pub const COLOR_ESO_MEAS: &RGBColor = &LIGHTBLUE; // Measured gyro rate +pub const COLOR_ESO_HAT: &RGBColor = &ORANGE; // ESO estimated rate (omega_hat) +pub const COLOR_ESO_FHAT: &RGBColor = &GREEN; // ESO disturbance estimate (f_hat, scaled) // src/constants.rs diff --git a/src/eso.rs b/src/eso.rs index 5eee45b5..795eac91 100644 --- a/src/eso.rs +++ b/src/eso.rs @@ -1,13 +1,23 @@ // src/eso.rs // 2nd-order Linear Extended State Observer (LESO) for flight controller blackbox data. -// Implements discrete Euler-forward LESO simulation with golden-section search for the +// Implements discrete Euler-forward LESO simulation with argmin GoldenSectionSearch for the // optimal observer bandwidth (omega_0) using PID sum as control input and filtered // gyro as measured output. +// +// Cost function: N-step-ahead open-loop prediction MSE (unimodal objective). +// After a correction-phase warm-up, the observer state at each sample is propagated +// ESO_N_AHEAD_STEPS forward WITHOUT correction; the prediction is compared to the actual +// measurement. This objective is U-shaped: too-low omega_0 leaves f_hat stale (poor +// prediction), too-high omega_0 amplifies noise into f_hat (also poor prediction). use std::error::Error; +use argmin::core::{CostFunction, Executor}; +use argmin::solver::goldensectionsearch::GoldenSectionSearch; + use crate::constants::{ - ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, + ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, + ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, }; use crate::data_input::log_data::LogRowData; @@ -46,10 +56,18 @@ pub struct EsoResult { pub beta2: f64, /// Control effectiveness used. pub b0: f64, - /// MSE tracking cost at the optimal omega0. + /// N-step-ahead prediction MSE at the optimal omega0. pub mse: f64, /// Number of samples used in the optimization. pub sample_count: usize, + /// Timestamps (seconds) aligned to the trace data. + pub timestamps: Vec, + /// Measured angular rate (filtered gyro) used for optimization. + pub omega_meas_trace: Vec, + /// omega_hat trace from final simulation with optimal gains. + pub omega_hat_trace: Vec, + /// f_hat (disturbance estimate) trace from final simulation. + pub f_hat_trace: Vec, } /// Compute 2nd-order bandwidth-parameterized LESO gains from omega_0. @@ -58,7 +76,7 @@ fn leso2_gains(omega0: f64) -> (f64, f64) { (2.0 * omega0, omega0 * omega0) } -/// Simulate 2nd-order discrete LESO (Euler forward) and return estimated rate sequence. +/// Simulate 2nd-order discrete LESO (Euler forward) and return (omega_hat, f_hat) traces. /// /// Discrete update at each step k: /// e = omega_meas[k] - omega_hat @@ -71,82 +89,120 @@ fn leso2_gains(omega0: f64) -> (f64, f64) { /// * `ts` - Sample period in seconds (1 / sample_rate). /// * `omega0` - Observer bandwidth (rad/s). /// * `b0` - Control effectiveness. -fn simulate_leso2(omega_meas: &[f64], u: &[f64], ts: f64, omega0: f64, b0: f64) -> Vec { +fn simulate_leso2( + omega_meas: &[f64], + u: &[f64], + ts: f64, + omega0: f64, + b0: f64, +) -> (Vec, Vec) { let (beta1, beta2) = leso2_gains(omega0); let n = omega_meas.len().min(u.len()); let mut omega_hat = omega_meas.first().copied().unwrap_or(0.0); let mut f_hat = 0.0_f64; - let mut estimated = Vec::with_capacity(n); + let mut omega_hats = Vec::with_capacity(n); + let mut f_hats = Vec::with_capacity(n); + for k in 0..n { - estimated.push(omega_hat); + omega_hats.push(omega_hat); + f_hats.push(f_hat); let e = omega_meas[k] - omega_hat; omega_hat += ts * (f_hat + b0 * u[k] + beta1 * e); f_hat += ts * (beta2 * e); } - estimated + (omega_hats, f_hats) } -/// Compute MSE between LESO-estimated and measured rate for a given omega_0. -fn mse_cost(omega_meas: &[f64], u: &[f64], ts: f64, omega0: f64, b0: f64) -> f64 { - let estimated = simulate_leso2(omega_meas, u, ts, omega0, b0); - let n = estimated.len().min(omega_meas.len()); - if n == 0 { +/// Compute the N-step-ahead open-loop prediction MSE for a given omega_0. +/// +/// The observer runs with full correction on all data (first pass) to obtain per-sample +/// states. Then for each sample after the warm-up fraction, the state is propagated +/// ESO_N_AHEAD_STEPS forward open-loop (no correction — f_hat frozen) and compared to +/// the actual measurement at k + N. This creates a unimodal cost: +/// - Low omega_0: f_hat is stale → poor N-step prediction. +/// - High omega_0: noise amplified into f_hat → poor N-step prediction. +/// - Optimal omega_0: balanced disturbance estimation → best N-step prediction. +fn nstep_prediction_mse(omega_meas: &[f64], u: &[f64], ts: f64, omega0: f64, b0: f64) -> f64 { + let n = omega_meas.len().min(u.len()); + if n <= ESO_N_AHEAD_STEPS + 1 { return f64::INFINITY; } - let sum_sq: f64 = estimated - .iter() - .zip(omega_meas.iter()) - .map(|(e, m)| (e - m).powi(2)) - .sum(); - sum_sq / n as f64 -} + let (beta1, beta2) = leso2_gains(omega0); -/// Golden-section search for the minimum of a unimodal function on [lo, hi]. -/// Returns (best_x, f(best_x)). -fn golden_section_search f64>( - f: F, - mut lo: f64, - mut hi: f64, - tol: f64, - max_iter: usize, -) -> (f64, f64) { - // Inverse golden ratio: (sqrt(5) - 1) / 2 - const PHI_INV: f64 = 0.618_033_988_749_895; - let mut x1 = hi - PHI_INV * (hi - lo); - let mut x2 = lo + PHI_INV * (hi - lo); - let mut f1 = f(x1); - let mut f2 = f(x2); - - for _ in 0..max_iter { - if (hi - lo).abs() < tol { - break; - } - if f1 < f2 { - hi = x2; - x2 = x1; - f2 = f1; - x1 = hi - PHI_INV * (hi - lo); - f1 = f(x1); - } else { - lo = x1; - x1 = x2; - f1 = f2; - x2 = lo + PHI_INV * (hi - lo); - f2 = f(x2); + // First pass: run observer with correction to capture states at each sample. + let mut omega_hat_states = vec![0.0_f64; n]; + let mut f_hat_states = vec![0.0_f64; n]; + let mut omega_hat = omega_meas[0]; + let mut f_hat = 0.0_f64; + for k in 0..n { + omega_hat_states[k] = omega_hat; + f_hat_states[k] = f_hat; + let e = omega_meas[k] - omega_hat; + omega_hat += ts * (f_hat + b0 * u[k] + beta1 * e); + f_hat += ts * (beta2 * e); + } + + // Warm-up: skip initial fraction to let the observer states converge. + let warmup = ((n as f64 * ESO_WARMUP_FRACTION) as usize).max(1); + let end = n.saturating_sub(ESO_N_AHEAD_STEPS); + if warmup >= end { + return f64::INFINITY; + } + + // Second pass: N-step open-loop prediction from each warm sample. + let mut sum_sq = 0.0_f64; + let mut count = 0usize; + for k in warmup..end { + let mut omega_pred = omega_hat_states[k]; + let f_pred = f_hat_states[k]; // frozen — no correction in open-loop propagation + for j in 0..ESO_N_AHEAD_STEPS { + omega_pred += ts * (f_pred + b0 * u[k + j]); } + sum_sq += (omega_pred - omega_meas[k + ESO_N_AHEAD_STEPS]).powi(2); + count += 1; + } + if count == 0 { + f64::INFINITY + } else { + sum_sq / count as f64 + } +} + +/// argmin CostFunction wrapper for single-axis ESO bandwidth search. +struct EsoCostFn<'a> { + omega_meas: &'a [f64], + u: &'a [f64], + ts: f64, + b0: f64, +} + +impl CostFunction for EsoCostFn<'_> { + type Param = f64; + type Output = f64; + + fn cost(&self, omega0: &f64) -> Result { + Ok(nstep_prediction_mse( + self.omega_meas, + self.u, + self.ts, + *omega0, + self.b0, + )) } - let best_x = (lo + hi) / 2.0; - (best_x, f(best_x)) } -/// Extract gyro measurements and PID sum for an axis from log data. +/// Extract gyro measurements, PID sum, and timestamps for an axis from log data. /// Rows with missing gyro are skipped; PID terms default to 0.0 if absent. -/// Returns (omega_meas, pid_sum) or None when fewer than 2 samples are available. -fn extract_axis_data(log_data: &[LogRowData], axis: usize) -> Option<(Vec, Vec)> { +/// Returns (omega_meas, pid_sum, timestamps_sec) or None when fewer than 2 samples are available. +fn extract_axis_data( + log_data: &[LogRowData], + axis: usize, +) -> Option<(Vec, Vec, Vec)> { let mut omega_meas = Vec::with_capacity(log_data.len()); let mut pid_sum = Vec::with_capacity(log_data.len()); + let mut timestamps = Vec::with_capacity(log_data.len()); for row in log_data { if let Some(gyro) = row.gyro[axis] { @@ -156,18 +212,20 @@ fn extract_axis_data(log_data: &[LogRowData], axis: usize) -> Option<(Vec, let f_val = row.f_term[axis].unwrap_or(0.0); omega_meas.push(gyro); pid_sum.push(p + i_val + d + f_val); + timestamps.push(row.time_sec.unwrap_or(0.0)); } } if omega_meas.len() < 2 { return None; } - Some((omega_meas, pid_sum)) + Some((omega_meas, pid_sum, timestamps)) } -/// Run ESO gain optimization for a single axis using golden-section search on omega_0. +/// Run ESO gain optimization for a single axis using argmin GoldenSectionSearch on omega_0. /// /// The search is constrained to omega_0 < sample_rate / 3 for discrete-time stability. +/// The cost function is N-step-ahead open-loop prediction MSE, which is unimodal. /// /// # Arguments /// * `log_data` - Parsed blackbox log rows. @@ -180,7 +238,7 @@ pub fn run_eso_optimization( axis: usize, config: &EsoConfig, ) -> Result> { - let (omega_meas, pid_sum) = extract_axis_data(log_data, axis) + let (omega_meas, pid_sum, timestamps) = extract_axis_data(log_data, axis) .ok_or("Insufficient data for ESO optimization (fewer than 2 usable samples)")?; // Enforce discrete-time stability: omega_0 < sample_rate / 3 @@ -196,18 +254,37 @@ pub fn run_eso_optimization( let ts = 1.0 / sample_rate; let b0 = config.b0; - let cost_fn = |omega0: f64| mse_cost(&omega_meas, &pid_sum, ts, omega0, b0); - let (omega0_opt, mse) = golden_section_search( - cost_fn, - config.omega0_min, - omega0_max_stable, - ESO_GSS_TOLERANCE, - ESO_GSS_MAX_ITER, - ); + let problem = EsoCostFn { + omega_meas: &omega_meas, + u: &pid_sum, + ts, + b0, + }; + + let solver = GoldenSectionSearch::new(config.omega0_min, omega0_max_stable) + .map_err(|e| -> Box { format!("argmin GSS init: {e}").into() })? + .with_tolerance(ESO_GSS_TOLERANCE) + .map_err(|e| -> Box { format!("argmin GSS tolerance: {e}").into() })?; + + let initial = (config.omega0_min + omega0_max_stable) / 2.0; + + let run_result = Executor::new(problem, solver) + .configure(|state| state.param(initial).max_iters(ESO_GSS_MAX_ITER)) + .run() + .map_err(|e| -> Box { format!("argmin optimization: {e}").into() })?; + + let omega0_opt = run_result + .state() + .best_param + .ok_or("ESO optimization returned no solution")?; + let mse = run_result.state().best_cost; let (beta1, beta2) = leso2_gains(omega0_opt); + // Final simulation with optimal gains to produce trace data for plotting. + let (omega_hat_trace, f_hat_trace) = simulate_leso2(&omega_meas, &pid_sum, ts, omega0_opt, b0); + Ok(EsoResult { axis, omega0_opt, @@ -216,5 +293,9 @@ pub fn run_eso_optimization( b0, mse, sample_count: omega_meas.len(), + timestamps, + omega_meas_trace: omega_meas, + omega_hat_trace, + f_hat_trace, }) } diff --git a/src/main.rs b/src/main.rs index c7473c2d..c58fafe9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1140,6 +1140,16 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." } else { println!(" [WARN] Sample rate unknown — skipping ESO optimization."); } + + // --- ESO Output Plot --- + let any_eso = eso_results.iter().any(|r| r.is_some()); + if any_eso { + println!("\n--- Generating ESO Output Plot ---"); + match plot_functions::plot_eso::plot_eso_output(&eso_results, &root_name_string) { + Ok(()) => println!(" [OK] ESO output plot written."), + Err(e) => eprintln!(" [ERROR] ESO output plot failed: {e}"), + } + } } // --- Markdown Statistical Report --- diff --git a/src/plot_functions/mod.rs b/src/plot_functions/mod.rs index 71ee9e90..5e5f888d 100644 --- a/src/plot_functions/mod.rs +++ b/src/plot_functions/mod.rs @@ -5,6 +5,7 @@ pub mod plot_bode; pub mod plot_d_term_heatmap; pub mod plot_d_term_psd; pub mod plot_d_term_spectrums; +pub mod plot_eso; pub mod plot_gyro_spectrums; pub mod plot_gyro_vs_unfilt; pub mod plot_motor_spectrums; diff --git a/src/plot_functions/plot_eso.rs b/src/plot_functions/plot_eso.rs new file mode 100644 index 00000000..fdcbf0f4 --- /dev/null +++ b/src/plot_functions/plot_eso.rs @@ -0,0 +1,170 @@ +// src/plot_functions/plot_eso.rs +// Time-domain plot of ESO estimated output (omega_hat) vs measured gyro (omega_meas), +// plus scaled disturbance estimate (f_hat) — one stacked subplot per axis. + +use plotters::style::RGBColor; +use std::error::Error; + +use crate::axis_names::AXIS_NAMES; +use crate::constants::{ + COLOR_ESO_FHAT, COLOR_ESO_HAT, COLOR_ESO_MEAS, LINE_WIDTH_PLOT, UNIFIED_Y_AXIS_HEADROOM_SCALE, + UNIFIED_Y_AXIS_MIN_SCALE, UNIFIED_Y_AXIS_PERCENTILE, +}; +use crate::eso::EsoResult; +use crate::plot_framework::{draw_stacked_plot, PlotSeries}; + +/// Generates the stacked ESO estimated-output plot. +/// +/// Each axis subplot shows: +/// - omega_meas: the measured (filtered) gyro rate (blue) +/// - omega_hat: the ESO estimated rate (orange) +/// - f_hat: the disturbance estimate, scaled to ±50% of the omega range (green) +/// +/// Only axes with a valid EsoResult are rendered; others show the unavailable message. +pub fn plot_eso_output( + eso_results: &[Option; 3], + root_name: &str, +) -> Result<(), Box> { + let output_file = format!("{root_name}_ESO_output_stacked.png"); + let plot_type_name = "ESO Output"; + + let color_meas: RGBColor = *COLOR_ESO_MEAS; + let color_hat: RGBColor = *COLOR_ESO_HAT; + let color_fhat: RGBColor = *COLOR_ESO_FHAT; + let stroke = LINE_WIDTH_PLOT; + + // Pre-compute per-axis plot data so the closure can own it. + let mut axis_data: [Option; 3] = [None, None, None]; + for (i, result) in eso_results.iter().enumerate() { + if let Some(eso) = result { + axis_data[i] = Some(build_axis_data(eso)); + } + } + + // Collect all |omega_meas| values across all axes for a unified Y range. + let mut all_abs: Vec = Vec::new(); + for data in axis_data.iter().flatten() { + for &(_, m, _) in &data.meas_hat { + all_abs.push(m.abs()); + } + } + let half_range = if !all_abs.is_empty() { + all_abs.sort_by(|a, b| a.total_cmp(b)); + let p95_idx = ((all_abs.len() - 1) as f64 * UNIFIED_Y_AXIS_PERCENTILE).floor() as usize; + (all_abs[p95_idx] * UNIFIED_Y_AXIS_HEADROOM_SCALE).max(UNIFIED_Y_AXIS_MIN_SCALE) + } else { + UNIFIED_Y_AXIS_MIN_SCALE + }; + + draw_stacked_plot(&output_file, root_name, plot_type_name, move |axis_index| { + let data = axis_data[axis_index].as_ref()?; + if data.meas_hat.is_empty() { + return None; + } + + let time_min = data.meas_hat.first().map(|&(t, _, _)| t).unwrap_or(0.0); + let time_max = data.meas_hat.last().map(|&(t, _, _)| t).unwrap_or(0.0); + if time_min >= time_max { + return None; + } + + let x_range = time_min..time_max; + let y_range = -half_range..half_range; + + let meas_data: Vec<(f64, f64)> = data.meas_hat.iter().map(|&(t, m, _)| (t, m)).collect(); + let hat_data: Vec<(f64, f64)> = data.meas_hat.iter().map(|&(t, _, h)| (t, h)).collect(); + + // Draw hat first (thick, background) then meas on top (thin, foreground). + // Since a well-tuned ESO tracks closely, the orange will only visibly peek + // out at transients where the observer briefly diverges from the measurement. + let mut series = vec![ + PlotSeries { + data: hat_data, + label: format!( + "ESO estimate (omega_hat, omega0={:.0} rad/s, b0={:.2})", + data.omega0_opt, data.b0 + ), + color: color_hat, + stroke_width: stroke + 1, + }, + PlotSeries { + data: meas_data, + label: "Measured gyro (omega_meas)".to_string(), + color: color_meas, + stroke_width: stroke, + }, + ]; + + // Scale f_hat to fit ±50% of the omega Y range for visual interpretability. + if let Some(scale) = data.fhat_scale { + let fhat_data: Vec<(f64, f64)> = + data.fhat.iter().map(|&(t, f)| (t, f * scale)).collect(); + series.push(PlotSeries { + data: fhat_data, + label: format!("f_hat x{scale:.2e} (disturbance est., scaled)"), + color: color_fhat, + stroke_width: stroke, + }); + } + + Some(( + format!("{} ESO Output", AXIS_NAMES[axis_index]), + x_range, + y_range, + series, + "Time (s)".to_string(), + "Rate (deg/s)".to_string(), + )) + }) +} + +// Internal per-axis pre-computed data. +struct AxisEsoData { + omega0_opt: f64, + b0: f64, + /// (time, omega_meas, omega_hat) triples + meas_hat: Vec<(f64, f64, f64)>, + /// (time, f_hat_raw) before visual scaling + fhat: Vec<(f64, f64)>, + /// Scale mapping f_hat peak to ±50% of UNIFIED_Y_AXIS_MIN_SCALE, or None if all-zero. + fhat_scale: Option, +} + +fn build_axis_data(eso: &EsoResult) -> AxisEsoData { + let n = eso + .timestamps + .len() + .min(eso.omega_meas_trace.len()) + .min(eso.omega_hat_trace.len()) + .min(eso.f_hat_trace.len()); + + let meas_hat: Vec<(f64, f64, f64)> = (0..n) + .map(|k| { + ( + eso.timestamps[k], + eso.omega_meas_trace[k], + eso.omega_hat_trace[k], + ) + }) + .collect(); + + let fhat: Vec<(f64, f64)> = (0..n) + .map(|k| (eso.timestamps[k], eso.f_hat_trace[k])) + .collect(); + + let fhat_max_abs = fhat.iter().map(|&(_, f)| f.abs()).fold(0.0_f64, f64::max); + + let fhat_scale = if fhat_max_abs > 1e-12 { + Some((UNIFIED_Y_AXIS_MIN_SCALE * 0.5) / fhat_max_abs) + } else { + None + }; + + AxisEsoData { + omega0_opt: eso.omega0_opt, + b0: eso.b0, + meas_hat, + fhat, + fhat_scale, + } +} diff --git a/src/report.rs b/src/report.rs index 1f56bb2e..a76893c2 100644 --- a/src/report.rs +++ b/src/report.rs @@ -212,7 +212,7 @@ pub fn generate_markdown_report( writeln!(md, "| beta1 = 2*omega_0 | {:.2} |", eso.beta1)?; writeln!(md, "| beta2 = omega_0^2 | {:.4} |", eso.beta2)?; writeln!(md, "| b0 (control effectiveness) | {:.4} |", eso.b0)?; - writeln!(md, "| MSE (tracking error) | {:.6} |", eso.mse)?; + writeln!(md, "| MSE (N-step-ahead prediction) | {:.6} |", eso.mse)?; writeln!(md, "| Samples | {} |", eso.sample_count)?; writeln!(md)?; writeln!( From 31a14f36e62ec56d3e56c14aae02791bd4d4ad8f Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 17 Apr 2026 14:49:28 -0500 Subject: [PATCH 3/7] eso: auto-estimate b0 via OLS on rate derivatives (QuickFlash method) - estimate_b0(): OLS closed-form b0 = sum(u*d_omega) / (Ts * sum(u^2)) on samples where |PID sum| >= ESO_B0_MIN_CONTROL_THRESHOLD (10.0) - b0 auto-estimated from data when user has not overridden --eso-b0; falls back to ESO_DEFAULT_B0 (1.0) when too few valid samples - EsoResult.b0_auto: true when estimated, false when user-supplied - main.rs: print shows b0=X.XXXX (estimated|user) - report.rs: b0 row labels (auto-estimated|user-supplied); updated guidance - Add ESO_B0_MIN_CONTROL_THRESHOLD = 10.0 to constants.rs - Fix pre-existing clippy collapsible_match in pid_metadata.rs --- src/constants.rs | 1 + src/data_input/pid_metadata.rs | 4 +-- src/eso.rs | 60 ++++++++++++++++++++++++++++++++-- src/main.rs | 9 +++-- src/report.rs | 15 +++++++-- 5 files changed, 78 insertions(+), 11 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 6583f0e4..78ac1ae1 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -267,6 +267,7 @@ pub const ESO_GSS_MAX_ITER: u64 = 100; // Maximum iterations for golden-section pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimensionless) pub const ESO_N_AHEAD_STEPS: usize = 5; // Steps ahead for open-loop prediction cost (unimodal objective) pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer spin-up before cost evaluation +pub const ESO_B0_MIN_CONTROL_THRESHOLD: f64 = 10.0; // Minimum |PID sum| to include a sample in b0 OLS estimation // ESO output plot colors pub const COLOR_ESO_MEAS: &RGBColor = &LIGHTBLUE; // Measured gyro rate diff --git a/src/data_input/pid_metadata.rs b/src/data_input/pid_metadata.rs index 74af6e73..ca279f9a 100644 --- a/src/data_input/pid_metadata.rs +++ b/src/data_input/pid_metadata.rs @@ -581,9 +581,7 @@ fn parse_axis_pid(pid_str: &str) -> AxisPid { match values.len() { 4 => { // INAV style: P,I,D,FF - if values[3] > 0 { - axis_pid.ff = Some(values[3]); - } + axis_pid.ff = (values[3] > 0).then_some(values[3]); } 5 => { // Betaflight 4.6+ style: P,I,D(base=D-Min),D-Max,FF diff --git a/src/eso.rs b/src/eso.rs index 795eac91..931e4407 100644 --- a/src/eso.rs +++ b/src/eso.rs @@ -16,8 +16,8 @@ use argmin::core::{CostFunction, Executor}; use argmin::solver::goldensectionsearch::GoldenSectionSearch; use crate::constants::{ - ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, - ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, + ESO_B0_MIN_CONTROL_THRESHOLD, ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, + ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, }; use crate::data_input::log_data::LogRowData; @@ -56,6 +56,8 @@ pub struct EsoResult { pub beta2: f64, /// Control effectiveness used. pub b0: f64, + /// True when b0 was estimated from data; false when provided by the user. + pub b0_auto: bool, /// N-step-ahead prediction MSE at the optimal omega0. pub mse: f64, /// Number of samples used in the optimization. @@ -193,6 +195,47 @@ impl CostFunction for EsoCostFn<'_> { } } +/// Estimate control effectiveness b0 via ordinary least squares on rate derivative increments. +/// +/// QuickFlash's guidance: set beta1/beta2 → 0 (no observer correction), find b0 that +/// minimises prediction error so "it's pretty much just b0 doing the job". +/// +/// With correction gains = 0 the LESO update reduces to: +/// ω[k+1] − ω[k] ≈ Ts · b0 · u[k] +/// +/// OLS closed form: b0 = Σ(u[k] · Δω[k]) / (Ts · Σ(u[k]²)) +/// +/// Only samples where |u[k]| ≥ ESO_B0_MIN_CONTROL_THRESHOLD are included to avoid +/// numerical issues from near-zero control inputs. +/// Returns None when fewer than 10 valid samples are available or when the estimate +/// is non-positive (which would indicate a mis-matched control law). +fn estimate_b0(omega_meas: &[f64], u: &[f64], ts: f64) -> Option { + let n = omega_meas.len().min(u.len()).saturating_sub(1); + let mut num = 0.0_f64; + let mut den = 0.0_f64; + let mut count = 0usize; + + for k in 0..n { + if u[k].abs() < ESO_B0_MIN_CONTROL_THRESHOLD { + continue; + } + let delta_omega = omega_meas[k + 1] - omega_meas[k]; + num += u[k] * delta_omega; + den += u[k] * u[k] * ts; + count += 1; + } + + if count < 10 || den.abs() < 1e-12 { + return None; + } + let b0 = num / den; + if b0.is_finite() && b0.abs() > 1e-9 { + Some(b0) + } else { + None + } +} + /// Extract gyro measurements, PID sum, and timestamps for an axis from log data. /// Rows with missing gyro are skipped; PID terms default to 0.0 if absent. /// Returns (omega_meas, pid_sum, timestamps_sec) or None when fewer than 2 samples are available. @@ -253,7 +296,17 @@ pub fn run_eso_optimization( } let ts = 1.0 / sample_rate; - let b0 = config.b0; + + // Stage 1: estimate b0 from data via OLS on rate derivatives (QuickFlash guidance). + // If the user explicitly provided a non-default b0 via --eso-b0, respect it. + let (b0, b0_auto) = if (config.b0 - ESO_DEFAULT_B0).abs() < 1e-12 { + match estimate_b0(&omega_meas, &pid_sum, ts) { + Some(estimated) => (estimated, true), + None => (config.b0, false), + } + } else { + (config.b0, false) + }; let problem = EsoCostFn { omega_meas: &omega_meas, @@ -291,6 +344,7 @@ pub fn run_eso_optimization( beta1, beta2, b0, + b0_auto, mse, sample_count: omega_meas.len(), timestamps, diff --git a/src/main.rs b/src/main.rs index c58fafe9..7327739b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1127,9 +1127,14 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." print!(" {axis_name}: running ... "); match eso::run_eso_optimization(&all_log_data, sr, axis_idx, &config) { Ok(result) => { + let b0_label = if result.b0_auto { + "(estimated)" + } else { + "(user)" + }; println!( - "[OK] omega0={:.1} rad/s beta1={:.2} beta2={:.2} MSE={:.6}", - result.omega0_opt, result.beta1, result.beta2, result.mse + "[OK] omega0={:.1} rad/s b0={:.4} {b0_label} beta1={:.2} beta2={:.2} MSE={:.6}", + result.omega0_opt, result.b0, result.beta1, result.beta2, result.mse ); *eso_slot = Some(result); } diff --git a/src/report.rs b/src/report.rs index a76893c2..55b874f1 100644 --- a/src/report.rs +++ b/src/report.rs @@ -211,15 +211,24 @@ pub fn generate_markdown_report( writeln!(md, "| omega_0 (optimal) | {:.2} rad/s |", eso.omega0_opt)?; writeln!(md, "| beta1 = 2*omega_0 | {:.2} |", eso.beta1)?; writeln!(md, "| beta2 = omega_0^2 | {:.4} |", eso.beta2)?; - writeln!(md, "| b0 (control effectiveness) | {:.4} |", eso.b0)?; + let b0_label = if eso.b0_auto { + "(auto-estimated from data)" + } else { + "(user-supplied)" + }; + writeln!( + md, + "| b0 (control effectiveness) | {:.4} {b0_label} |", + eso.b0 + )?; writeln!(md, "| MSE (N-step-ahead prediction) | {:.6} |", eso.mse)?; writeln!(md, "| Samples | {} |", eso.sample_count)?; writeln!(md)?; writeln!( md, "> Stability requirement: omega_0 < loop_rate / 3. \ - Use in an ADRC implementation. b0=1.0 (default) is dimensionless; \ - tune with physical inertia for absolute accuracy." + Use in an ADRC implementation. b0 is estimated from data via OLS on rate \ + derivatives (QuickFlash method); override with --eso-b0 if needed." )?; writeln!(md)?; } From 6e600ca8c7f0aa9bf3555665b78b44527880d668 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 24 Apr 2026 12:43:21 -0500 Subject: [PATCH 4/7] Fix CodeRabbitAI review issues from PR#136 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - OVERVIEW.md: move *_report.md bullet out of 'Generated PNG Plots' into a new 'Generated Reports' subsection (was self-contradictory) - src/eso.rs: add upfront validation in run_eso_optimization * axis >= AXIS_COUNT → descriptive Err instead of panic * non-finite / non-positive sample_rate → Err * invalid EsoConfig fields (omega0_min/max, b0) → Err * add control-input excitation guard: reject axes where pid_sum is all-zero / sub-epsilon so flat-input axes are not 'optimised' - src/report.rs: fix PID term collection in extract_axis_signals * replace unwrap_or(0.0) with per-field Option guards so absent P/I/D/F terms are not silently reported as real zero samples * replace hardcoded 'axis < 4' magic literal with row.setpoint.get(axis).copied().flatten() (bounds-safe) - src/plot_functions/plot_eso.rs + src/constants.rs: * add ESO_FHAT_Y_FRACTION = 0.5 constant (no more magic 0.5) * store fhat_max_abs in AxisEsoData instead of pre-computed scale * compute fhat_scale inside draw_stacked_plot after half_range is known: (half_range * ESO_FHAT_Y_FRACTION) / fhat_max_abs fixes f_hat being squashed when half_range > UNIFIED_Y_AXIS_MIN_SCALE - src/main.rs: --eso-axis and --eso-b0 now imply --eso (previously silently ignored when --eso was omitted) --- OVERVIEW.md | 3 +++ src/constants.rs | 1 + src/eso.rs | 25 ++++++++++++++++++++++++- src/main.rs | 2 ++ src/plot_functions/plot_eso.rs | 19 +++++++------------ src/report.rs | 26 +++++++++++++++++--------- 6 files changed, 54 insertions(+), 22 deletions(-) diff --git a/OVERVIEW.md b/OVERVIEW.md index f7e37919..61c4097c 100644 --- a/OVERVIEW.md +++ b/OVERVIEW.md @@ -204,6 +204,9 @@ When `--step` flag is not used, all plots below are generated: - **`*_Gyro_PSD_Spectrogram_comparative.png`** — Gyro spectrogram (PSD vs. time) using Short-Time Fourier Transform - **`*_Throttle_Freq_Heatmap_comparative.png`** — System noise characteristics across throttle levels and frequencies - **`*_PID_Activity_stacked.png`** — P, I, D term activity over time for each axis (Roll, Pitch, Yaw). Displays all three PID components on the same time-domain plot with unified Y-axis scaling for visual comparison. Each term shows min/avg/max statistics in the legend. Useful for visualizing PID contribution balance during flight and identifying control issues (persistent P-term offset, I-term wind direction, D-term phase lag). + +#### Generated Reports + - **`*_report.md`** — Markdown statistical report (requires `--report`). Per-axis signal statistics table and, when combined with `--eso`, the optimised LESO gains. #### P:D Ratio Recommendations diff --git a/src/constants.rs b/src/constants.rs index 78ac1ae1..6c2810d2 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -268,6 +268,7 @@ pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimension pub const ESO_N_AHEAD_STEPS: usize = 5; // Steps ahead for open-loop prediction cost (unimodal objective) pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer spin-up before cost evaluation pub const ESO_B0_MIN_CONTROL_THRESHOLD: f64 = 10.0; // Minimum |PID sum| to include a sample in b0 OLS estimation +pub const ESO_FHAT_Y_FRACTION: f64 = 0.5; // f_hat is scaled to fill this fraction of the Y half-range in the ESO plot // ESO output plot colors pub const COLOR_ESO_MEAS: &RGBColor = &LIGHTBLUE; // Measured gyro rate diff --git a/src/eso.rs b/src/eso.rs index 931e4407..8e61f6cd 100644 --- a/src/eso.rs +++ b/src/eso.rs @@ -15,9 +15,10 @@ use std::error::Error; use argmin::core::{CostFunction, Executor}; use argmin::solver::goldensectionsearch::GoldenSectionSearch; +use crate::axis_names::AXIS_COUNT; use crate::constants::{ ESO_B0_MIN_CONTROL_THRESHOLD, ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, - ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, + ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, VALUE_EPSILON, }; use crate::data_input::log_data::LogRowData; @@ -281,9 +282,31 @@ pub fn run_eso_optimization( axis: usize, config: &EsoConfig, ) -> Result> { + if axis >= AXIS_COUNT { + return Err(format!("Invalid axis index {axis}; expected 0..{}", AXIS_COUNT - 1).into()); + } + if !sample_rate.is_finite() || sample_rate <= 0.0 { + return Err(format!("Invalid sample rate: {sample_rate}").into()); + } + if !config.b0.is_finite() + || !config.omega0_min.is_finite() + || !config.omega0_max.is_finite() + || config.omega0_min <= 0.0 + || config.omega0_min >= config.omega0_max + { + return Err("Invalid ESO configuration".into()); + } + let (omega_meas, pid_sum, timestamps) = extract_axis_data(log_data, axis) .ok_or("Insufficient data for ESO optimization (fewer than 2 usable samples)")?; + if !pid_sum + .iter() + .any(|u| u.is_finite() && u.abs() > VALUE_EPSILON) + { + return Err("Insufficient control-input excitation for ESO optimization".into()); + } + // Enforce discrete-time stability: omega_0 < sample_rate / 3 let omega0_max_stable = (sample_rate / 3.0).min(config.omega0_max); if omega0_max_stable <= config.omega0_min { diff --git a/src/main.rs b/src/main.rs index 7327739b..aeeb74b9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1284,6 +1284,7 @@ fn main() -> Result<(), Box> { match args[i + 1].parse::() { Ok(val) if val > 0.0 && val.is_finite() => { plot_config.eso_b0 = val; + plot_config.run_eso = true; i += 1; } _ => { @@ -1333,6 +1334,7 @@ fn main() -> Result<(), Box> { } if any_valid { plot_config.eso_axes = axes; + plot_config.run_eso = true; } i += 1; } else if arg.starts_with("--") { diff --git a/src/plot_functions/plot_eso.rs b/src/plot_functions/plot_eso.rs index fdcbf0f4..fadaa0c7 100644 --- a/src/plot_functions/plot_eso.rs +++ b/src/plot_functions/plot_eso.rs @@ -7,8 +7,8 @@ use std::error::Error; use crate::axis_names::AXIS_NAMES; use crate::constants::{ - COLOR_ESO_FHAT, COLOR_ESO_HAT, COLOR_ESO_MEAS, LINE_WIDTH_PLOT, UNIFIED_Y_AXIS_HEADROOM_SCALE, - UNIFIED_Y_AXIS_MIN_SCALE, UNIFIED_Y_AXIS_PERCENTILE, + COLOR_ESO_FHAT, COLOR_ESO_HAT, COLOR_ESO_MEAS, ESO_FHAT_Y_FRACTION, LINE_WIDTH_PLOT, + UNIFIED_Y_AXIS_HEADROOM_SCALE, UNIFIED_Y_AXIS_MIN_SCALE, UNIFIED_Y_AXIS_PERCENTILE, }; use crate::eso::EsoResult; use crate::plot_framework::{draw_stacked_plot, PlotSeries}; @@ -96,7 +96,8 @@ pub fn plot_eso_output( ]; // Scale f_hat to fit ±50% of the omega Y range for visual interpretability. - if let Some(scale) = data.fhat_scale { + if data.fhat_max_abs > 1e-12 { + let scale = (half_range * ESO_FHAT_Y_FRACTION) / data.fhat_max_abs; let fhat_data: Vec<(f64, f64)> = data.fhat.iter().map(|&(t, f)| (t, f * scale)).collect(); series.push(PlotSeries { @@ -126,8 +127,8 @@ struct AxisEsoData { meas_hat: Vec<(f64, f64, f64)>, /// (time, f_hat_raw) before visual scaling fhat: Vec<(f64, f64)>, - /// Scale mapping f_hat peak to ±50% of UNIFIED_Y_AXIS_MIN_SCALE, or None if all-zero. - fhat_scale: Option, + /// Maximum absolute f_hat value; used to compute scale inside draw_stacked_plot. + fhat_max_abs: f64, } fn build_axis_data(eso: &EsoResult) -> AxisEsoData { @@ -154,17 +155,11 @@ fn build_axis_data(eso: &EsoResult) -> AxisEsoData { let fhat_max_abs = fhat.iter().map(|&(_, f)| f.abs()).fold(0.0_f64, f64::max); - let fhat_scale = if fhat_max_abs > 1e-12 { - Some((UNIFIED_Y_AXIS_MIN_SCALE * 0.5) / fhat_max_abs) - } else { - None - }; - AxisEsoData { omega0_opt: eso.omega0_opt, b0: eso.b0, meas_hat, fhat, - fhat_scale, + fhat_max_abs, } } diff --git a/src/report.rs b/src/report.rs index 55b874f1..b9028116 100644 --- a/src/report.rs +++ b/src/report.rs @@ -65,20 +65,28 @@ fn extract_axis_signals(log_data: &[LogRowData], axis: usize) -> AxisSignals { for row in log_data { if let Some(g) = row.gyro[axis] { - let p = row.p_term[axis].unwrap_or(0.0); - let i_val = row.i_term[axis].unwrap_or(0.0); - let d = row.d_term[axis].unwrap_or(0.0); - let f_val = row.f_term[axis].unwrap_or(0.0); gyro.push(g); + } + if let Some(p) = row.p_term[axis] { pid_p.push(p); + } + if let Some(i_val) = row.i_term[axis] { pid_i.push(i_val); + } + if let Some(d) = row.d_term[axis] { pid_d.push(d); - pid_sum.push(p + i_val + d + f_val); } - if axis < 4 { - if let Some(sp) = row.setpoint[axis] { - setpoint.push(sp); - } + let pid_terms = [ + row.p_term[axis], + row.i_term[axis], + row.d_term[axis], + row.f_term[axis], + ]; + if pid_terms.iter().any(|term| term.is_some()) { + pid_sum.push(pid_terms.iter().map(|term| term.unwrap_or(0.0)).sum()); + } + if let Some(sp) = row.setpoint.get(axis).copied().flatten() { + setpoint.push(sp); } } From 287d515d930d638b169637f4f61cf4f6cd87c7e4 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 24 Apr 2026 14:26:44 -0500 Subject: [PATCH 5/7] Fix CodeRabbitAI review issues from PR#136 (review 4172427843) - src/eso.rs: fix estimate_b0 doc/behavior mismatch and magic numbers * replace 'count < 10' with ESO_B0_MIN_OLS_SAMPLES constant * replace 'den.abs() < 1e-12' with VALUE_EPSILON (already imported) * replace 'b0.abs() > 1e-9' with 'b0 > ESO_B0_ESTIMATE_MIN_POSITIVE' to enforce strictly positive estimates (negative b0 = inverted sign convention, was accepted silently despite docstring saying otherwise) * update docstring to reflect strict positivity requirement - src/eso.rs + src/main.rs: replace fragile float epsilon b0 detection * add b0_user_override: bool to EsoConfig * add eso_b0_user_override: bool to PlotConfig * --eso-b0 CLI flag now sets eso_b0_user_override = true * run_eso_optimization branches on config.b0_user_override instead of (config.b0 - ESO_DEFAULT_B0).abs() < 1e-12 so an explicit '--eso-b0 1.0' is respected rather than silently overridden by OLS - src/constants.rs: add two new constants for estimate_b0 thresholds * ESO_B0_MIN_OLS_SAMPLES: usize = 10 * ESO_B0_ESTIMATE_MIN_POSITIVE: f64 = 1e-9 * add explanatory comment on ESO_OMEGA0_MAX conservative ceiling (links to min(sample_rate/3, config.omega0_max) clamping behavior) - src/report.rs: replace AXIS_NAMES.len().min(3) with AXIS_COUNT * imports AXIS_COUNT from axis_names alongside AXIS_NAMES * eliminates redundant .min(3) magic number - src/main.rs: fix --eso-axis help text and error message * help now lists 'roll,pitch,yaw,all (or 0,1,2)' * error message lists 'roll, pitch, yaw, all, or 0, 1, 2' --- src/constants.rs | 6 ++++++ src/eso.rs | 26 ++++++++++++++++---------- src/main.rs | 11 +++++++++-- src/report.rs | 4 ++-- 4 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 6c2810d2..1c500727 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -261,6 +261,10 @@ pub const PHASE_PLOT_MARGIN_DEG: f64 = 30.0; // Padding above/below phase data f // ESO (Extended State Observer) optimization constants pub const ESO_OMEGA0_MIN: f64 = 50.0; // Lower bound for observer bandwidth search (rad/s) + // Conservative ceiling kept intentionally below sample_rate/3 for typical 1–2 kHz logs. + // At ≥4 kHz the discrete-stability cap (sample_rate/3) would allow ~1300–2660 rad/s, but + // empirical tuning shows gains above ~500 rad/s rarely improve MSE and amplify noise. + // Override per-run with --eso-b0 or raise this if higher bandwidths are needed. pub const ESO_OMEGA0_MAX: f64 = 500.0; // Upper bound for observer bandwidth search (rad/s) pub const ESO_GSS_TOLERANCE: f64 = 0.01; // Golden-section search convergence tolerance (rad/s) pub const ESO_GSS_MAX_ITER: u64 = 100; // Maximum iterations for golden-section search @@ -268,6 +272,8 @@ pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimension pub const ESO_N_AHEAD_STEPS: usize = 5; // Steps ahead for open-loop prediction cost (unimodal objective) pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer spin-up before cost evaluation pub const ESO_B0_MIN_CONTROL_THRESHOLD: f64 = 10.0; // Minimum |PID sum| to include a sample in b0 OLS estimation +pub const ESO_B0_MIN_OLS_SAMPLES: usize = 10; // Minimum high-excitation samples required for OLS b0 estimation +pub const ESO_B0_ESTIMATE_MIN_POSITIVE: f64 = 1e-9; // Minimum strictly-positive b0 to accept (rejects ~0 and negative estimates) pub const ESO_FHAT_Y_FRACTION: f64 = 0.5; // f_hat is scaled to fill this fraction of the Y half-range in the ESO plot // ESO output plot colors diff --git a/src/eso.rs b/src/eso.rs index 8e61f6cd..d71619c6 100644 --- a/src/eso.rs +++ b/src/eso.rs @@ -17,8 +17,9 @@ use argmin::solver::goldensectionsearch::GoldenSectionSearch; use crate::axis_names::AXIS_COUNT; use crate::constants::{ - ESO_B0_MIN_CONTROL_THRESHOLD, ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, - ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, VALUE_EPSILON, + ESO_B0_ESTIMATE_MIN_POSITIVE, ESO_B0_MIN_CONTROL_THRESHOLD, ESO_B0_MIN_OLS_SAMPLES, + ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, + ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, VALUE_EPSILON, }; use crate::data_input::log_data::LogRowData; @@ -27,6 +28,9 @@ use crate::data_input::log_data::LogRowData; pub struct EsoConfig { /// Control effectiveness (scales PID sum to angular acceleration). Default: 1.0. pub b0: f64, + /// True when `b0` was explicitly supplied by the user via `--eso-b0`. + /// When false, `run_eso_optimization` will attempt OLS auto-estimation. + pub b0_user_override: bool, /// Observer bandwidth search lower bound (rad/s). pub omega0_min: f64, /// Observer bandwidth search upper bound (rad/s). @@ -37,6 +41,7 @@ impl Default for EsoConfig { fn default() -> Self { Self { b0: ESO_DEFAULT_B0, + b0_user_override: false, omega0_min: ESO_OMEGA0_MIN, omega0_max: ESO_OMEGA0_MAX, } @@ -208,8 +213,9 @@ impl CostFunction for EsoCostFn<'_> { /// /// Only samples where |u[k]| ≥ ESO_B0_MIN_CONTROL_THRESHOLD are included to avoid /// numerical issues from near-zero control inputs. -/// Returns None when fewer than 10 valid samples are available or when the estimate -/// is non-positive (which would indicate a mis-matched control law). +/// Returns None when fewer than ESO_B0_MIN_OLS_SAMPLES valid samples are available, +/// when the denominator is near-zero, or when the estimate is not strictly positive +/// (a negative estimate indicates an inverted sign convention between u and the gyro axis). fn estimate_b0(omega_meas: &[f64], u: &[f64], ts: f64) -> Option { let n = omega_meas.len().min(u.len()).saturating_sub(1); let mut num = 0.0_f64; @@ -226,11 +232,11 @@ fn estimate_b0(omega_meas: &[f64], u: &[f64], ts: f64) -> Option { count += 1; } - if count < 10 || den.abs() < 1e-12 { + if count < ESO_B0_MIN_OLS_SAMPLES || den.abs() < VALUE_EPSILON { return None; } let b0 = num / den; - if b0.is_finite() && b0.abs() > 1e-9 { + if b0.is_finite() && b0 > ESO_B0_ESTIMATE_MIN_POSITIVE { Some(b0) } else { None @@ -321,14 +327,14 @@ pub fn run_eso_optimization( let ts = 1.0 / sample_rate; // Stage 1: estimate b0 from data via OLS on rate derivatives (QuickFlash guidance). - // If the user explicitly provided a non-default b0 via --eso-b0, respect it. - let (b0, b0_auto) = if (config.b0 - ESO_DEFAULT_B0).abs() < 1e-12 { + // If the user explicitly provided b0 via --eso-b0 (b0_user_override = true), respect it. + let (b0, b0_auto) = if config.b0_user_override { + (config.b0, false) + } else { match estimate_b0(&omega_meas, &pid_sum, ts) { Some(estimated) => (estimated, true), None => (config.b0, false), } - } else { - (config.b0, false) }; let problem = EsoCostFn { diff --git a/src/main.rs b/src/main.rs index aeeb74b9..d7bec9f6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -51,6 +51,7 @@ struct PlotConfig { pub run_eso: bool, pub run_report: bool, pub eso_b0: f64, + pub eso_b0_user_override: bool, pub eso_axes: [bool; 3], } @@ -75,6 +76,7 @@ impl Default for PlotConfig { run_eso: false, run_report: false, eso_b0: crate::constants::ESO_DEFAULT_B0, + eso_b0_user_override: false, eso_axes: [true; 3], } } @@ -101,6 +103,7 @@ impl PlotConfig { run_eso: false, run_report: false, eso_b0: crate::constants::ESO_DEFAULT_B0, + eso_b0_user_override: false, eso_axes: [true; 3], } } @@ -382,7 +385,7 @@ fn print_usage_and_exit(program_name: &str) { ); eprintln!(" --eso: Run 2nd-order LESO bandwidth optimization (omega_0) per axis."); eprintln!( - " --eso-axis : Axes to optimize (comma-separated: roll,pitch,yaw). Default: all." + " --eso-axis : Axes to optimize (comma-separated: roll,pitch,yaw,all or 0,1,2). Default: all." ); eprintln!(" --eso-b0 : Control effectiveness b0 for ESO (default: 1.0)."); eprintln!(" --report: Write a markdown statistical report (_report.md)."); @@ -1112,6 +1115,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." if let Some(sr) = sample_rate { let config = eso::EsoConfig { b0: plot_config.eso_b0, + b0_user_override: plot_config.eso_b0_user_override, ..Default::default() }; for (axis_idx, (axis_enabled, eso_slot)) in plot_config @@ -1284,6 +1288,7 @@ fn main() -> Result<(), Box> { match args[i + 1].parse::() { Ok(val) if val > 0.0 && val.is_finite() => { plot_config.eso_b0 = val; + plot_config.eso_b0_user_override = true; plot_config.run_eso = true; i += 1; } @@ -1325,7 +1330,7 @@ fn main() -> Result<(), Box> { } other => { eprintln!( - "Error: Unknown axis '{}'. Use: roll, pitch, yaw, or all.", + "Error: Unknown axis '{}'. Use: roll, pitch, yaw, all, or 0, 1, 2.", other ); print_usage_and_exit(program_name); @@ -1352,11 +1357,13 @@ fn main() -> Result<(), Box> { let saved_run_eso = plot_config.run_eso; let saved_run_report = plot_config.run_report; let saved_eso_b0 = plot_config.eso_b0; + let saved_eso_b0_user_override = plot_config.eso_b0_user_override; let saved_eso_axes = plot_config.eso_axes; plot_config = PlotConfig::none(); plot_config.run_eso = saved_run_eso; plot_config.run_report = saved_run_report; plot_config.eso_b0 = saved_eso_b0; + plot_config.eso_b0_user_override = saved_eso_b0_user_override; plot_config.eso_axes = saved_eso_axes; plot_config.step_response = step_requested; plot_config.motor_spectrums = motor_requested; diff --git a/src/report.rs b/src/report.rs index b9028116..cff88fa5 100644 --- a/src/report.rs +++ b/src/report.rs @@ -7,7 +7,7 @@ use std::fmt::Write; use std::fs; use std::path::Path; -use crate::axis_names::AXIS_NAMES; +use crate::axis_names::{AXIS_COUNT, AXIS_NAMES}; use crate::data_input::log_data::LogRowData; use crate::eso::EsoResult; @@ -181,7 +181,7 @@ pub fn generate_markdown_report( writeln!(md, "## Per-Axis Signal Statistics")?; writeln!(md)?; - for axis_idx in 0..AXIS_NAMES.len().min(3) { + for axis_idx in 0..AXIS_COUNT { let axis_name = AXIS_NAMES[axis_idx]; let sigs = extract_axis_signals(log_data, axis_idx); From 9c4f38e6e11283e939cda22c7012a33111535e47 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 24 Apr 2026 14:48:58 -0500 Subject: [PATCH 6/7] Fix CodeRabbitAI review nitpicks from PR#136 (review 4172945541) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - src/constants.rs: add ESO_OMEGA0_STABILITY_RATIO = 3.0 * named constant for the LESO discrete-time stability divisor (omega_0 < sample_rate / ESO_OMEGA0_STABILITY_RATIO) - src/eso.rs: replace hardcoded 3.0 stability divisor with constant * omega0_max_stable = (sample_rate / ESO_OMEGA0_STABILITY_RATIO).min(...) * error threshold = config.omega0_min * ESO_OMEGA0_STABILITY_RATIO * import ESO_OMEGA0_STABILITY_RATIO from constants - src/main.rs: replace all [bool; 3] / [true; 3] / [false; 3] with AXIS_COUNT * PlotConfig::eso_axes: [bool; AXIS_COUNT] * Default / none() initializers: [true; AXIS_COUNT] * --eso-axis parser: [false; AXIS_COUNT] and [true; AXIS_COUNT] for 'all' * eso_results declaration: [Option; AXIS_COUNT] * import crate::axis_names::AXIS_COUNT at top level - src/main.rs: replace fragile save/restore with PlotConfig::disable_plots() * add disable_plots(&mut self) method that zeroes all plot-type flags but leaves run_eso, run_report, eso_b0, eso_b0_user_override, eso_axes untouched — prevents future ESO field additions from silently regressing * remove now-unused PlotConfig::none() (was only called in the save/restore block) * replace the 9-line save/restore block with plot_config.disable_plots() - src/report.rs: change eso_results parameter type from [_; 3] to [_; AXIS_COUNT] - src/report.rs: document population-variance convention on compute_signal_stats * add doc comment stating N (not N-1) divisor is intentional for a complete time-series and showing how to switch to sample variance --- src/constants.rs | 1 + src/eso.rs | 8 +++--- src/main.rs | 65 +++++++++++++++++++----------------------------- src/report.rs | 6 ++++- 4 files changed, 35 insertions(+), 45 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index 1c500727..e148127c 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -274,6 +274,7 @@ pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer pub const ESO_B0_MIN_CONTROL_THRESHOLD: f64 = 10.0; // Minimum |PID sum| to include a sample in b0 OLS estimation pub const ESO_B0_MIN_OLS_SAMPLES: usize = 10; // Minimum high-excitation samples required for OLS b0 estimation pub const ESO_B0_ESTIMATE_MIN_POSITIVE: f64 = 1e-9; // Minimum strictly-positive b0 to accept (rejects ~0 and negative estimates) +pub const ESO_OMEGA0_STABILITY_RATIO: f64 = 3.0; // LESO discrete-time stability divisor: omega_0 < sample_rate / ESO_OMEGA0_STABILITY_RATIO pub const ESO_FHAT_Y_FRACTION: f64 = 0.5; // f_hat is scaled to fill this fraction of the Y half-range in the ESO plot // ESO output plot colors diff --git a/src/eso.rs b/src/eso.rs index d71619c6..707875c6 100644 --- a/src/eso.rs +++ b/src/eso.rs @@ -19,7 +19,7 @@ use crate::axis_names::AXIS_COUNT; use crate::constants::{ ESO_B0_ESTIMATE_MIN_POSITIVE, ESO_B0_MIN_CONTROL_THRESHOLD, ESO_B0_MIN_OLS_SAMPLES, ESO_DEFAULT_B0, ESO_GSS_MAX_ITER, ESO_GSS_TOLERANCE, ESO_N_AHEAD_STEPS, ESO_OMEGA0_MAX, - ESO_OMEGA0_MIN, ESO_WARMUP_FRACTION, VALUE_EPSILON, + ESO_OMEGA0_MIN, ESO_OMEGA0_STABILITY_RATIO, ESO_WARMUP_FRACTION, VALUE_EPSILON, }; use crate::data_input::log_data::LogRowData; @@ -313,13 +313,13 @@ pub fn run_eso_optimization( return Err("Insufficient control-input excitation for ESO optimization".into()); } - // Enforce discrete-time stability: omega_0 < sample_rate / 3 - let omega0_max_stable = (sample_rate / 3.0).min(config.omega0_max); + // Enforce discrete-time stability: omega_0 < sample_rate / ESO_OMEGA0_STABILITY_RATIO + let omega0_max_stable = (sample_rate / ESO_OMEGA0_STABILITY_RATIO).min(config.omega0_max); if omega0_max_stable <= config.omega0_min { return Err(format!( "Sample rate {:.1} Hz too low for ESO search (need > {:.1} Hz)", sample_rate, - config.omega0_min * 3.0 + config.omega0_min * ESO_OMEGA0_STABILITY_RATIO ) .into()); } diff --git a/src/main.rs b/src/main.rs index d7bec9f6..a7e40f60 100644 --- a/src/main.rs +++ b/src/main.rs @@ -21,6 +21,7 @@ use std::path::{Path, PathBuf}; use ndarray::Array1; +use crate::axis_names::AXIS_COUNT; use crate::types::StepResponseResults; // Build version string from git info with fallbacks for builds without vergen metadata @@ -52,7 +53,7 @@ struct PlotConfig { pub run_report: bool, pub eso_b0: f64, pub eso_b0_user_override: bool, - pub eso_axes: [bool; 3], + pub eso_axes: [bool; AXIS_COUNT], } impl Default for PlotConfig { @@ -77,35 +78,30 @@ impl Default for PlotConfig { run_report: false, eso_b0: crate::constants::ESO_DEFAULT_B0, eso_b0_user_override: false, - eso_axes: [true; 3], + eso_axes: [true; AXIS_COUNT], } } } impl PlotConfig { - fn none() -> Self { - Self { - step_response: false, - pidsum_error_setpoint: false, - setpoint_vs_gyro: false, - setpoint_derivative: false, - gyro_vs_unfilt: false, - gyro_spectrums: false, - d_term_psd: false, - d_term_spectrums: false, - psd: false, - psd_db_heatmap: false, - throttle_freq_heatmap: false, - d_term_heatmap: false, - motor_spectrums: false, - bode: false, - pid_activity: false, - run_eso: false, - run_report: false, - eso_b0: crate::constants::ESO_DEFAULT_B0, - eso_b0_user_override: false, - eso_axes: [true; 3], - } + /// Reset all plot-type flags to `false` while preserving ESO and report settings + /// (`run_eso`, `run_report`, `eso_b0`, `eso_b0_user_override`, `eso_axes`). + fn disable_plots(&mut self) { + self.step_response = false; + self.pidsum_error_setpoint = false; + self.setpoint_vs_gyro = false; + self.setpoint_derivative = false; + self.gyro_vs_unfilt = false; + self.gyro_spectrums = false; + self.d_term_psd = false; + self.d_term_spectrums = false; + self.psd = false; + self.psd_db_heatmap = false; + self.throttle_freq_heatmap = false; + self.d_term_heatmap = false; + self.motor_spectrums = false; + self.bode = false; + self.pid_activity = false; } } @@ -1109,7 +1105,7 @@ INFO ({input_file_str}): Skipping Step Response input data filtering: {reason}." // CWD restoration happens automatically when _cwd_guard goes out of scope // --- ESO Gain Optimization --- - let mut eso_results: [Option; 3] = [None, None, None]; + let mut eso_results: [Option; AXIS_COUNT] = [None, None, None]; if plot_config.run_eso { println!("\n--- ESO Gain Optimization (2nd-order LESO) ---"); if let Some(sr) = sample_rate { @@ -1308,7 +1304,7 @@ fn main() -> Result<(), Box> { print_usage_and_exit(program_name); } let val = args[i + 1].to_ascii_lowercase(); - let mut axes = [false; 3]; + let mut axes = [false; AXIS_COUNT]; let mut any_valid = false; for part in val.split(',') { match part.trim() { @@ -1325,7 +1321,7 @@ fn main() -> Result<(), Box> { any_valid = true; } "all" => { - axes = [true; 3]; + axes = [true; AXIS_COUNT]; any_valid = true; } other => { @@ -1353,18 +1349,7 @@ fn main() -> Result<(), Box> { // Apply "only" flags if any were specified (non-mutually exclusive: OR together) if has_only_flags { - // Preserve ESO/report settings through the plot-type reset - let saved_run_eso = plot_config.run_eso; - let saved_run_report = plot_config.run_report; - let saved_eso_b0 = plot_config.eso_b0; - let saved_eso_b0_user_override = plot_config.eso_b0_user_override; - let saved_eso_axes = plot_config.eso_axes; - plot_config = PlotConfig::none(); - plot_config.run_eso = saved_run_eso; - plot_config.run_report = saved_run_report; - plot_config.eso_b0 = saved_eso_b0; - plot_config.eso_b0_user_override = saved_eso_b0_user_override; - plot_config.eso_axes = saved_eso_axes; + plot_config.disable_plots(); plot_config.step_response = step_requested; plot_config.motor_spectrums = motor_requested; plot_config.bode = bode_requested; diff --git a/src/report.rs b/src/report.rs index cff88fa5..a46d1869 100644 --- a/src/report.rs +++ b/src/report.rs @@ -24,6 +24,10 @@ pub struct SignalStats { /// Compute descriptive statistics for a slice of f64 values. /// Returns None if the slice is empty. +/// +/// **Variance convention:** population variance (divide by N), not sample variance (N-1). +/// This is appropriate for a complete recorded time-series rather than a statistical sample. +/// To switch to sample std-dev, change the divisor to `(count - 1)` and guard for `count < 2`. pub fn compute_signal_stats(data: &[f64]) -> Option { let count = data.len(); if count == 0 { @@ -121,7 +125,7 @@ pub fn generate_markdown_report( sample_rate: Option, header_metadata: &[(String, String)], output_path: &Path, - eso_results: &[Option; 3], + eso_results: &[Option; AXIS_COUNT], ) -> Result<(), Box> { let mut md = String::new(); From 0d8fe0bdb7a73d3d92eeac37a0af2854f6b09111 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 11 May 2026 15:36:13 -0500 Subject: [PATCH 7/7] fix: resolve remaining PR#136 nitpicks - --eso-axis error message now matches help text (roll,pitch,yaw,all or 0,1,2) - add duplicate-arg detection for --eso-b0 and --eso-axis (mirrors --dps pattern) - remove unreachable any_valid guard; assignment is now unconditional - rename p95_idx -> pctl_idx in plot_eso.rs (derives from UNIFIED_Y_AXIS_PERCENTILE) - expand ESO_OMEGA0_MAX comment explaining conservative ~80 Hz ceiling --- src/constants.rs | 2 +- src/main.rs | 25 +++++++++++++++---------- src/plot_functions/plot_eso.rs | 4 ++-- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/constants.rs b/src/constants.rs index e148127c..6bd018c3 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -265,7 +265,7 @@ pub const ESO_OMEGA0_MIN: f64 = 50.0; // Lower bound for observer bandwidth sear // At ≥4 kHz the discrete-stability cap (sample_rate/3) would allow ~1300–2660 rad/s, but // empirical tuning shows gains above ~500 rad/s rarely improve MSE and amplify noise. // Override per-run with --eso-b0 or raise this if higher bandwidths are needed. -pub const ESO_OMEGA0_MAX: f64 = 500.0; // Upper bound for observer bandwidth search (rad/s) +pub const ESO_OMEGA0_MAX: f64 = 500.0; // Upper bound for observer bandwidth search (rad/s); conservative ceiling (~80 Hz) — even at high loop rates where sample_rate/3 would allow more, this cap avoids instability in noisy logs pub const ESO_GSS_TOLERANCE: f64 = 0.01; // Golden-section search convergence tolerance (rad/s) pub const ESO_GSS_MAX_ITER: u64 = 100; // Maximum iterations for golden-section search pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimensionless) diff --git a/src/main.rs b/src/main.rs index a7e40f60..654bf16b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1194,6 +1194,8 @@ fn main() -> Result<(), Box> { let mut input_paths: Vec = Vec::new(); let mut setpoint_threshold_override: Option = None; let mut dps_flag_present = false; + let mut eso_b0_flag_present = false; + let mut eso_axis_flag_present = false; let mut output_dir: Option = None; // None = not specified (use source folder), Some(dir) = --output-dir with value let mut debug_mode = false; let mut show_butterworth = false; @@ -1277,6 +1279,10 @@ fn main() -> Result<(), Box> { } else if arg == "--report" { plot_config.run_report = true; } else if arg == "--eso-b0" { + if eso_b0_flag_present { + eprintln!("Error: --eso-b0 argument specified more than once."); + print_usage_and_exit(program_name); + } if i + 1 >= args.len() { eprintln!("Error: --eso-b0 requires a numeric value."); print_usage_and_exit(program_name); @@ -1286,6 +1292,7 @@ fn main() -> Result<(), Box> { plot_config.eso_b0 = val; plot_config.eso_b0_user_override = true; plot_config.run_eso = true; + eso_b0_flag_present = true; i += 1; } _ => { @@ -1297,32 +1304,31 @@ fn main() -> Result<(), Box> { } } } else if arg == "--eso-axis" { + if eso_axis_flag_present { + eprintln!("Error: --eso-axis argument specified more than once."); + print_usage_and_exit(program_name); + } if i + 1 >= args.len() { eprintln!( - "Error: --eso-axis requires a comma-separated list of axes (roll,pitch,yaw)." + "Error: --eso-axis requires a comma-separated list of axes (roll,pitch,yaw,all or 0,1,2)." ); print_usage_and_exit(program_name); } let val = args[i + 1].to_ascii_lowercase(); let mut axes = [false; AXIS_COUNT]; - let mut any_valid = false; for part in val.split(',') { match part.trim() { "roll" | "0" => { axes[0] = true; - any_valid = true; } "pitch" | "1" => { axes[1] = true; - any_valid = true; } "yaw" | "2" => { axes[2] = true; - any_valid = true; } "all" => { axes = [true; AXIS_COUNT]; - any_valid = true; } other => { eprintln!( @@ -1333,10 +1339,9 @@ fn main() -> Result<(), Box> { } } } - if any_valid { - plot_config.eso_axes = axes; - plot_config.run_eso = true; - } + plot_config.eso_axes = axes; + plot_config.run_eso = true; + eso_axis_flag_present = true; i += 1; } else if arg.starts_with("--") { eprintln!("Error: Unknown option '{arg}'"); diff --git a/src/plot_functions/plot_eso.rs b/src/plot_functions/plot_eso.rs index fadaa0c7..06b37e6e 100644 --- a/src/plot_functions/plot_eso.rs +++ b/src/plot_functions/plot_eso.rs @@ -50,8 +50,8 @@ pub fn plot_eso_output( } let half_range = if !all_abs.is_empty() { all_abs.sort_by(|a, b| a.total_cmp(b)); - let p95_idx = ((all_abs.len() - 1) as f64 * UNIFIED_Y_AXIS_PERCENTILE).floor() as usize; - (all_abs[p95_idx] * UNIFIED_Y_AXIS_HEADROOM_SCALE).max(UNIFIED_Y_AXIS_MIN_SCALE) + let pctl_idx = ((all_abs.len() - 1) as f64 * UNIFIED_Y_AXIS_PERCENTILE).floor() as usize; + (all_abs[pctl_idx] * UNIFIED_Y_AXIS_HEADROOM_SCALE).max(UNIFIED_Y_AXIS_MIN_SCALE) } else { UNIFIED_Y_AXIS_MIN_SCALE };