Skip to content

Commit f506c57

Browse files
nerdCopterclaude
andauthored
fix: guard debug[0-2] gyroUnfilt fallback to GYRO_SCALED mode only (#157)
* Implement ESO gain optimization and markdown report generation - Add src/eso.rs: 2nd-order LESO bandwidth optimization via golden-section search - Add src/report.rs: Per-axis signal statistics and ESO results in markdown format - Add CLI flags: --eso, --eso-axis, --eso-b0, --report - Add ESO constants to src/constants.rs (omega_0 bounds, GSS tolerance) - Update lib.rs and main.rs to wire modules and implement ESO/report execution - Update OVERVIEW.md with ESO and report sections - Add INFORMATION/ESO_HOWTO.md user guide - Tested on Betaflight 2025.12 and EmuFlight 0.4.3 full flights * eso: replace hand-rolled GSS with argmin, fix cost function, add time-domain plot - Add argmin 0.11 dependency; implement CostFunction trait for ESO bandwidth search - Replace monotone 1-step MSE cost with N-step-ahead open-loop prediction cost (unimodal: low omega0 = stale f_hat; high omega0 = noise amplification) Constants: ESO_N_AHEAD_STEPS=5, ESO_WARMUP_FRACTION=0.20 - EsoResult now carries omega_meas_trace, omega_hat_trace, f_hat_trace, timestamps - Add plot_eso.rs: stacked time-domain ESO output plot per axis (omega_meas blue thin over omega_hat orange thick, f_hat green scaled) - Wire ESO output plot into main.rs after optimization; generated as *_ESO_output_stacked.png - Update report.rs: MSE label clarified to N-step-ahead prediction - Add ESO plot colors to constants.rs (COLOR_ESO_MEAS/HAT/FHAT) - ESO_GSS_MAX_ITER type corrected to u64 for argmin compatibility * eso: auto-estimate b0 via OLS on rate derivatives (QuickFlash method) - estimate_b0(): OLS closed-form b0 = sum(u*d_omega) / (Ts * sum(u^2)) on samples where |PID sum| >= ESO_B0_MIN_CONTROL_THRESHOLD (10.0) - b0 auto-estimated from data when user has not overridden --eso-b0; falls back to ESO_DEFAULT_B0 (1.0) when too few valid samples - EsoResult.b0_auto: true when estimated, false when user-supplied - main.rs: print shows b0=X.XXXX (estimated|user) - report.rs: b0 row labels (auto-estimated|user-supplied); updated guidance - Add ESO_B0_MIN_CONTROL_THRESHOLD = 10.0 to constants.rs - Fix pre-existing clippy collapsible_match in pid_metadata.rs * Fix CodeRabbitAI review issues from PR#136 - OVERVIEW.md: move *_report.md bullet out of 'Generated PNG Plots' into a new 'Generated Reports' subsection (was self-contradictory) - src/eso.rs: add upfront validation in run_eso_optimization * axis >= AXIS_COUNT → descriptive Err instead of panic * non-finite / non-positive sample_rate → Err * invalid EsoConfig fields (omega0_min/max, b0) → Err * add control-input excitation guard: reject axes where pid_sum is all-zero / sub-epsilon so flat-input axes are not 'optimised' - src/report.rs: fix PID term collection in extract_axis_signals * replace unwrap_or(0.0) with per-field Option guards so absent P/I/D/F terms are not silently reported as real zero samples * replace hardcoded 'axis < 4' magic literal with row.setpoint.get(axis).copied().flatten() (bounds-safe) - src/plot_functions/plot_eso.rs + src/constants.rs: * add ESO_FHAT_Y_FRACTION = 0.5 constant (no more magic 0.5) * store fhat_max_abs in AxisEsoData instead of pre-computed scale * compute fhat_scale inside draw_stacked_plot after half_range is known: (half_range * ESO_FHAT_Y_FRACTION) / fhat_max_abs fixes f_hat being squashed when half_range > UNIFIED_Y_AXIS_MIN_SCALE - src/main.rs: --eso-axis and --eso-b0 now imply --eso (previously silently ignored when --eso was omitted) * Fix CodeRabbitAI review issues from PR#136 (review 4172427843) - src/eso.rs: fix estimate_b0 doc/behavior mismatch and magic numbers * replace 'count < 10' with ESO_B0_MIN_OLS_SAMPLES constant * replace 'den.abs() < 1e-12' with VALUE_EPSILON (already imported) * replace 'b0.abs() > 1e-9' with 'b0 > ESO_B0_ESTIMATE_MIN_POSITIVE' to enforce strictly positive estimates (negative b0 = inverted sign convention, was accepted silently despite docstring saying otherwise) * update docstring to reflect strict positivity requirement - src/eso.rs + src/main.rs: replace fragile float epsilon b0 detection * add b0_user_override: bool to EsoConfig * add eso_b0_user_override: bool to PlotConfig * --eso-b0 CLI flag now sets eso_b0_user_override = true * run_eso_optimization branches on config.b0_user_override instead of (config.b0 - ESO_DEFAULT_B0).abs() < 1e-12 so an explicit '--eso-b0 1.0' is respected rather than silently overridden by OLS - src/constants.rs: add two new constants for estimate_b0 thresholds * ESO_B0_MIN_OLS_SAMPLES: usize = 10 * ESO_B0_ESTIMATE_MIN_POSITIVE: f64 = 1e-9 * add explanatory comment on ESO_OMEGA0_MAX conservative ceiling (links to min(sample_rate/3, config.omega0_max) clamping behavior) - src/report.rs: replace AXIS_NAMES.len().min(3) with AXIS_COUNT * imports AXIS_COUNT from axis_names alongside AXIS_NAMES * eliminates redundant .min(3) magic number - src/main.rs: fix --eso-axis help text and error message * help now lists 'roll,pitch,yaw,all (or 0,1,2)' * error message lists 'roll, pitch, yaw, all, or 0, 1, 2' * Fix CodeRabbitAI review nitpicks from PR#136 (review 4172945541) - src/constants.rs: add ESO_OMEGA0_STABILITY_RATIO = 3.0 * named constant for the LESO discrete-time stability divisor (omega_0 < sample_rate / ESO_OMEGA0_STABILITY_RATIO) - src/eso.rs: replace hardcoded 3.0 stability divisor with constant * omega0_max_stable = (sample_rate / ESO_OMEGA0_STABILITY_RATIO).min(...) * error threshold = config.omega0_min * ESO_OMEGA0_STABILITY_RATIO * import ESO_OMEGA0_STABILITY_RATIO from constants - src/main.rs: replace all [bool; 3] / [true; 3] / [false; 3] with AXIS_COUNT * PlotConfig::eso_axes: [bool; AXIS_COUNT] * Default / none() initializers: [true; AXIS_COUNT] * --eso-axis parser: [false; AXIS_COUNT] and [true; AXIS_COUNT] for 'all' * eso_results declaration: [Option<eso::EsoResult>; AXIS_COUNT] * import crate::axis_names::AXIS_COUNT at top level - src/main.rs: replace fragile save/restore with PlotConfig::disable_plots() * add disable_plots(&mut self) method that zeroes all plot-type flags but leaves run_eso, run_report, eso_b0, eso_b0_user_override, eso_axes untouched — prevents future ESO field additions from silently regressing * remove now-unused PlotConfig::none() (was only called in the save/restore block) * replace the 9-line save/restore block with plot_config.disable_plots() - src/report.rs: change eso_results parameter type from [_; 3] to [_; AXIS_COUNT] - src/report.rs: document population-variance convention on compute_signal_stats * add doc comment stating N (not N-1) divisor is intentional for a complete time-series and showing how to switch to sample variance * fix: resolve remaining PR#136 nitpicks - --eso-axis error message now matches help text (roll,pitch,yaw,all or 0,1,2) - add duplicate-arg detection for --eso-b0 and --eso-axis (mirrors --dps pattern) - remove unreachable any_valid guard; assignment is now unconditional - rename p95_idx -> pctl_idx in plot_eso.rs (derives from UNIFIED_Y_AXIS_PERCENTILE) - expand ESO_OMEGA0_MAX comment explaining conservative ~80 Hz ceiling * feat: flag ESO ceiling in legend and result struct Add at_ceiling: bool to EsoResult, set when omega0_opt reaches the stability-constrained search bound (omega0_max_stable - GSS_TOLERANCE). Propagate through AxisEsoData into the plot legend as ' [at ceiling]' so users can see at a glance when the optimizer hit its upper bound rather than finding a true interior optimum. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> * fix: guard debug[0-2] gyroUnfilt fallback to GYRO_SCALED mode only (#154) Previously, debug[0-2] was unconditionally used as a fallback for gyroUnfilt when dedicated gyroUnfilt columns were absent. This produced incorrect unfiltered gyro analysis for any debug_mode other than GYRO_SCALED (6), e.g. RC_SMOOTHING (7) or ACCELEROMETER (4), where debug[0-2] does not contain raw gyro data. Now the fallback is guarded: only proceeds when debug_mode=6, warns and skips otherwise. When debug_mode is absent from headers, the fallback is allowed with an assumption warning. Closes #154 Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> * refactor: named constant for GYRO_SCALED and remove obsolete --eso-axis/--report docs - Add DEBUG_MODE_GYRO_SCALED = 6 to src/constants.rs; use it in log_parser.rs instead of hardcoded magic number 6 (CodeRabbit #157) - Remove --eso-axis and --report flag references from OVERVIEW.md; these flags were previously removed from the CLI but documentation was not updated (CodeRabbit #157) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> --------- Co-authored-by: Claude Sonnet 4.6 <noreply@anthropic.com>
1 parent 3e54f4f commit f506c57

11 files changed

Lines changed: 1128 additions & 26 deletions

File tree

Cargo.lock

Lines changed: 126 additions & 7 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

Cargo.toml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ ndarray = "0.15"
1515
ndarray-stats = "0.5"
1616
colorous = "1.0.16"
1717
rusttype = "0.9"
18+
argmin = "0.11"
1819

1920
[build-dependencies]
2021
anyhow = "1.0"

OVERVIEW.md

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
- [Implementation Details](#implementation-details)
1010
- [Filter Response Curves](#filter-response-curves)
1111
- [Bode Plot Analysis (Optional)](#bode-plot-analysis-optional)
12+
- [ESO Gain Optimization (Optional)](#eso-gain-optimization-optional)
1213
- [Optimal P Estimation (Optional, Experimental)](#optimal-p-estimation-optional-experimental)
1314
- [Step-Response Comparison with Other Analysis Tools](#step-response-comparison-with-other-analysis-tools)
1415
- [Compared to PIDtoolbox/Matlab (PTstepcalc.m)](#compared-to-pidtoolboxmatlab-ptstepcalcm)
@@ -27,6 +28,7 @@ All analysis parameters, thresholds, plot dimensions, and algorithmic constants
2728
* Additional options include `--help` and `--version` for user assistance.
2829
* The `--output-dir` parameter now requires a directory path when specified. If omitted, plots are saved in the source folder (input file's directory).
2930
* 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.
31+
* **ESO flags:** `--eso` enables 2nd-order LESO bandwidth optimization; `--eso-b0 <value>` sets control effectiveness (default: 1.0).
3032

3133
2. **File Processing (`src/main.rs:process_file`):**
3234
* For each input CSV:
@@ -163,7 +165,24 @@ All analysis parameters, thresholds, plot dimensions, and algorithmic constants
163165
* **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.
164166
* **Warning:** A runtime warning is displayed when `--bode` is used to inform users of these requirements and recommend spectrum analysis for normal flights.
165167

166-
### Output and Tuning Recommendations
168+
### ESO Gain Optimization (Optional)
169+
170+
* **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.
171+
* **Activation:** Disabled by default; enable with `--eso`. Set control effectiveness with `--eso-b0 <value>`.
172+
* **Algorithm (`src/eso.rs`):**
173+
* Extracts filtered gyro (omega) and PID sum (P+I+D+F) per axis as measured output and control input respectively.
174+
* Simulates a discrete Euler-forward 2nd-order LESO at each candidate omega_0:
175+
* `e = omega_meas[k] - omega_hat`
176+
* `omega_hat += Ts * (f_hat + b0 * u[k] + beta1 * e)`
177+
* `f_hat += Ts * (beta2 * e)`
178+
* Bandwidth parameterisation (Gao 2003): `beta1 = 2*omega_0`, `beta2 = omega_0^2`.
179+
* Minimises MSE(omega_hat, omega_meas) via golden-section search over `[ESO_OMEGA0_MIN, min(sample_rate/3, ESO_OMEGA0_MAX)]`.
180+
* **Stability constraint:** omega_0 < sample_rate / 3 (enforced automatically).
181+
* **Output:** Prints optimal omega_0, beta1, beta2, and MSE per axis to console.
182+
* **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.
183+
184+
185+
167186

168187
#### Generated PNG Plots
169188

@@ -182,6 +201,8 @@ When `--step` flag is not used, all plots below are generated:
182201
- **`*_Throttle_Freq_Heatmap_comparative.png`** — System noise characteristics across throttle levels and frequencies
183202
- **`*_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).
184203

204+
#### Generated Reports
205+
185206
#### P:D Ratio Recommendations
186207

187208
The system provides intelligent P:D tuning recommendations based on step-response peak analysis:

src/constants.rs

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -273,6 +273,29 @@ pub const PSD_EPSILON: f64 = 1e-12; // Guard against division by zero for PSD va
273273
pub const MAGNITUDE_PLOT_MARGIN_DB: f64 = 10.0; // Padding above/below magnitude data for plot range
274274
pub const PHASE_PLOT_MARGIN_DEG: f64 = 30.0; // Padding above/below phase data for plot range
275275

276+
// ESO (Extended State Observer) optimization constants
277+
pub const ESO_OMEGA0_MIN: f64 = 50.0; // Lower bound for observer bandwidth search (rad/s)
278+
// Conservative ceiling kept intentionally below sample_rate/3 for typical 1–2 kHz logs.
279+
// At ≥4 kHz the discrete-stability cap (sample_rate/3) would allow ~1300–2660 rad/s, but
280+
// empirical tuning shows gains above ~500 rad/s rarely improve MSE and amplify noise.
281+
// Override per-run with --eso-b0 or raise this if higher bandwidths are needed.
282+
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
283+
pub const ESO_GSS_TOLERANCE: f64 = 0.01; // Golden-section search convergence tolerance (rad/s)
284+
pub const ESO_GSS_MAX_ITER: u64 = 100; // Maximum iterations for golden-section search
285+
pub const ESO_DEFAULT_B0: f64 = 1.0; // Default control effectiveness (dimensionless)
286+
pub const ESO_N_AHEAD_STEPS: usize = 5; // Steps ahead for open-loop prediction cost (unimodal objective)
287+
pub const ESO_WARMUP_FRACTION: f64 = 0.20; // Fraction of data used for observer spin-up before cost evaluation
288+
pub const ESO_B0_MIN_CONTROL_THRESHOLD: f64 = 10.0; // Minimum |PID sum| to include a sample in b0 OLS estimation
289+
pub const ESO_B0_MIN_OLS_SAMPLES: usize = 10; // Minimum high-excitation samples required for OLS b0 estimation
290+
pub const ESO_B0_ESTIMATE_MIN_POSITIVE: f64 = 1e-9; // Minimum strictly-positive b0 to accept (rejects ~0 and negative estimates)
291+
pub const ESO_OMEGA0_STABILITY_RATIO: f64 = 3.0; // LESO discrete-time stability divisor: omega_0 < sample_rate / ESO_OMEGA0_STABILITY_RATIO
292+
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
293+
294+
// ESO output plot colors
295+
pub const COLOR_ESO_MEAS: &RGBColor = &LIGHTBLUE; // Measured gyro rate
296+
pub const COLOR_ESO_HAT: &RGBColor = &ORANGE; // ESO estimated rate (omega_hat)
297+
pub const COLOR_ESO_FHAT: &RGBColor = &GREEN; // ESO disturbance estimate (f_hat, scaled)
298+
276299
// High-frequency noise analysis for P headroom estimation
277300
// D-term energy above this frequency threshold indicates noise constraints
278301
pub const DTERM_HF_CUTOFF_HZ: f64 = 200.0; // Frequency above which high-frequency noise is measured
@@ -370,3 +393,8 @@ pub const TORQUE_PROFILER_P_SCALE: f64 = 100.0;
370393
/// Empirically calibrated on a 5" 6S freestyle build (HELIO H7); may need
371394
/// adjustment for significantly heavier or lighter aircraft classes.
372395
pub const TORQUE_PROFILER_ACHIEVABILITY_FACTOR: f64 = 2.50;
396+
397+
/// Betaflight/EmuFlight debug_mode value for GYRO_SCALED.
398+
/// Only this mode populates debug[0-2] with raw unfiltered gyro data,
399+
/// making it a valid fallback source for gyroUnfilt.
400+
pub const DEBUG_MODE_GYRO_SCALED: u32 = 6;

0 commit comments

Comments
 (0)