diff --git a/Cargo.lock b/Cargo.lock index ba963de..a3ffed2 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 97a2c7c..8e06309 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/OVERVIEW.md b/OVERVIEW.md index 6d64b2e..61c4097 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 @@ -176,6 +205,10 @@ 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 + +- **`*_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 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 6f1f041..6bd018c 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -259,4 +259,27 @@ 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) + // src/constants.rs diff --git a/src/eso.rs b/src/eso.rs new file mode 100644 index 0000000..707875c --- /dev/null +++ b/src/eso.rs @@ -0,0 +1,384 @@ +// 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, + /// 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. +/// 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 (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, + 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 6d46635..f241aa9 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 ec5a4a0..654bf16 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; @@ -19,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 @@ -46,6 +49,11 @@ 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_b0_user_override: bool, + pub eso_axes: [bool; AXIS_COUNT], } impl Default for PlotConfig { @@ -66,29 +74,34 @@ 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_b0_user_override: false, + 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, - } + /// 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; } } @@ -366,6 +379,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,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)."); eprintln!(); eprintln!("=== GENERAL ==="); eprintln!(); @@ -1084,6 +1103,77 @@ 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, (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) => { + 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}"), + } + } + } + + // --- 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(()) } @@ -1104,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; @@ -1182,6 +1274,75 @@ 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 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() => { + plot_config.eso_b0 = val; + plot_config.eso_b0_user_override = true; + plot_config.run_eso = 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 == "--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,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]; + for part in val.split(',') { + match part.trim() { + "roll" | "0" => { + axes[0] = true; + } + "pitch" | "1" => { + axes[1] = true; + } + "yaw" | "2" => { + axes[2] = true; + } + "all" => { + axes = [true; AXIS_COUNT]; + } + other => { + eprintln!( + "Error: Unknown axis '{}'. Use: roll, pitch, yaw, all, or 0, 1, 2.", + other + ); + print_usage_and_exit(program_name); + } + } + } + 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}'"); print_usage_and_exit(program_name); @@ -1193,7 +1354,7 @@ fn main() -> Result<(), Box> { // Apply "only" flags if any were specified (non-mutually exclusive: OR together) if has_only_flags { - plot_config = PlotConfig::none(); + 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/plot_functions/mod.rs b/src/plot_functions/mod.rs index 71ee9e9..5e5f888 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 0000000..06b37e6 --- /dev/null +++ b/src/plot_functions/plot_eso.rs @@ -0,0 +1,165 @@ +// 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: 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 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, + /// (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, + meas_hat, + fhat, + fhat_max_abs, + } +} diff --git a/src/report.rs b/src/report.rs new file mode 100644 index 0000000..a46d186 --- /dev/null +++ b/src/report.rs @@ -0,0 +1,251 @@ +// 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(()) +}