feat: ESO gain optimization (2nd-order LESO, argmin GSS, at_ceiling marker)

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"

OVERVIEW.md

@@ -9,6 +9,7 @@
   - [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)
@@ -27,6 +28,7 @@ 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 <value>` sets control effectiveness (default: 1.0).
 
 2.  **File Processing (`src/main.rs:process_file`):**
     * For each input CSV:
@@ -163,7 +165,24 @@ 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`. Set control effectiveness with `--eso-b0 <value>`.
+* **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.
+
+
+
 
 #### Generated PNG Plots
 

src/constants.rs

@@ -273,6 +273,29 @@ 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

src/data_input/log_parser.rs

@@ -455,6 +455,7 @@ pub fn parse_log_file(input_file_path: &Path, debug_mode: bool) -> LogParseResul
                 .and_then(|(_, v)| v.parse::<u32>().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)"
                     );