mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-10 11:58:53 +08:00
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:
@@ -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)
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user