diff --git a/Cargo.lock b/Cargo.lock index a3ffed2..ba963de 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -7,14 +7,13 @@ name = "BlackBox_CSV_Render" version = "0.1.0" dependencies = [ "anyhow", - "argmin", "colorous", "csv", "ndarray", "ndarray-stats", "num-complex", "plotters", - "rand 0.8.5", + "rand", "realfft", "rusttype", "vergen", @@ -53,37 +52,6 @@ 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" @@ -399,18 +367,6 @@ 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" @@ -595,7 +551,7 @@ dependencies = [ "noisy_float", "num-integer", "num-traits", - "rand 0.8.5", + "rand", ] [[package]] @@ -670,12 +626,6 @@ 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" @@ -802,12 +752,6 @@ 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" @@ -815,18 +759,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" dependencies = [ "libc", - "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", + "rand_chacha", + "rand_core", ] [[package]] @@ -836,17 +770,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" dependencies = [ "ppv-lite86", - "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", + "rand_core", ] [[package]] @@ -855,25 +779,7 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" dependencies = [ - "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", + "getrandom", ] [[package]] @@ -897,7 +803,7 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dd6f9d3d47bdd2ad6945c5015a226ec6155d0bcdfd8f7cd29f86b71f8de99d2b" dependencies = [ - "getrandom 0.2.16", + "getrandom", "libredox", "thiserror", ] @@ -1127,15 +1033,6 @@ 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" @@ -1204,16 +1101,6 @@ 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" @@ -1456,12 +1343,6 @@ 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 8e06309..97a2c7c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,6 @@ 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/OVERVIEW.md b/OVERVIEW.md index 969a960..6c5d412 100644 --- a/OVERVIEW.md +++ b/OVERVIEW.md @@ -9,7 +9,6 @@ - [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) - [Optimal P Estimation (Optional, Experimental)](#optimal-p-estimation-optional-experimental) - [Step-Response Comparison with Other Analysis Tools](#step-response-comparison-with-other-analysis-tools) - [Compared to PIDtoolbox/Matlab (PTstepcalc.m)](#compared-to-pidtoolboxmatlab-ptstepcalcm) @@ -28,7 +27,6 @@ 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-b0 ` sets control effectiveness (default: 1.0). 2. **File Processing (`src/main.rs:process_file`):** * For each input CSV: @@ -165,24 +163,7 @@ 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. -### 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`. 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. -* **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. - - - +### Output and Tuning Recommendations #### Generated PNG Plots @@ -201,8 +182,6 @@ When `--step` flag is not used, all plots below are generated: - **`*_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 - #### P:D Ratio Recommendations The system provides intelligent P:D tuning recommendations based on step-response peak analysis: diff --git a/src/constants.rs b/src/constants.rs index ce9b052..4f2cdba 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -273,29 +273,6 @@ 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) - // 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); 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) -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_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 -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) - // High-frequency noise analysis for P headroom estimation // D-term energy above this frequency threshold indicates noise constraints pub const DTERM_HF_CUTOFF_HZ: f64 = 200.0; // Frequency above which high-frequency noise is measured @@ -393,8 +370,3 @@ pub const TORQUE_PROFILER_P_SCALE: f64 = 100.0; /// Empirically calibrated on a 5" 6S freestyle build (HELIO H7); may need /// adjustment for significantly heavier or lighter aircraft classes. pub const TORQUE_PROFILER_ACHIEVABILITY_FACTOR: f64 = 2.50; - -/// Betaflight/EmuFlight debug_mode value for GYRO_SCALED. -/// Only this mode populates debug[0-2] with raw unfiltered gyro data, -/// making it a valid fallback source for gyroUnfilt. -pub const DEBUG_MODE_GYRO_SCALED: u32 = 6; diff --git a/src/data_input/log_parser.rs b/src/data_input/log_parser.rs index 1f3f29a..d770f55 100644 --- a/src/data_input/log_parser.rs +++ b/src/data_input/log_parser.rs @@ -6,7 +6,6 @@ use std::fs::File; use std::io::{BufRead, BufReader, Seek, SeekFrom}; use std::path::Path; -use crate::constants::DEBUG_MODE_GYRO_SCALED; use crate::data_input::log_data::LogRowData; use crate::types::LogParseResult; @@ -207,7 +206,6 @@ pub fn parse_log_file(input_file_path: &Path, debug_mode: bool) -> LogParseResul let header_indices: Vec>; let mut motor_indices: Vec = Vec::new(); - let using_debug_fallback: bool; // Read CSV header and map target headers to indices. { @@ -444,39 +442,22 @@ pub fn parse_log_file(input_file_path: &Path, debug_mode: bool) -> LogParseResul .into()); } - // Apply debug[0-2] fallback for gyroUnfilt only when debug_mode=GYRO_SCALED (6). - // Other debug modes do not populate debug[0-2] with raw gyro data. - let have_gyro_unfilt = gyro_unfilt_header_found.iter().any(|&found| found); - let have_debug_axes = debug_header_found.iter().take(3).any(|&found| found); - using_debug_fallback = if !have_gyro_unfilt && have_debug_axes { - let debug_mode_val = header_metadata - .iter() - .find(|(k, _)| k == "debug_mode") - .and_then(|(_, v)| v.parse::().ok()); - match debug_mode_val { - Some(DEBUG_MODE_GYRO_SCALED) => { - // GYRO_SCALED: debug[0-2] contains raw unfiltered gyro - println!( - " ⚠️ Using debug[0-2] as gyroUnfilt fallback (debug_mode=GYRO_SCALED)" - ); - true - } - Some(mode) => { - println!( - " ⚠️ Skipping debug[0-2] gyroUnfilt fallback: debug_mode={} is not GYRO_SCALED ({}) — unfiltered gyro analysis unavailable", - mode, DEBUG_MODE_GYRO_SCALED - ); - false - } - None => { - // debug_mode absent from headers — allow but warn - println!(" ⚠️ Using debug[0-2] as gyroUnfilt fallback (debug_mode unknown, assuming GYRO_SCALED)"); - true + // Report debug fallback usage if applicable + let using_debug_fallback = !gyro_unfilt_header_found.iter().any(|&found| found) + && debug_header_found.iter().take(3).any(|&found| found); + + if using_debug_fallback { + println!(" ⚠️ Using debug[0-2] as fallback for gyroUnfilt[0-2]"); + + // Try to report which debug mode is being used + if let Some((_, debug_mode_value)) = + header_metadata.iter().find(|(k, _)| k == "debug_mode") + { + if let Ok(debug_int) = debug_mode_value.parse::() { + println!(" Debug mode value: {}", debug_int); } } - } else { - false - }; + } } // --- Data Reading and Storage --- @@ -566,12 +547,10 @@ pub fn parse_log_file(input_file_path: &Path, debug_mode: bool) -> LogParseResul } // Apply Fallback Logic for gyro_unfilt (debug[0-2] --> gyroUnfilt[0-2]) - // Fallback is only valid when debug_mode=GYRO_SCALED (6); see using_debug_fallback. for axis in 0..crate::axis_names::AXIS_NAMES.len() { current_row_data.gyro_unfilt[axis] = match parsed_gyro_unfilt[axis] { Some(val) => Some(val), - None if using_debug_fallback => parsed_debug[axis], - None => None, + None => parsed_debug[axis], }; } diff --git a/src/eso.rs b/src/eso.rs deleted file mode 100644 index 12b80e9..0000000 --- a/src/eso.rs +++ /dev/null @@ -1,389 +0,0 @@ -// src/eso.rs -// 2nd-order Linear Extended State Observer (LESO) for flight controller blackbox data. -// 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::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_OMEGA0_STABILITY_RATIO, ESO_WARMUP_FRACTION, VALUE_EPSILON, -}; -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, - /// 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). - pub omega0_max: f64, -} - -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, - } - } -} - -/// 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, - /// 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, - /// True when omega0_opt is at the search ceiling (result may not be the true optimum). - pub at_ceiling: bool, - /// Number of samples used in the optimization. - #[allow(dead_code)] - 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. -/// 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 (omega_hat, f_hat) traces. -/// -/// 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, 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 omega_hats = Vec::with_capacity(n); - let mut f_hats = Vec::with_capacity(n); - - for k in 0..n { - 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); - } - (omega_hats, f_hats) -} - -/// 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 (beta1, beta2) = leso2_gains(omega0); - - // 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, - )) - } -} - -/// 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 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; - 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 < ESO_B0_MIN_OLS_SAMPLES || den.abs() < VALUE_EPSILON { - return None; - } - let b0 = num / den; - if b0.is_finite() && b0 > ESO_B0_ESTIMATE_MIN_POSITIVE { - 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. -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] { - 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); - timestamps.push(row.time_sec.unwrap_or(0.0)); - } - } - - if omega_meas.len() < 2 { - return None; - } - Some((omega_meas, pid_sum, timestamps)) -} - -/// 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. -/// * `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> { - 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 / 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 * ESO_OMEGA0_STABILITY_RATIO - ) - .into()); - } - - let ts = 1.0 / sample_rate; - - // Stage 1: estimate b0 from data via OLS on rate derivatives (QuickFlash guidance). - // 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), - } - }; - - 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 at_ceiling = omega0_opt >= omega0_max_stable - ESO_GSS_TOLERANCE; - - 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, - beta1, - beta2, - b0, - b0_auto, - mse, - at_ceiling, - sample_count: omega_meas.len(), - timestamps, - omega_meas_trace: omega_meas, - omega_hat_trace, - f_hat_trace, - }) -} diff --git a/src/lib.rs b/src/lib.rs index 26d6168..6d46635 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -6,7 +6,6 @@ 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; diff --git a/src/main.rs b/src/main.rs index 4fce722..6339372 100644 --- a/src/main.rs +++ b/src/main.rs @@ -5,7 +5,6 @@ mod constants; mod data_analysis; mod data_input; mod debug_mode_lookup; -mod eso; mod font_config; mod pid_context; mod plot_framework; @@ -20,7 +19,6 @@ use std::path::{Path, PathBuf}; use ndarray::Array1; -use crate::axis_names::AXIS_COUNT; use crate::data_analysis::torque_inertia_profiler::{extract_punch_ratios, AircraftProfile}; use crate::types::StepResponseResults; @@ -50,9 +48,6 @@ struct PlotConfig { pub motor_spectrums: bool, pub bode: bool, pub pid_activity: bool, - pub run_eso: bool, - pub eso_b0: f64, - pub eso_b0_user_override: bool, } impl Default for PlotConfig { @@ -74,9 +69,6 @@ impl Default for PlotConfig { motor_spectrums: true, bode: false, pid_activity: false, - run_eso: false, - eso_b0: crate::constants::ESO_DEFAULT_B0, - eso_b0_user_override: false, } } } @@ -99,9 +91,6 @@ impl PlotConfig { motor_spectrums: false, bode: false, pid_activity: false, - run_eso: false, - eso_b0: crate::constants::ESO_DEFAULT_B0, - eso_b0_user_override: false, } } @@ -122,9 +111,6 @@ impl PlotConfig { motor_spectrums: true, bode: false, // Bode requires specialized logs pid_activity: true, - run_eso: false, - eso_b0: crate::constants::ESO_DEFAULT_B0, - eso_b0_user_override: false, } } } @@ -413,8 +399,6 @@ 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-b0 Control effectiveness b0 for ESO (default: 1.0)."); eprintln!(" --estimate-optimal-p [EXPERIMENTAL] Optimal P estimation from throttle-punch"); eprintln!(" dynamics. Requires .headers.csv; skips if absent."); eprintln!(); @@ -1527,53 +1511,6 @@ 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; AXIS_COUNT] = [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, - b0_user_override: plot_config.eso_b0_user_override, - ..Default::default() - }; - for (axis_idx, eso_slot) in eso_results.iter_mut().enumerate() { - 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) => { - let b0_label = if result.b0_auto { - "(estimated)" - } else { - "(user)" - }; - println!( - "[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); - } - Err(e) => eprintln!("[WARN] Skipped: {e}"), - } - } - } 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}"), - } - } - } - println!("--- Finished processing file: {input_file_str} ---"); Ok(()) } @@ -1594,7 +1531,6 @@ 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 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; @@ -1602,9 +1538,6 @@ fn main() -> Result<(), Box> { let mut extended_requested = false; let mut step_requested = false; let mut bode_requested = false; - let mut eso_requested = false; - let mut eso_b0_value: f64 = crate::constants::ESO_DEFAULT_B0; - let mut eso_b0_user_override = false; let mut recursive = false; let mut estimate_optimal_p = false; @@ -1667,33 +1600,6 @@ fn main() -> Result<(), Box> { step_requested = true; } else if arg == "--bode" { bode_requested = true; - } else if arg == "--eso" { - eso_requested = 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); - } - match args[i + 1].parse::() { - Ok(val) if val > 0.0 && val.is_finite() => { - eso_b0_value = val; - eso_b0_user_override = true; - eso_requested = true; - eso_b0_flag_present = true; - i += 1; - } - _ => { - eprintln!( - "Error: --eso-b0 must be a positive finite number: {}", - args[i + 1] - ); - print_usage_and_exit(program_name); - } - } } else if arg == "--estimate-optimal-p" { estimate_optimal_p = true; } else if arg.starts_with("--") { @@ -1711,7 +1617,7 @@ fn main() -> Result<(), Box> { } // Derive plot configuration from flags - let mut plot_config = if extended_requested { + let plot_config = if extended_requested { let mut cfg = PlotConfig::all(); if bode_requested { cfg.bode = true; @@ -1726,21 +1632,15 @@ fn main() -> Result<(), Box> { cfg.bode = true; } cfg - } else if eso_requested && !core_requested { - // --eso alone (no core/extended/step/bode): ESO analysis only, no regular plots - PlotConfig::none() } else { PlotConfig::default() }; - plot_config.run_eso = eso_requested; - plot_config.eso_b0 = eso_b0_value; - plot_config.eso_b0_user_override = eso_b0_user_override; // Show debug information when the runtime --debug flag is present if debug_mode { println!( - "DEBUG: extended={}, step={}, bode={}, eso={}, plot_config={:?}", - extended_requested, step_requested, bode_requested, eso_requested, plot_config + "DEBUG: extended={}, step={}, bode={}, plot_config={:?}", + extended_requested, step_requested, bode_requested, plot_config ); } diff --git a/src/plot_functions/mod.rs b/src/plot_functions/mod.rs index 5e5f888..71ee9e9 100644 --- a/src/plot_functions/mod.rs +++ b/src/plot_functions/mod.rs @@ -5,7 +5,6 @@ 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 deleted file mode 100644 index c34181c..0000000 --- a/src/plot_functions/plot_eso.rs +++ /dev/null @@ -1,170 +0,0 @@ -// 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, 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}; - -/// 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 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 - }; - - 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: { - let ceiling = if data.at_ceiling { " [at ceiling]" } else { "" }; - format!( - "ESO estimate (omega_hat, omega0={:.0} rad/s, b0={:.2}){ceiling}", - 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 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 { - 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, - at_ceiling: bool, - /// (time, omega_meas, omega_hat) triples - meas_hat: Vec<(f64, f64, f64)>, - /// (time, f_hat_raw) before visual scaling - fhat: Vec<(f64, f64)>, - /// Maximum absolute f_hat value; used to compute scale inside draw_stacked_plot. - fhat_max_abs: f64, -} - -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); - - AxisEsoData { - omega0_opt: eso.omega0_opt, - b0: eso.b0, - at_ceiling: eso.at_ceiling, - meas_hat, - fhat, - fhat_max_abs, - } -} diff --git a/src/report.rs b/src/report.rs deleted file mode 100644 index a46d186..0000000 --- a/src/report.rs +++ /dev/null @@ -1,251 +0,0 @@ -// 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_COUNT, 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. -/// -/// **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 { - 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] { - 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); - } - 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); - } - } - - 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; AXIS_COUNT], -) -> 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_COUNT { - 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)?; - 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 is estimated from data via OLS on rate \ - derivatives (QuickFlash method); override with --eso-b0 if needed." - )?; - writeln!(md)?; - } - } - - fs::write(output_path, md)?; - Ok(()) -}