From 850d4e2da321ea4af4110ed1c0e9668177d4c6d6 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 31 Mar 2026 22:39:10 -0800 Subject: [PATCH] feat(docs): baro throttle compensation --- Tools/baro_compensation/README.md | 39 ++++++ docs/en/SUMMARY.md | 1 + .../barometer_thrust_compensation.md | 116 ++++++++++++++++++ docs/en/advanced_config/index.md | 1 + .../baro_thrust_cf_rls.cpp | 4 +- 5 files changed, 159 insertions(+), 2 deletions(-) create mode 100644 docs/en/advanced_config/barometer_thrust_compensation.md diff --git a/Tools/baro_compensation/README.md b/Tools/baro_compensation/README.md index cc5ae9ee18..fcee86e054 100644 --- a/Tools/baro_compensation/README.md +++ b/Tools/baro_compensation/README.md @@ -110,6 +110,11 @@ vs offline correction applied. **Online vs Offline Scatter** — Error vs thrust scatter for raw, online- corrected, and offline-corrected data. +**CF Bandwidth Sensitivity** (range sensor required) — Sweeps `SENS_BAR_CF_BW` +to show how the CF crossover frequency affects K identification and +compensation quality. See [CF Bandwidth Tuning](#cf-bandwidth-tuning) for +how to interpret this page. + ## Manual Calibration Procedure If not using the online estimator, you can calibrate manually: @@ -145,9 +150,43 @@ If not using the online estimator, you can calibrate manually: - **Online/offline K disagreement > 2 m**: The estimator may not have had enough excitation. Fly longer or with more altitude variation. +## CF Bandwidth Tuning + +The default `SENS_BAR_CF_BW` (0.1 Hz) works well for most vehicles. Only +adjust it if the online estimator consistently fails to converge or produces +a K that disagrees with range-sensor ground truth. + +When a range sensor is present, the tool generates a **CF Bandwidth +Sensitivity** page with two panels: + +**Left — K vs Bandwidth**: Shows how the identified K changes with CF +crossover frequency. The green dashed line is the range-sensor ground truth K. + +- If the curve is **flat near ground truth** around the default (gray line): + the default bandwidth is fine, K identification is robust. +- If the curve **crosses ground truth far from the default**: the default + bandwidth is producing a biased K. Consider setting `SENS_BAR_CF_BW` to + the "Best K match" bandwidth (blue dashed line). + +**Right — Compensated Error Std vs Bandwidth**: Shows the residual baro +error standard deviation after applying the K identified at each bandwidth. +Lower is better. + +- The green line is the theoretical minimum (range-sensor optimal PCOEF). +- The orange line is the current PCOEF performance. +- If the default bandwidth is already near the minimum: leave it alone. +- If a different bandwidth gives significantly lower error std: consider + changing `SENS_BAR_CF_BW` to that value. + +In practice, the default is conservative and works across vehicle types. +Raising the bandwidth makes K identification faster and more accurate when +the IMU is good, but noisier when it's not. Lower bandwidth is more robust +to IMU vibration but slower to converge. + ## Parameters Reference | Parameter | Description | Range | Default | |-----------|-------------|-------|---------| | `SENS_BARO_PCOEF` | Baro altitude correction per unit vertical thrust [m] | -30 to 30 | 0.0 | | `SENS_BAR_AUTOCAL` | Bitmask: bit 0 = GNSS offset, bit 1 = online thrust cal | 0 to 3 | 1 | +| `SENS_BAR_CF_BW` | CF crossover frequency for the online estimator [Hz] | 0.01 to 1.0 | 0.1 | diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 6b842e17fd..7a95096809 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -228,6 +228,7 @@ - [Airspeed Validation](advanced_config/airspeed_validation.md) - [TFSlot Airspeed Sensor](sensor/airspeed_tfslot.md) - [Barometers](sensor/barometer.md) + - [Thrust Compensation](advanced_config/barometer_thrust_compensation.md) - [Distance Sensors \(Rangefinders\)](sensor/rangefinders.md) - [Ainstein US-D1 Standard Radar Altimeter](sensor/ulanding_radar.md) - [ARK DIST SR (CAN/UART)](dronecan/ark_dist.md) diff --git a/docs/en/advanced_config/barometer_thrust_compensation.md b/docs/en/advanced_config/barometer_thrust_compensation.md new file mode 100644 index 0000000000..c914a23376 --- /dev/null +++ b/docs/en/advanced_config/barometer_thrust_compensation.md @@ -0,0 +1,116 @@ +# Barometer Thrust Compensation + +Propellers change the static pressure at the barometer sensor proportional to motor output. +This creates a thrust-dependent altitude error that can reach several metres on small vehicles with the barometer close to the propellers. +The direction and magnitude depend on sensor placement relative to the propellers. + +PX4 can compensate for this error by applying a correction proportional to the vertical thrust setpoint: + +``` +corrected_baro_alt = raw_baro_alt + SENS_BARO_PCOEF * |thrust_z| +``` + +where `thrust_z` is the Z body-axis component of `vehicle_thrust_setpoint` (negative for upward thrust in FRD frame). + +The compensation parameter [SENS_BARO_PCOEF](../advanced_config/parameter_reference.md#SENS_BARO_PCOEF) can be identified automatically during flight or manually from a flight log. + +::: info +This feature compensates for _propwash-induced_ pressure error, which depends on motor output. +For _airspeed-induced_ static pressure error (due to vehicle forward motion), see [Static Pressure Buildup](../advanced_config/static_pressure_buildup.md). +::: + +## Online Calibration (Recommended) + +The online estimator identifies `SENS_BARO_PCOEF` automatically during flight and saves the result on disarm. + +### How It Works + +A complementary filter (CF) fuses barometer altitude with double-integrated accelerometer data at a very low crossover frequency (default 0.1 Hz). +The CF trusts the accelerometer for fast altitude changes and the barometer for slow drift, so the CF residual (baro minus accel prediction) isolates thrust-correlated pressure error while rejecting real altitude changes. + +A Recursive Least Squares (RLS) estimator then fits the linear model `residual = K * thrust + bias` to identify the gain K. +Once the estimate converges (stable K, low variance, sufficient thrust excitation), K is locked and saved to `SENS_BARO_PCOEF` on disarm. + +The estimator refines the parameter over subsequent flights — each flight corrects for whatever residual error remains after the previous calibration. + +### Setup + +1. Set [SENS_BAR_AUTOCAL](../advanced_config/parameter_reference.md#SENS_BAR_AUTOCAL) to **3** (enables both GNSS altitude calibration and thrust compensation). +2. Fly normally for at least 60 seconds with some altitude variation. +3. On disarm, the estimated `SENS_BARO_PCOEF` is saved automatically if the estimator converged. +4. Check convergence after flight: + - In the console: `baro_thrust_estimator status` + - In a log: look at the `baro_thrust_estimate` topic — `converged` should be true. + +The estimator uses several convergence gates before saving: + +| Gate | Threshold | Purpose | +|------|-----------|---------| +| Minimum flight time | 30 s | Allow RLS to settle | +| K variance (P[0][0]) | < 3.0 | Parameter estimate is precise | +| Prediction error | Low absolute or relative | Model fits the data | +| Thrust excitation | std > 0.05 | Enough signal to identify K | +| K stability | Stable for 10 s | Estimate is not drifting | +| Hold time | 10 s | Convergence is sustained | + +::: tip +Altitude changes during hover provide the thrust excitation the estimator needs. +Constant-thrust hover with no altitude variation will not converge. +::: + +### Parameters + +| Parameter | Default | Description | +|-----------|---------|-------------| +| [SENS_BARO_PCOEF](../advanced_config/parameter_reference.md#SENS_BARO_PCOEF) | 0.0 | Baro altitude correction per unit vertical thrust \[m\]. Identified by the estimator or set manually. | +| [SENS_BAR_AUTOCAL](../advanced_config/parameter_reference.md#SENS_BAR_AUTOCAL) | 1 | Bitmask: bit 0 = GNSS altitude offset, bit 1 = online thrust compensation. Set to 3 for both. | +| [SENS_BAR_CF_BW](../advanced_config/parameter_reference.md#SENS_BAR_CF_BW) | 0.1 | CF crossover frequency \[Hz\]. Lower = more conservative, higher = faster identification but noisier. | + +### Soft Guards + +The estimator pauses RLS updates (while keeping the CF running) when: + +- Vertical speed exceeds 2 m/s +- Horizontal speed exceeds 5 m/s + +This prevents high-speed flight dynamics from corrupting the estimate. +Once converged, the RLS is frozen entirely to prevent ground-effect contamination during landing. + +## Manual Calibration + +If you prefer not to use the online estimator, you can identify `SENS_BARO_PCOEF` from a flight log using a range sensor as ground truth. + +1. Set `SENS_BARO_PCOEF` to 0 (disable existing compensation). +2. Fly a hover at 2-5 m AGL for at least 60 seconds with gentle altitude changes. A downward-facing range sensor must be installed. +3. Run the analysis script on the log: + ```sh + python3 Tools/baro_compensation/baro_thrust_calibration.py + ``` +4. Apply the recommended `SENS_BARO_PCOEF` value. +5. Fly again and re-run the script to verify the compensation. + +The script automatically selects an analysis mode based on available data: + +| Mode | Data Required | Output | +|------|--------------|--------| +| Estimator review | Online estimator logged | K convergence, residual analysis | +| Full validation | Estimator + range sensor | Cross-validation of online vs offline K | +| Standalone calibration | Range sensor only | Recommended PCOEF from least-squares fit | + +## Interpreting Results + +| Metric | Good | Marginal | Poor | +|--------|------|----------|------| +| Thrust correlation \|r\| | > 0.6 | 0.3 - 0.6 | < 0.3 | +| Model R^2 | > 0.3 | 0.1 - 0.3 | < 0.1 | +| Compensated \|r\| | < 0.2 | 0.2 - 0.4 | > 0.4 | + +- **Low R^2**: Thrust is not the dominant baro error source. Check for thermal drift, ground effect, or sensor placement issues. +- **Online/offline K disagreement > 2 m**: The estimator may need more excitation. Fly longer or with more altitude variation. + +## See Also + +- [Static Pressure Buildup](../advanced_config/static_pressure_buildup.md) — airspeed-induced barometer error +- [Compass Power Compensation](../advanced_config/compass_power_compensation.md) — analogous compensation for magnetometer +- [Sensor Thermal Compensation](../advanced_config/sensor_thermal_calibration.md) — temperature-induced sensor error +- [Using PX4's Navigation Filter (EKF2)](../advanced_config/tuning_the_ecl_ekf.md) — EKF2 height fusion configuration diff --git a/docs/en/advanced_config/index.md b/docs/en/advanced_config/index.md index a01ce3a507..2a16bedbac 100644 --- a/docs/en/advanced_config/index.md +++ b/docs/en/advanced_config/index.md @@ -22,6 +22,7 @@ This topic lists configuration topics that are not particularly vehicle specific - [Compass Power Compensation](../advanced_config/compass_power_compensation.md) - [Advanced Controller Orientation](../advanced_config/advanced_flight_controller_orientation_leveling.md) - [Static Pressure Buildup](../advanced_config/static_pressure_buildup.md) +- [Barometer Thrust Compensation](../advanced_config/barometer_thrust_compensation.md) ## Serial port/Ethernet configuration diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp index ecb0c218f6..a0d652b7f1 100644 --- a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp @@ -186,8 +186,8 @@ void BaroThrustCfRls::RlsEstimator::update(float residual, float thrust, float d const float e = residual - (theta[0] * phi[0] + theta[1] * phi[1]); const float Pphi[2] = { - P[0][0] * phi[0] + P[0][1] * phi[1], - P[1][0] * phi[0] + P[1][1] * phi[1] + P[0][0] *phi[0] + P[0][1] *phi[1], + P[1][0] *phi[0] + P[1][1] *phi[1] }; const float phiPphi = phi[0] * Pphi[0] + phi[1] * Pphi[1];