fix(sensors): lower baro thrust estimator CF bandwidth default to 0.05 Hz

Sweep against range-sensor ground truth shows 0.05 Hz produces K estimates
that match range K more closely than 0.1 Hz. Update default in firmware,
offline analysis tool, and docs.
This commit is contained in:
Jacob Dahl
2026-03-31 23:43:48 -08:00
parent 850d4e2da3
commit c75c9cbe95
6 changed files with 88 additions and 6 deletions
+82
View File
@@ -0,0 +1,82 @@
# PX4-Autopilot
## Build
```bash
make ark_fmu-v6x_default -j$(nproc) # ARK FPV target
make px4_sitl_default -j$(nproc) # SITL
```
## Conventions
- Commits: `type(scope): description` (conventional commits). No `Co-Authored-By`.
- PR titles: same conventional commit format. No Claude attribution.
- Push to `jake` remote for fork PRs, `origin` for upstream PRs.
## Active branch: `dakejahl/baro-thrust-estimator`
PR: https://github.com/dakejahl/PX4-Autopilot/pull/41
### What it does
Propwash-induced barometer error compensation. Propellers create static pressure changes at the baro sensor proportional to motor output (~7m error, r=0.91 on ARK FPV). Correction: `baro_alt += SENS_BARO_PCOEF * mean_motor_output`.
### Components
**Thrust compensation** (`src/modules/sensors/vehicle_air_data/VehicleAirData.cpp`)
- Motor ring buffer (8 samples) with time-matched lookup against baro `timestamp_sample`
- Param: `SENS_BARO_PCOEF` (m per unit motor output, identified as ~-20 to -22 on test vehicle)
**BMP388 timestamp fix** (`src/drivers/barometer/bmp388/bmp388.cpp`)
- `timestamp_sample` corrected from read time to integration midpoint (`- measurement_time/2`)
- Eliminated 38ms baro-thrust lag. Other baro drivers not yet corrected.
**Online estimator** (`src/modules/baro_thrust_estimator/`)
- CF (complementary filter) isolates thrust-correlated baro error at 0.05Hz crossover
- RLS (recursive least squares) identifies gain K online, saves to `SENS_BARO_PCOEF` on disarm
- Controlled by `SENS_BAR_AUTOCAL` bit 1
**Rate limiting refactor**
- Removed `SENS_BARO_RATE`/`SENS_MAG_RATE` from publishers (had aliasing bug)
- Added `EKF2_BARO_RATE`/`EKF2_MAG_RATE` in EKF2.cpp
- Param translations in `src/lib/parameters/param_translation.cpp`
**Analysis tools** (`Tools/baro_compensation/`)
- `baro_thrust_calibration.py <log.ulg>` — offline calibration/validation, PDF output
- `motor_latency_analysis.py <log.ulg>` — command-to-RPM delay analysis
### Parameters
| Parameter | Default | Description |
|-----------|---------|-------------|
| `SENS_BARO_PCOEF` | 0.0 | Baro alt correction per unit motor output [m] |
| `SENS_BAR_AUTOCAL` | 3 | Bitmask: bit 0 = GNSS cal, bit 1 = thrust comp |
| `EKF2_BARO_RATE` | 20.0 | Max baro fusion rate in EKF [Hz] |
| `SENS_BAR_CF_BW` | 0.05 | CF crossover frequency for thrust estimator [Hz] |
| `EKF2_MAG_RATE` | 15.0 | Max mag fusion rate in EKF [Hz] |
### Test vehicle
ARK FPV: BMP390 baro (23Hz, 16x oversampling, 43ms forced-mode), 1404 4000KV motors, 3" triblade props, AM32 ESCs with BDShot, ARK Flow (range + optical flow) for validation.
### Key findings
- K ≈ 20-22m consistent across flights, online/offline agree within 0.5m
- Compensation: thrust r from 0.91 to 0.16, error std from 2.3m to 0.95m
- BMP390 integration window (37ms) smooths over motor response (~15ms), no delay param needed
- AM32 ESC RPM telemetry has ~6ms filtering latency (6-sample moving avg + IIR)
### Motor delay analysis (ARK FPV: 1404 4000KV + 3" triblade + AM32)
Measured 25ms command→RPM cross-correlation lag. Breakdown:
- ~15ms true electromechanical response (ESC commutation + prop inertia)
- ~6ms AM32 RPM filtering (6-sample commutation interval moving avg + IIR with alpha=0.25, see ~/code/ark/AM32/Src/main.c:1901,1626)
- ~4ms BDShot telemetry round-robin (4 motors at 800Hz output rate = 200Hz per channel)
The motor delay is invisible to baro-thrust compensation because the BMP390's 37ms integration window low-passes over it. A `SENS_BARO_PTAU` delay param was prototyped and removed for this reason. On faster baro sensors (lower oversampling, shorter integration) the motor delay could resurface.
### Next steps
- Install ARK Flow for position hold
- Test on other vehicles/baro sensors
- Correct `timestamp_sample` in other baro drivers (MS5611, DPS310, BMP280)
+2 -2
View File
@@ -152,7 +152,7 @@ If not using the online estimator, you can calibrate manually:
## CF Bandwidth Tuning
The default `SENS_BAR_CF_BW` (0.1 Hz) works well for most vehicles. Only
The default `SENS_BAR_CF_BW` (0.05 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.
@@ -189,4 +189,4 @@ to IMU vibration but slower to converge.
|-----------|-------------|-------|---------|
| `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 |
| `SENS_BAR_CF_BW` | CF crossover frequency for the online estimator [Hz] | 0.01 to 1.0 | 0.05 |
@@ -227,7 +227,7 @@ class CfRls:
"""Python port of BaroThrustCfRls. Constants match baro_thrust_cf_rls.hpp."""
# Defaults (matching baro_thrust_cf_rls.hpp)
DEFAULT_CF_BANDWIDTH = 0.1
DEFAULT_CF_BANDWIDTH = 0.05
DEFAULT_RLS_LAMBDA = 0.998
RLS_P_INIT = 100.0
ERROR_VAR_INIT = 10.0
@@ -25,7 +25,7 @@ The online estimator identifies `SENS_BARO_PCOEF` automatically during flight an
### 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).
A complementary filter (CF) fuses barometer altitude with double-integrated accelerometer data at a very low crossover frequency (default 0.05 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.
@@ -101,7 +101,7 @@ public:
static constexpr float K_STABILITY_DIFF_THR = 0.5f; ///< max |K_smoothed - K_raw| for stability [m]
// --- Complementary filter ---
static constexpr float CF_BANDWIDTH_HZ_DEFAULT = 0.1f; ///< default crossover frequency [Hz]
static constexpr float CF_BANDWIDTH_HZ_DEFAULT = 0.05f; ///< default crossover frequency [Hz]
static constexpr float ERROR_VAR_INIT = 10.f; ///< initial prediction error variance
/**
@@ -12,7 +12,7 @@ parameters:
higher values let more baro error through to the RLS estimator
(more accurate K identification but noisier with bad IMU).
type: float
default: 0.1
default: 0.05
min: 0.01
max: 1.0
unit: Hz