From ab59b0bcb87b908e342ac13941ad297879e302cd Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 31 Mar 2026 17:11:28 -0800 Subject: [PATCH] feat(sensors): barometer thrust compensation with online estimator Add propwash-induced barometer error compensation. Propellers create static pressure changes at the baro sensor proportional to motor output. Correction: baro_alt += SENS_BARO_PCOEF * mean_motor_output. Thrust compensation in VehicleAirData uses a motor ring buffer (8 samples) with time-matched lookup against baro timestamp_sample. BMP388 timestamp_sample corrected from read time to integration midpoint (- measurement_time/2), eliminating 38ms baro-thrust lag. Online estimator module (baro_thrust_estimator) uses a complementary filter to isolate thrust-correlated baro error at 0.05Hz crossover, and recursive least squares to identify gain K online. Saves to SENS_BARO_PCOEF on disarm. Controlled by SENS_BAR_AUTOCAL bit 1. SENS_BAR_AUTOCAL migrated from boolean to bitmask (bit 0: GNSS cal, bit 1: thrust comp). Includes offline calibration/validation tool (Tools/baro_compensation/baro_thrust_calibration.py). --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 7 + Tools/baro_compensation/README.md | 153 +++ .../baro_thrust_calibration.py | 1136 +++++++++++++++++ msg/BaroThrustEstimate.msg | 11 + msg/CMakeLists.txt | 1 + src/lib/parameters/param_translation.cpp | 10 + .../BaroThrustEstimator.cpp | 388 ++++++ .../BaroThrustEstimator.hpp | 123 ++ .../baro_thrust_estimator/CMakeLists.txt | 55 + src/modules/baro_thrust_estimator/Kconfig | 13 + .../baro_thrust_cf_rls.cpp | 223 ++++ .../baro_thrust_cf_rls.hpp | 172 +++ .../baro_thrust_cf_rls_test.cpp | 252 ++++ src/modules/baro_thrust_estimator/params.yaml | 19 + src/modules/logger/logged_topics.cpp | 1 + .../vehicle_air_data/VehicleAirData.cpp | 64 +- .../vehicle_air_data/VehicleAirData.hpp | 24 +- .../sensors/vehicle_air_data/params.yaml | 26 +- 18 files changed, 2673 insertions(+), 5 deletions(-) create mode 100644 Tools/baro_compensation/README.md create mode 100755 Tools/baro_compensation/baro_thrust_calibration.py create mode 100644 msg/BaroThrustEstimate.msg create mode 100644 src/modules/baro_thrust_estimator/BaroThrustEstimator.cpp create mode 100644 src/modules/baro_thrust_estimator/BaroThrustEstimator.hpp create mode 100644 src/modules/baro_thrust_estimator/CMakeLists.txt create mode 100644 src/modules/baro_thrust_estimator/Kconfig create mode 100644 src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp create mode 100644 src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp create mode 100644 src/modules/baro_thrust_estimator/baro_thrust_cf_rls_test.cpp create mode 100644 src/modules/baro_thrust_estimator/params.yaml diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 24a3f81ed7..2962ee0d7e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -29,6 +29,13 @@ fi # Start Multicopter Position Controller. # mc_hover_thrust_estimator start + +# Bit 1 of SENS_BAR_AUTOCAL enables online thrust compensation (value > 1) +if param greater -s SENS_BAR_AUTOCAL 1 +then + baro_thrust_estimator start +fi + flight_mode_manager start mc_pos_control start diff --git a/Tools/baro_compensation/README.md b/Tools/baro_compensation/README.md new file mode 100644 index 0000000000..cc5ae9ee18 --- /dev/null +++ b/Tools/baro_compensation/README.md @@ -0,0 +1,153 @@ +# Barometer Thrust Compensation Analysis + +Post-flight analysis tool for barometer thrust compensation. Works with the +online `baro_thrust_estimator` module and/or a range sensor for ground truth. + +## Background + +Propwash from propellers changes the static pressure at the barometer sensor. +This creates a thrust-dependent altitude error: as thrust increases, the baro +reading shifts. The direction and magnitude depend on sensor placement relative +to the propellers. + +The `vehicle_air_data` module compensates for this by applying a correction to +the barometer altitude before publishing: + +``` +corrected_baro_alt = raw_baro_alt + SENS_BARO_PCOEF * abs(thrust_z) +``` + +where `thrust_z` is the Z body-axis component (`vehicle_thrust_setpoint.xyz[2]`), +which is signed in PX4 (negative for upward thrust in FRD). The correction uses +its magnitude `|thrust_z|` in [0, 1]. Using the vertical thrust setpoint (rather +than individual motor outputs) provides correct behavior for both multicopter and +VTOL aircraft. + +## How Calibration Works + +### Online (Preferred) + +The online estimator is disabled by default. Enable it by setting +`SENS_BAR_AUTOCAL` bit 1 (e.g. set to 3 for both GNSS cal and thrust comp). +The `baro_thrust_estimator` module identifies `SENS_BARO_PCOEF` automatically +during flight using an accel-baro complementary filter and RLS estimation. +Parameters are saved to flash on disarm once converged, and the estimator +continues to refine PCOEF over subsequent flights. No range sensor required. + +### Offline (This Tool) + +This tool identifies PCOEF from a flight log using a range sensor as ground +truth. Useful for: +- Validating online estimator results against ground truth +- Analyzing compensation effectiveness +- Initial calibration when the online estimator hasn't converged yet + +## Prerequisites + +- **Python packages**: `pyulog`, `numpy`, `matplotlib` + + ```bash + pip install pyulog numpy matplotlib + ``` + +## Analysis Modes + +The tool automatically selects a mode based on what data is in the log: + +### 1. Estimator Review (online estimator logged, no range sensor) + +Shows what the online estimator did during the flight: +- K estimate convergence with uncertainty band +- Prediction error and thrust excitation +- Convergence status timeline +- Compensation effect (residual before/after correction) + +### 2. Full Validation (online estimator + range sensor) + +Cross-validates the online estimator against range-sensor ground truth: +- All estimator review plots (above) +- Side-by-side comparison of online vs offline compensation +- Scatter plots comparing raw, online-corrected, and offline-corrected error + +### 3. Standalone Calibration (range sensor, no estimator) + +Identifies PCOEF from baro vs range error: +- Cross-correlation delay analysis +- Recommended PCOEF value + +## Usage + +```bash +python3 baro_thrust_calibration.py [--output-dir ] +``` + +## Output + +### Console + +Summary including parameters found in the log, online estimator convergence +status, range-based error statistics, and cross-validation between online and +offline identification (when both are available). + +### PDF Report (`.pdf`) + +Pages vary by mode. When the online estimator is present: + +**Altitude Overview** — Baro observation, EKF altitude, distance sensor (if +available), and thrust over time. + +**Estimator Convergence** — K estimate with variance band, error variance +and thrust excitation, convergence flags over time. + +**Compensation Effect** — CF residual before/after applying estimated K, +scatter plots, and compensation statistics. + +When a range sensor is present, additional pages show: + +**Compensation Comparison** — Side-by-side baro/range altitude with online +vs offline correction applied. + +**Online vs Offline Scatter** — Error vs thrust scatter for raw, online- +corrected, and offline-corrected data. + +## Manual Calibration Procedure + +If not using the online estimator, you can calibrate manually: + +1. **Disable existing compensation**: + ``` + param set SENS_BARO_PCOEF 0.0 + ``` + +2. **Fly a hover** at 2-5 m AGL for at least 60 seconds with some gentle + altitude changes. A range sensor must be installed. + +3. **Run this tool** on the log and apply the recommended parameter: + ``` + param set SENS_BARO_PCOEF + ``` + +4. **Fly again** and re-run the tool to verify (it will auto-detect validation + mode when compensation parameters are active). + +## Interpreting Results + +| Metric | Good | Marginal | Poor | +|--------|------|----------|------| +| Thrust correlation abs(r) | > 0.6 | 0.3 - 0.6 | < 0.3 | +| Model R^2 | > 0.3 | 0.1 - 0.3 | < 0.1 | +| Compensated abs(r) | < 0.2 | 0.2 - 0.4 | > 0.4 | + +- **Low R^2**: Thrust is not the dominant baro error source. Consider thermal + drift, ground effect, or sensor placement issues. +- **Very large K (> 5 m)**: May indicate a sensor mounting issue. The baro + should be shielded from direct propwash where possible. +- **Online/offline K disagreement > 2 m**: The estimator may not have had + enough excitation. Fly longer or with more altitude variation. + +## 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 | diff --git a/Tools/baro_compensation/baro_thrust_calibration.py b/Tools/baro_compensation/baro_thrust_calibration.py new file mode 100755 index 0000000000..0c2dcf80ca --- /dev/null +++ b/Tools/baro_compensation/baro_thrust_calibration.py @@ -0,0 +1,1136 @@ +#!/usr/bin/env python3 +""" +Barometer thrust compensation analysis. + +Three-way comparison of K estimates: + 1. Online CF+RLS (from logged baro_thrust_estimate) + 2. Offline CF+RLS replay (baro + accel, mirroring the firmware algorithm) + 3. Distance sensor ground truth (baro vs range, if available) + +The correction model: baro_alt += SENS_BARO_PCOEF * |thrust_z| + +Usage: + python3 baro_thrust_calibration.py [--output-dir ] + If --output-dir is not given, results go to logs// in the PX4 root. +""" + +import argparse +import os +import shutil +import sys + +import numpy as np + +try: + from pyulog import ULog +except ImportError: + print("Error: pyulog not installed. Run: pip install pyulog", file=sys.stderr) + sys.exit(1) + +try: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + from matplotlib.backends.backend_pdf import PdfPages +except ImportError: + print("Error: matplotlib not installed. Run: pip install matplotlib", file=sys.stderr) + sys.exit(1) + +GRAVITY = 9.80665 + + +# --------------------------------------------------------------------------- +# ULog helpers +# --------------------------------------------------------------------------- + +def get_topic(ulog, name, multi_id=0): + for d in ulog.data_list: + if d.name == name and d.multi_id == multi_id: + return d + return None + + +def get_param(ulog, name, default=None): + return ulog.initial_parameters.get(name, default) + + +def us_to_s(ts_us, start_us): + return (ts_us.astype(np.int64) - np.int64(start_us)) / 1e6 + + +def effective_rate(time_s): + if len(time_s) < 2: + return 0.0 + dt = np.diff(time_s) + dt = dt[dt > 0] + return 1.0 / np.median(dt) if len(dt) > 0 else 0.0 + + +def safe_corrcoef(x, y): + if len(x) < 2 or np.std(x) < 1e-10 or np.std(y) < 1e-10: + return 0.0 + return float(np.corrcoef(x, y)[0, 1]) + + +# --------------------------------------------------------------------------- +# Data extraction +# --------------------------------------------------------------------------- + +def extract_baro(ulog): + d = get_topic(ulog, "vehicle_air_data") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp_sample"], ulog.start_timestamp), + "alt_m": d.data["baro_alt_meter"], + } + + +def extract_accel(ulog): + d = get_topic(ulog, "vehicle_acceleration") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp_sample"], ulog.start_timestamp), + "x": d.data["xyz[0]"], + "y": d.data["xyz[1]"], + "z": d.data["xyz[2]"], + } + + +def extract_attitude(ulog): + d = get_topic(ulog, "vehicle_attitude") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp_sample"], ulog.start_timestamp), + "qw": d.data["q[0]"], + "qx": d.data["q[1]"], + "qy": d.data["q[2]"], + "qz": d.data["q[3]"], + } + + +def extract_thrust(ulog): + d = get_topic(ulog, "vehicle_thrust_setpoint") + if d is None: + return None + z = d.data.get("xyz[2]", None) + if z is None: + return None + return { + "time_s": us_to_s(d.data["timestamp"], ulog.start_timestamp), + "thrust": np.where(np.isfinite(z), np.abs(z), 0.0), + } + + +def extract_range(ulog): + d = get_topic(ulog, "distance_sensor") + if d is None: + return None + t = us_to_s(d.data["timestamp"], ulog.start_timestamp) + dist = d.data["current_distance"] + valid = np.isfinite(t) & np.isfinite(dist) + if "signal_quality" in d.data: + valid &= d.data["signal_quality"] > 0 + if valid.sum() < 2: + return None + return {"time_s": t[valid], "distance_m": dist[valid]} + + +def extract_online_estimate(ulog): + d = get_topic(ulog, "baro_thrust_estimate") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp"], ulog.start_timestamp), + "residual": d.data["residual"], + "k_estimate": d.data["k_estimate"], + "k_estimate_var": d.data["k_estimate_var"], + "error_var": d.data.get("error_var", + np.zeros(len(d.data["timestamp"]))), + "thrust_std": d.data["thrust_std"], + "converged": d.data["converged"], + "estimation_active": d.data["estimation_active"], + } + + +def extract_ekf_z(ulog): + d = get_topic(ulog, "vehicle_local_position") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp"], ulog.start_timestamp), + "z": d.data["z"], + } + + +def extract_ekf_baro_obs(ulog): + d = get_topic(ulog, "estimator_aid_src_baro_hgt") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp"], ulog.start_timestamp), + "observation": d.data["observation"], + } + + +def extract_landed(ulog): + d = get_topic(ulog, "vehicle_land_detected") + if d is None: + return None + return { + "time_s": us_to_s(d.data["timestamp"], ulog.start_timestamp), + "landed": d.data["landed"].astype(bool), + } + + +def detect_armed_period(ulog): + start_us = ulog.start_timestamp + vstatus = get_topic(ulog, "vehicle_status") + if vstatus is not None and "arming_state" in vstatus.data: + ts = us_to_s(vstatus.data["timestamp"], start_us) + armed_idx = np.where(vstatus.data["arming_state"] == 2)[0] + if len(armed_idx) > 0: + return float(ts[armed_idx[0]]), float(ts[armed_idx[-1]]) + motors = get_topic(ulog, "actuator_motors") + if motors is not None: + ts = us_to_s(motors.data["timestamp"], start_us) + active = np.zeros(len(ts), dtype=bool) + for i in range(12): + key = f"control[{i}]" + if key in motors.data: + active |= (motors.data[key] > 0.05) + active_idx = np.where(active)[0] + if len(active_idx) > 0: + return float(ts[active_idx[0]]), float(ts[active_idx[-1]]) + return 0.0, float((ulog.last_timestamp - start_us) / 1e6) + + +# --------------------------------------------------------------------------- +# Offline CF+RLS estimator (Python port of baro_thrust_cf_rls.cpp) +# --------------------------------------------------------------------------- + +def compute_accel_up(ax, ay, az, qw, qx, qy, qz): + """Vectorized: body-frame specific force + quaternion -> upward linear accel. + + Rotates body accel to NED (3rd row of quaternion DCM), then converts: + accel_up = -(specific_force_ned_z + g) + """ + ned_z = ((2 * (qx * qz - qw * qy)) * ax + + (2 * (qy * qz + qw * qx)) * ay + + (1 - 2 * (qx**2 + qy**2)) * az) + return -(ned_z + GRAVITY) + + +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_RLS_LAMBDA = 0.998 + RLS_P_INIT = 100.0 + ERROR_VAR_INIT = 10.0 + ALPHA_ERR = 0.01 + + def __init__(self, cf_bandwidth=None, rls_lambda=None): + bw = cf_bandwidth if cf_bandwidth is not None else self.DEFAULT_CF_BANDWIDTH + self.CF_OMEGA = 2.0 * np.pi * bw + self.CF_K1 = 2.0 * self.CF_OMEGA + self.CF_K2 = self.CF_OMEGA ** 2 + self.RLS_LAMBDA = (rls_lambda if rls_lambda is not None + else self.DEFAULT_RLS_LAMBDA) + self.reset() + + def reset(self): + self.cf_alt = 0.0 + self.cf_vel = 0.0 + self.cf_init = False + self.theta = np.zeros(2) # [K, bias] + self.P = np.eye(2) * self.RLS_P_INIT + self.error_var = self.ERROR_VAR_INIT + self._thrust_mean = 0.0 + self._thrust_var = 0.0 + self._k_smoothed = 0.0 + + def update_cf(self, baro_alt, accel_up, dt): + if not self.cf_init: + self.cf_alt = baro_alt + self.cf_vel = 0.0 + self.cf_init = True + return 0.0 + alt_pred = self.cf_alt + self.cf_vel * dt + 0.5 * accel_up * dt * dt + vel_pred = self.cf_vel + accel_up * dt + residual = baro_alt - alt_pred + self.cf_alt = alt_pred + self.CF_K1 * dt * residual + self.cf_vel = vel_pred + self.CF_K2 * dt * residual + if not (np.isfinite(self.cf_alt) and np.isfinite(self.cf_vel)): + self.cf_alt = baro_alt + self.cf_vel = 0.0 + return 0.0 + return residual + + def update_rls(self, residual, thrust, dt): + phi = np.array([thrust, 1.0]) + e = residual - self.theta @ phi + + Pphi = self.P @ phi + denom = self.RLS_LAMBDA + phi @ Pphi + if abs(denom) < 1e-10: + return + inv = 1.0 / denom + + self.theta += Pphi * inv * e + self.P = (self.P - np.outer(Pphi, Pphi) * inv) / self.RLS_LAMBDA + self.error_var = (1 - self.ALPHA_ERR) * self.error_var + self.ALPHA_ERR * e * e + + # Thrust excitation tracking (deviation computed before mean update) + alpha = dt / (2.0 + dt) + dev = thrust - self._thrust_mean + self._thrust_mean = (1 - alpha) * self._thrust_mean + alpha * thrust + self._thrust_var = (1 - alpha) * self._thrust_var + alpha * dev * dev + + # K smoothing + alpha_k = dt / (5.0 + dt) + self._k_smoothed = (1 - alpha_k) * self._k_smoothed + alpha_k * self.theta[0] + + if not (np.isfinite(self.theta).all() and np.isfinite(self.P).all() + and np.isfinite(self.error_var)): + self.reset() + + @property + def k(self): + return float(self.theta[0]) + + @property + def k_var(self): + return float(self.P[0, 0]) + + @property + def thrust_std(self): + return float(np.sqrt(max(self._thrust_var, 0.0))) + + +def run_offline_cf_rls(baro, accel, attitude, thrust, landed, + armed_start, armed_end, + cf_bandwidth=None, rls_lambda=None): + """Replay CF+RLS on logged sensor data. Returns K trace and residuals.""" + baro_t = baro["time_s"] + baro_alt = baro["alt_m"] + + # Compute accel_up at accel timestamps, then interp to baro timestamps + qw = np.interp(accel["time_s"], attitude["time_s"], attitude["qw"]) + qx = np.interp(accel["time_s"], attitude["time_s"], attitude["qx"]) + qy = np.interp(accel["time_s"], attitude["time_s"], attitude["qy"]) + qz = np.interp(accel["time_s"], attitude["time_s"], attitude["qz"]) + accel_up_all = compute_accel_up(accel["x"], accel["y"], accel["z"], + qw, qx, qy, qz) + + accel_up = np.interp(baro_t, accel["time_s"], accel_up_all) + thrust_interp = np.interp(baro_t, thrust["time_s"], thrust["thrust"]) + + # Airborne mask: armed AND not landed (matches firmware hard gates) + armed = (baro_t >= armed_start) & (baro_t <= armed_end) + if landed is not None: + is_landed = (np.interp(baro_t, landed["time_s"], + landed["landed"].astype(float)) > 0.5) + else: + is_landed = np.zeros(len(baro_t), dtype=bool) + airborne = armed & ~is_landed + + est = CfRls(cf_bandwidth=cf_bandwidth, rls_lambda=rls_lambda) + n = len(baro_t) + k_trace = np.full(n, np.nan) + k_var_trace = np.full(n, np.nan) + residual = np.full(n, np.nan) + error_var = np.full(n, np.nan) + thrust_std = np.full(n, np.nan) + + prev_t = None + for i in range(n): + if not airborne[i]: + continue + if prev_t is None: + prev_t = baro_t[i] + residual[i] = 0.0 + k_trace[i] = est.k + k_var_trace[i] = est.k_var + error_var[i] = est.error_var + thrust_std[i] = est.thrust_std + continue + + dt = float(np.clip(baro_t[i] - prev_t, 0.001, 0.5)) + prev_t = baro_t[i] + + res = est.update_cf(float(baro_alt[i]), float(accel_up[i]), dt) + est.update_rls(res, float(thrust_interp[i]), dt) + + residual[i] = res + k_trace[i] = est.k + k_var_trace[i] = est.k_var + error_var[i] = est.error_var + thrust_std[i] = est.thrust_std + + return { + "time_s": baro_t, + "k_trace": k_trace, + "k_var_trace": k_var_trace, + "residual": residual, + "error_var": error_var, + "thrust_std": thrust_std, + "final_k": est.k, + "final_k_var": est.k_var, + } + + +# --------------------------------------------------------------------------- +# Range-based calibration (distance sensor ground truth) +# --------------------------------------------------------------------------- + +def run_range_calibration(baro, range_data, thrust, + armed_start, armed_end, existing_pcoef=0.0): + """Least-squares calibration using range sensor as ground truth. + + Undoes existing PCOEF compensation to recover raw baro, then fits: + raw_baro_error = K_total * thrust + c + + Returns K_total (from-scratch gain), K_residual (remaining after existing + PCOEF), and data arrays for plotting. + """ + baro_t, baro_alt = baro["time_s"], baro["alt_m"] + rng_t, rng_dist = range_data["time_s"], range_data["distance_m"] + + # Zero baro at arm time + baro_zeroed = baro_alt - np.interp(armed_start, baro_t, baro_alt) + + # Undo existing compensation at baro timestamps + thrust_at_baro = np.interp(baro_t, thrust["time_s"], thrust["thrust"]) + raw_baro = baro_zeroed - existing_pcoef * thrust_at_baro + + # Interpolate both baro versions to range timestamps + raw_baro_interp = np.interp(rng_t, baro_t, raw_baro) + comp_baro_interp = np.interp(rng_t, baro_t, baro_zeroed) + raw_error = raw_baro_interp - rng_dist + comp_error = comp_baro_interp - rng_dist + + # Filter: armed, above ground proximity + MIN_RANGE = 0.5 + mask = ((rng_t >= armed_start) & (rng_t <= armed_end) + & (rng_dist > MIN_RANGE)) + if mask.sum() < 20: + return None + + t_fit = rng_t[mask] + raw_err_fit = raw_error[mask] + comp_err_fit = comp_error[mask] + thrust_fit = np.interp(t_fit, thrust["time_s"], thrust["thrust"]) + + # Fit on raw (uncompensated) error -> K_total + A = np.column_stack([thrust_fit, np.ones(len(thrust_fit))]) + coeffs, _, _, _ = np.linalg.lstsq(A, raw_err_fit, rcond=None) + K_total = float(coeffs[0]) + + raw_var = np.var(raw_err_fit) + resid = raw_err_fit - A @ coeffs + r2 = 1.0 - np.var(resid) / raw_var if raw_var > 1e-10 else 0.0 + + # Compensated fit for comparison + coeffs_comp, _, _, _ = np.linalg.lstsq(A, comp_err_fit, rcond=None) + + K_residual = K_total + existing_pcoef + + return { + "K_total": K_total, + "K_residual": K_residual, + "r2": float(r2), + "rmse": float(np.sqrt(np.mean(resid ** 2))), + "pcoef_recommended": -K_total, + # Plotting data + "time_s": rng_t, + "raw_error": raw_error, + "comp_error": comp_error, + "mask": mask, + "t_fit": t_fit, + "raw_err_fit": raw_err_fit, + "comp_err_fit": comp_err_fit, + "thrust_fit": thrust_fit, + "raw_coeffs": coeffs, + "comp_coeffs": coeffs_comp, + } + + +# --------------------------------------------------------------------------- +# CF+RLS parameter sweep +# --------------------------------------------------------------------------- + +def sweep_cf_params(baro, accel, attitude, thrust, landed, + armed_start, armed_end, range_cal, existing_pcoef): + """Sweep CF bandwidth at two lambda values, evaluate against range truth.""" + bandwidths = np.logspace(np.log10(0.01), np.log10(1.0), 30) + lambdas = [("lambda_0.998", 0.998), ("lambda_1.0", 1.0)] + + # Precompute shared data (same for all parameter combinations) + baro_t = baro["time_s"] + baro_alt = baro["alt_m"] + + qw = np.interp(accel["time_s"], attitude["time_s"], attitude["qw"]) + qx = np.interp(accel["time_s"], attitude["time_s"], attitude["qx"]) + qy = np.interp(accel["time_s"], attitude["time_s"], attitude["qy"]) + qz = np.interp(accel["time_s"], attitude["time_s"], attitude["qz"]) + accel_up_all = compute_accel_up(accel["x"], accel["y"], accel["z"], + qw, qx, qy, qz) + accel_up = np.interp(baro_t, accel["time_s"], accel_up_all) + thrust_interp = np.interp(baro_t, thrust["time_s"], thrust["thrust"]) + + armed = (baro_t >= armed_start) & (baro_t <= armed_end) + if landed is not None: + is_landed = (np.interp(baro_t, landed["time_s"], + landed["landed"].astype(float)) > 0.5) + else: + is_landed = np.zeros(len(baro_t), dtype=bool) + airborne_idx = np.where(armed & ~is_landed)[0] + + raw_err = range_cal["raw_err_fit"] + thr_fit = range_cal["thrust_fit"] + + results = {"bandwidth": bandwidths} + + for lam_key, lam in lambdas: + k_list = [] + std_list = [] + + for bw in bandwidths: + est = CfRls(cf_bandwidth=bw, rls_lambda=lam) + prev_t = None + for i in airborne_idx: + if prev_t is None: + prev_t = baro_t[i] + continue + dt = float(np.clip(baro_t[i] - prev_t, 0.001, 0.5)) + prev_t = baro_t[i] + res = est.update_cf(float(baro_alt[i]), + float(accel_up[i]), dt) + est.update_rls(res, float(thrust_interp[i]), dt) + + total_pcoef = existing_pcoef - est.k + corrected = raw_err + total_pcoef * thr_fit + + k_list.append(est.k) + std_list.append(float(np.std(corrected))) + + results[lam_key] = { + "k_residual": np.array(k_list), + "comp_error_std": np.array(std_list), + } + + return results + + +# --------------------------------------------------------------------------- +# Plotting +# --------------------------------------------------------------------------- + +_SUBTITLE_Y = 0.94 +_LAYOUT_TOP = 0.93 + + +def plot_altitude_overview(ekf_baro_obs, ekf_z, range_data, thrust_data, + armed_start, armed_end): + """Page 1: Altitude overview — baro, range, EKF, thrust.""" + fig, axes = plt.subplots(2, 1, figsize=(14, 8), sharex=True) + fig.suptitle("Altitude Overview", fontsize=14, fontweight="bold") + + ax = axes[0] + if range_data is not None: + ax.plot(range_data["time_s"], range_data["distance_m"], + label="Distance sensor", color="tab:blue", linewidth=1.2) + if ekf_baro_obs is not None: + bt = ekf_baro_obs["time_s"] + ba = -ekf_baro_obs["observation"] + ba -= np.interp(armed_start, bt, ba) + ax.plot(bt, ba, label="Baro observation (EKF input)", + color="tab:red", linewidth=1.0, alpha=0.8) + if ekf_z is not None: + et = ekf_z["time_s"] + ea = -ekf_z["z"] + ea -= np.interp(armed_start, et, ea) + ax.plot(et, ea, label="EKF altitude (-Z)", + color="tab:green", linewidth=1.0, alpha=0.8) + ax.axvspan(armed_start, armed_end, alpha=0.04, color="green", label="Armed") + ax.set_ylabel("Altitude AGL [m]") + ax.legend(fontsize=9, loc="upper left") + ax.grid(True, alpha=0.3) + + ax = axes[1] + ax.plot(thrust_data["time_s"], thrust_data["thrust"], + color="tab:orange", linewidth=0.8) + ax.set_ylabel("Thrust |z| [0-1]") + ax.set_xlabel("Time [s]") + ax.grid(True, alpha=0.3) + + plt.tight_layout(rect=[0, 0, 1, 0.96]) + return fig + + +def plot_k_convergence(online, offline, range_cal, + armed_start, armed_end, existing_pcoef): + """Page 2: K estimate convergence — online + offline overlaid.""" + fig, axes = plt.subplots(3, 1, figsize=(14, 10), sharex=True) + fig.suptitle("K Estimate Convergence", fontsize=14, fontweight="bold") + fig.text(0.5, _SUBTITLE_Y, + "Online (firmware) and offline (replay) CF+RLS. " + "Range K shown as ground-truth reference.", + ha="center", va="top", fontsize=9, style="italic", color="0.4") + + # --- Panel 1: K traces with variance bands --- + ax = axes[0] + if online is not None: + t = online["time_s"] + m = (t >= armed_start) & (t <= armed_end) + k = online["k_estimate"][m] + ks = np.sqrt(np.clip(online["k_estimate_var"][m], 0, None)) + ax.plot(t[m], k, color="tab:blue", linewidth=1.0, label="Online K") + ax.fill_between(t[m], k - ks, k + ks, alpha=0.1, color="tab:blue") + + if offline is not None: + t = offline["time_s"] + v = np.isfinite(offline["k_trace"]) + k = offline["k_trace"][v] + ks = np.sqrt(np.clip(offline["k_var_trace"][v], 0, None)) + ax.plot(t[v], k, color="tab:red", linewidth=1.0, + label="Offline K", alpha=0.8) + ax.fill_between(t[v], k - ks, k + ks, alpha=0.1, color="tab:red") + + if range_cal is not None: + ax.axhline(range_cal["K_residual"], color="tab:green", linestyle="--", + linewidth=1.0, + label=f"Range K\u2081 = {range_cal['K_residual']:.2f}") + + ax.set_ylabel("K [m / unit thrust]") + ax.legend(fontsize=9) + ax.grid(True, alpha=0.3) + + # --- Panel 2: Error variance --- + ax = axes[1] + if online is not None: + t = online["time_s"] + m = (t >= armed_start) & (t <= armed_end) + ax.plot(t[m], online["error_var"][m], color="tab:blue", + linewidth=0.8, label="Online") + if offline is not None: + t = offline["time_s"] + v = np.isfinite(offline["error_var"]) + ax.plot(t[v], offline["error_var"][v], color="tab:red", + linewidth=0.8, label="Offline", alpha=0.8) + ax.set_ylabel("Error Variance [m\u00b2]") + ax.legend(fontsize=9) + ax.grid(True, alpha=0.3) + + # --- Panel 3: Thrust excitation + convergence flags --- + ax = axes[2] + if online is not None: + t = online["time_s"] + m = (t >= armed_start) & (t <= armed_end) + conv = online["converged"][m].astype(float) + ax.fill_between(t[m], 0, conv * 0.08, alpha=0.3, color="tab:green", + step="post", label="Converged") + ax.plot(t[m], online["thrust_std"][m], color="tab:blue", + linewidth=0.8, label="Online thrust std") + if offline is not None: + t = offline["time_s"] + v = np.isfinite(offline["thrust_std"]) + ax.plot(t[v], offline["thrust_std"][v], color="tab:red", + linewidth=0.8, label="Offline thrust std", alpha=0.8) + ax.axhline(0.05, color="k", linestyle=":", linewidth=0.5, + label="Min excitation (0.05)") + ax.set_ylabel("Thrust Std") + ax.set_xlabel("Time [s]") + ax.legend(fontsize=9) + ax.grid(True, alpha=0.3) + + plt.tight_layout(rect=[0, 0, 1, _LAYOUT_TOP]) + return fig + + +def plot_residual_comparison(online, offline, thrust, + armed_start, armed_end): + """Page 3: CF residual vs thrust — online and offline side by side.""" + fig = plt.figure(figsize=(14, 10)) + gs = fig.add_gridspec(2, 2, height_ratios=[1, 1]) + fig.suptitle("CF Residual Analysis", fontsize=14, fontweight="bold") + fig.text(0.5, _SUBTITLE_Y, + "Top: residual time series (online blue, offline red). " + "Bottom: residual vs thrust scatter. Slope = K.", + ha="center", va="top", fontsize=9, style="italic", color="0.4") + + # --- Top: overlaid time series --- + ax_ts = fig.add_subplot(gs[0, :]) + if online is not None: + t = online["time_s"] + m = (t >= armed_start) & (t <= armed_end) + ax_ts.plot(t[m], online["residual"][m], color="tab:blue", + linewidth=0.6, alpha=0.7, label="Online") + if offline is not None: + t = offline["time_s"] + v = np.isfinite(offline["residual"]) + ax_ts.plot(t[v], offline["residual"][v], color="tab:red", + linewidth=0.6, alpha=0.7, label="Offline") + ax_ts.axhline(0, color="k", linewidth=0.5, linestyle="--") + ax_ts.set_ylabel("CF Residual [m]") + ax_ts.set_xlabel("Time [s]") + ax_ts.legend(fontsize=9) + ax_ts.grid(True, alpha=0.3) + + # --- Bottom: scatter plots --- + datasets = [ + (online, "Online", "tab:blue", gs[1, 0]), + (offline, "Offline", "tab:red", gs[1, 1]), + ] + for data, label, color, gs_pos in datasets: + ax = fig.add_subplot(gs_pos) + if data is None: + ax.text(0.5, 0.5, f"No {label.lower()} data", + transform=ax.transAxes, ha="center", va="center") + ax.set_title(label) + continue + + t = data["time_s"] + res = data["residual"] + m = (t >= armed_start) & (t <= armed_end) & np.isfinite(res) + res_m = res[m] + thr_m = np.interp(t[m], thrust["time_s"], thrust["thrust"]) + + ax.scatter(thr_m, res_m, s=2, alpha=0.3, color=color) + if np.std(thr_m) > 1e-6 and len(thr_m) > 5: + z = np.polyfit(thr_m, res_m, 1) + x_fit = np.linspace(thr_m.min(), thr_m.max(), 50) + ax.plot(x_fit, np.polyval(z, x_fit), "k--", linewidth=1.2) + r = safe_corrcoef(thr_m, res_m) + ax.set_title(f"{label}: slope = {z[0]:.2f}, r = {r:.3f}") + else: + ax.set_title(label) + ax.set_xlabel("Thrust [0-1]") + ax.set_ylabel("CF Residual [m]") + ax.axhline(0, color="k", linewidth=0.5, linestyle="--", alpha=0.3) + ax.grid(True, alpha=0.3) + + plt.tight_layout(rect=[0, 0, 1, _LAYOUT_TOP]) + return fig + + +def plot_ground_truth(range_cal, existing_pcoef): + """Page 4: Range-sensor ground truth — raw and compensated error.""" + fig, axes = plt.subplots(2, 2, figsize=(14, 10)) + fig.suptitle("Ground Truth Validation (Distance Sensor)", + fontsize=14, fontweight="bold") + fig.text(0.5, _SUBTITLE_Y, + "Left: raw baro error (compensation undone). " + "Right: as-flown (with existing PCOEF). " + "Slope = thrust-correlated error.", + ha="center", va="top", fontsize=9, style="italic", color="0.4") + + thrust = range_cal["thrust_fit"] + x_fit = np.linspace(thrust.min(), thrust.max(), 50) + + # Top-left: Raw error vs thrust scatter + ax = axes[0, 0] + ax.scatter(thrust, range_cal["raw_err_fit"], s=2, alpha=0.3, + color="tab:orange") + c = range_cal["raw_coeffs"] + ax.plot(x_fit, c[0] * x_fit + c[1], "k--", linewidth=1.2) + r = safe_corrcoef(thrust, range_cal["raw_err_fit"]) + ax.set_title(f"Raw: K = {c[0]:.1f}, r = {r:.3f}, R\u00b2 = {range_cal['r2']:.3f}") + ax.set_xlabel("Thrust [0-1]") + ax.set_ylabel("Baro Error [m]") + ax.grid(True, alpha=0.3) + + # Top-right: Compensated error vs thrust scatter + ax = axes[0, 1] + ax.scatter(thrust, range_cal["comp_err_fit"], s=2, alpha=0.3, + color="tab:blue") + cc = range_cal["comp_coeffs"] + ax.plot(x_fit, cc[0] * x_fit + cc[1], "k--", linewidth=1.2) + r = safe_corrcoef(thrust, range_cal["comp_err_fit"]) + ax.set_title(f"Compensated (PCOEF={existing_pcoef:+.1f}): " + f"slope = {cc[0]:.2f}, r = {r:.3f}") + ax.set_xlabel("Thrust [0-1]") + ax.set_ylabel("Baro Error [m]") + ax.axhline(0, color="k", linewidth=0.5, linestyle="--", alpha=0.3) + ax.grid(True, alpha=0.3) + + # Sync top row y-axes + ylim = [min(axes[0, 0].get_ylim()[0], axes[0, 1].get_ylim()[0]), + max(axes[0, 0].get_ylim()[1], axes[0, 1].get_ylim()[1])] + axes[0, 0].set_ylim(ylim) + axes[0, 1].set_ylim(ylim) + + # Bottom-left: Raw error time series + mask = range_cal["mask"] + t = range_cal["t_fit"] + ax = axes[1, 0] + ax.plot(t, range_cal["raw_err_fit"], color="tab:orange", linewidth=0.8) + ax.axhline(0, color="k", linewidth=0.5, linestyle="--") + std_raw = float(np.std(range_cal["raw_err_fit"])) + ax.set_title(f"Raw error: std = {std_raw:.2f} m") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Baro Error [m]") + ax.grid(True, alpha=0.3) + + # Bottom-right: Compensated error time series + ax = axes[1, 1] + ax.plot(t, range_cal["comp_err_fit"], color="tab:blue", linewidth=0.8) + ax.axhline(0, color="k", linewidth=0.5, linestyle="--") + std_comp = float(np.std(range_cal["comp_err_fit"])) + ax.set_title(f"Compensated error: std = {std_comp:.2f} m") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Baro Error [m]") + ax.grid(True, alpha=0.3) + + # Sync bottom row y-axes + ylim = [min(axes[1, 0].get_ylim()[0], axes[1, 1].get_ylim()[0]), + max(axes[1, 0].get_ylim()[1], axes[1, 1].get_ylim()[1])] + axes[1, 0].set_ylim(ylim) + axes[1, 1].set_ylim(ylim) + + plt.tight_layout(rect=[0, 0, 1, _LAYOUT_TOP]) + return fig + + +def plot_cf_tuning(sweep, range_cal, existing_pcoef): + """Page: CF parameter sensitivity — K and error std vs bandwidth.""" + fig, axes = plt.subplots(1, 2, figsize=(14, 6)) + fig.suptitle("CF+RLS Parameter Sensitivity", fontsize=14, fontweight="bold") + fig.text(0.5, _SUBTITLE_Y, + "Sweeping CF bandwidth: how does K and compensation quality " + "change? Range K is ground truth. Lower error std = better.", + ha="center", va="top", fontsize=9, style="italic", color="0.4") + + bw = sweep["bandwidth"] + default_bw = CfRls.DEFAULT_CF_BANDWIDTH + range_k = range_cal["K_residual"] + + styles = [ + ("lambda_0.998", "\u03bb=0.998 (default)", "tab:red"), + ("lambda_1.0", "\u03bb=1.0 (no forget)", "tab:purple"), + ] + + # --- Left: K vs bandwidth --- + ax = axes[0] + for key, label, color in styles: + ax.semilogx(bw, sweep[key]["k_residual"], "o-", color=color, + markersize=3, linewidth=1.0, label=label) + ax.axhline(range_k, color="tab:green", linestyle="--", linewidth=1.0, + label=f"Range K = {range_k:.2f}") + ax.axvline(default_bw, color="tab:gray", linestyle=":", linewidth=0.8, + label=f"Default ({default_bw} Hz)") + + # Best K match (default lambda) + k_def = sweep["lambda_0.998"]["k_residual"] + idx_best = int(np.argmin(np.abs(k_def - range_k))) + best_bw = float(bw[idx_best]) + ax.axvline(best_bw, color="tab:blue", linestyle="--", linewidth=0.8, + label=f"Best K match ({best_bw:.3f} Hz)") + + ax.set_xlabel("CF Bandwidth [Hz]") + ax.set_ylabel("K (residual)") + ax.set_title("K vs CF Bandwidth") + ax.legend(fontsize=8) + ax.grid(True, alpha=0.3) + + # --- Right: Error std vs bandwidth --- + ax = axes[1] + for key, label, color in styles: + ax.semilogx(bw, sweep[key]["comp_error_std"], "o-", color=color, + markersize=3, linewidth=1.0, label=label) + + ideal_err = (range_cal["raw_err_fit"] + + range_cal["pcoef_recommended"] * range_cal["thrust_fit"]) + ideal_std = float(np.std(ideal_err)) + ax.axhline(ideal_std, color="tab:green", linestyle="--", linewidth=1.0, + label=f"Range optimal: {ideal_std:.2f} m") + + current_std = float(np.std(range_cal["comp_err_fit"])) + ax.axhline(current_std, color="tab:orange", linestyle=":", linewidth=1.0, + label=f"Current PCOEF: {current_std:.2f} m") + + ax.axvline(default_bw, color="tab:gray", linestyle=":", linewidth=0.8) + ax.axvline(best_bw, color="tab:blue", linestyle="--", linewidth=0.8) + + # Min error std + std_def = sweep["lambda_0.998"]["comp_error_std"] + idx_min = int(np.argmin(std_def)) + min_bw = float(bw[idx_min]) + min_std = float(std_def[idx_min]) + if abs(min_bw - best_bw) / best_bw > 0.1: # only annotate if different + ax.axvline(min_bw, color="tab:red", linestyle="--", linewidth=0.8, + alpha=0.5, label=f"Min std ({min_bw:.3f} Hz)") + + ax.set_xlabel("CF Bandwidth [Hz]") + ax.set_ylabel("Compensated Error Std [m]") + ax.set_title("Compensation Quality vs CF Bandwidth") + ax.legend(fontsize=8) + ax.grid(True, alpha=0.3) + + plt.tight_layout(rect=[0, 0, 1, _LAYOUT_TOP]) + return fig, best_bw, min_bw, min_std + + +def plot_summary(text_lines): + """Final page: text summary.""" + fig, ax = plt.subplots(1, 1, figsize=(14, 10)) + ax.axis("off") + fig.suptitle("Analysis Summary", fontsize=14, fontweight="bold") + ax.text(0.02, 0.98, "\n".join(text_lines), transform=ax.transAxes, + fontsize=10, verticalalignment="top", family="monospace", + bbox=dict(boxstyle="round,pad=0.5", facecolor="#f8f8f8", + edgecolor="#cccccc")) + plt.tight_layout(rect=[0, 0, 1, 0.95]) + return fig + + +# --------------------------------------------------------------------------- +# Main +# --------------------------------------------------------------------------- + +def main(): + parser = argparse.ArgumentParser( + description="Barometer thrust compensation analysis") + parser.add_argument("ulog_file", help="Path to .ulg flight log") + px4_root = os.path.dirname(os.path.dirname(os.path.dirname( + os.path.abspath(__file__)))) + default_log_dir = os.path.join(px4_root, "logs") + parser.add_argument("--output-dir", "-o", default=default_log_dir, + help="Output base directory (default: /logs/)") + args = parser.parse_args() + + if not os.path.isfile(args.ulog_file): + print(f"Error: file not found: {args.ulog_file}", file=sys.stderr) + sys.exit(1) + + log_name = os.path.splitext(os.path.basename(args.ulog_file))[0] + if args.output_dir == default_log_dir: + output_dir = os.path.join(args.output_dir, log_name) + else: + output_dir = args.output_dir + os.makedirs(output_dir, exist_ok=True) + + ulg_dest = os.path.join(output_dir, os.path.basename(args.ulog_file)) + if not os.path.exists(ulg_dest): + shutil.copy2(args.ulog_file, ulg_dest) + + # ── Load ── + print(f"Loading {args.ulog_file}") + ulog = ULog(args.ulog_file) + duration = (ulog.last_timestamp - ulog.start_timestamp) / 1e6 + existing_pcoef = float(get_param(ulog, "SENS_BARO_PCOEF", 0.0)) + armed_start, armed_end = detect_armed_period(ulog) + + # ── Extract ── + baro = extract_baro(ulog) + accel = extract_accel(ulog) + attitude = extract_attitude(ulog) + thrust = extract_thrust(ulog) + range_data = extract_range(ulog) + online = extract_online_estimate(ulog) + ekf_z = extract_ekf_z(ulog) + ekf_baro_obs = extract_ekf_baro_obs(ulog) + landed = extract_landed(ulog) + + if baro is None or thrust is None: + print("Error: missing barometer or thrust data", file=sys.stderr) + sys.exit(1) + + # ── Summary header ── + summary = [] + summary.append(f"Log: {log_name}") + summary.append(f"Duration: {duration:.1f}s") + summary.append(f"Armed: {armed_start:.1f}s - {armed_end:.1f}s") + summary.append(f"SENS_BARO_PCOEF: {existing_pcoef:+.2f}") + summary.append("") + + for pname in ["EKF2_HGT_REF", "EKF2_BARO_CTRL", "EKF2_BARO_NOISE", + "SENS_BAR_AUTOCAL"]: + val = get_param(ulog, pname) + if val is not None: + summary.append(f" {pname:24s} = {val}") + summary.append("") + + summary.append("Data rates:") + summary.append(f" Baro: {effective_rate(baro['time_s']):5.1f} Hz " + f"({len(baro['time_s'])} samples)") + summary.append(f" Thrust: {effective_rate(thrust['time_s']):5.1f} Hz " + f"({len(thrust['time_s'])} samples)") + if accel is not None: + summary.append(f" Accel: {effective_rate(accel['time_s']):5.1f} Hz " + f"({len(accel['time_s'])} samples)") + if attitude is not None: + summary.append(f" Attitude:{effective_rate(attitude['time_s']):5.1f} Hz " + f"({len(attitude['time_s'])} samples)") + if range_data is not None: + summary.append(f" Range: {effective_rate(range_data['time_s']):5.1f} Hz " + f"({len(range_data['time_s'])} samples)") + if online is not None: + summary.append(f" Online: {effective_rate(online['time_s']):5.1f} Hz " + f"({len(online['time_s'])} samples)") + summary.append("") + + # ── Analysis ── + figures = [] + figures.append(plot_altitude_overview( + ekf_baro_obs, ekf_z, range_data, thrust, armed_start, armed_end)) + + # 1. Online estimator results + online_k = None + if online is not None: + armed_mask = ((online["time_s"] >= armed_start) + & (online["time_s"] <= armed_end)) + conv_mask = armed_mask & (online["converged"] > 0) + if conv_mask.any(): + conv_idx = np.where(conv_mask)[0][0] + online_k = float(online["k_estimate"][conv_idx]) + conv_time = float(online["time_s"][conv_idx]) - armed_start + summary.append(f"Online CF+RLS:") + summary.append(f" Converged at {conv_time:.0f}s after arm") + summary.append(f" K = {online_k:.3f}") + summary.append(f" Total PCOEF: {existing_pcoef:+.2f} " + f"- {online_k:.2f} = " + f"{existing_pcoef - online_k:+.2f}") + else: + last_k = online["k_estimate"][armed_mask] + online_k = float(last_k[-1]) if len(last_k) > 0 else None + if online_k is not None: + summary.append(f"Online CF+RLS: did NOT converge " + f"(last K = {online_k:.3f})") + else: + summary.append("Online CF+RLS: no armed data") + summary.append("") + + # 2. Offline CF+RLS replay + offline = None + if accel is not None and attitude is not None: + offline = run_offline_cf_rls(baro, accel, attitude, thrust, landed, + armed_start, armed_end) + summary.append(f"Offline CF+RLS:") + summary.append(f" Final K = {offline['final_k']:.3f}") + summary.append(f" Total PCOEF: {existing_pcoef:+.2f} " + f"- {offline['final_k']:.2f} = " + f"{existing_pcoef - offline['final_k']:+.2f}") + summary.append("") + else: + summary.append("Offline CF+RLS: skipped (missing accel or attitude)") + summary.append("") + + # 3. Range calibration + range_cal = None + if range_data is not None: + range_cal = run_range_calibration(baro, range_data, thrust, + armed_start, armed_end, + existing_pcoef) + if range_cal is not None: + summary.append(f"Range ground truth:") + summary.append(f" K_total = {range_cal['K_total']:.3f} " + f"(R\u00b2 = {range_cal['r2']:.3f}, " + f"RMSE = {range_cal['rmse']:.3f} m)") + summary.append(f" K_residual = {range_cal['K_residual']:.3f}") + summary.append(f" PCOEF recommended: " + f"{range_cal['pcoef_recommended']:+.2f}") + summary.append("") + else: + summary.append("Range calibration: insufficient data") + summary.append("") + + # ── Comparison table ── + sep = "=" * 60 + summary.append(sep) + r2_label = "R\u00b2" + summary.append(f" {'Method':<22} {'K':>8} {'Total PCOEF':>12} " + f"{r2_label:>6}") + summary.append("-" * 60) + + if online_k is not None: + pcoef = existing_pcoef - online_k + summary.append(f" {'Online CF+RLS':<22} {online_k:>8.3f} " + f"{pcoef:>+12.2f}") + + if offline is not None: + pcoef = existing_pcoef - offline["final_k"] + summary.append(f" {'Offline CF+RLS':<22} " + f"{offline['final_k']:>8.3f} {pcoef:>+12.2f}") + + if range_cal is not None: + summary.append(f" {'Range ground truth':<22} " + f"{range_cal['K_residual']:>8.3f} " + f"{range_cal['pcoef_recommended']:>+12.2f} " + f"{range_cal['r2']:>6.3f}") + + summary.append(sep) + + # Pairwise differences + if online_k is not None and offline is not None: + d = abs(online_k - offline["final_k"]) + summary.append(f" Online vs Offline: \u0394K = {d:.3f}") + if online_k is not None and range_cal is not None: + d = abs(online_k - range_cal["K_residual"]) + summary.append(f" Online vs Range: \u0394K = {d:.3f}") + if offline is not None and range_cal is not None: + d = abs(offline["final_k"] - range_cal["K_residual"]) + summary.append(f" Offline vs Range: \u0394K = {d:.3f}") + + summary.append("") + + # Print to console + for line in summary: + print(line) + + # ── Generate plots ── + figures.append(plot_k_convergence( + online, offline, range_cal, armed_start, armed_end, existing_pcoef)) + + if online is not None or offline is not None: + figures.append(plot_residual_comparison( + online, offline, thrust, armed_start, armed_end)) + + if range_cal is not None: + figures.append(plot_ground_truth(range_cal, existing_pcoef)) + + # ── Parameter sweep (when range ground truth available) ── + if (range_cal is not None and accel is not None + and attitude is not None): + print("\nSweeping CF bandwidth...") + sweep = sweep_cf_params(baro, accel, attitude, thrust, landed, + armed_start, armed_end, range_cal, + existing_pcoef) + fig, best_bw, min_bw, min_std = plot_cf_tuning( + sweep, range_cal, existing_pcoef) + figures.append(fig) + + range_k = range_cal["K_residual"] + k_def = sweep["lambda_0.998"]["k_residual"] + idx_best = int(np.argmin(np.abs(k_def - range_k))) + best_k = float(k_def[idx_best]) + + summary.append("") + summary.append("CF Bandwidth Tuning:") + summary.append(f" Default (0.050 Hz): K = {float(k_def[np.argmin(np.abs(sweep['bandwidth'] - 0.05))]):+.3f}") + summary.append(f" Best K match: {best_bw:.3f} Hz " + f"(K = {best_k:+.3f})") + summary.append(f" Min error std: {min_bw:.3f} Hz " + f"(std = {min_std:.3f} m)") + summary.append("") + + for line in summary[-6:]: + print(line) + + figures.append(plot_summary(summary)) + + # ── Save PDF ── + pdf_path = os.path.join(output_dir, f"{log_name}.pdf") + with PdfPages(pdf_path) as pdf: + for fig in figures: + pdf.savefig(fig) + plt.close(fig) + print(f"\nSaved: {pdf_path}") + + +if __name__ == "__main__": + main() diff --git a/msg/BaroThrustEstimate.msg b/msg/BaroThrustEstimate.msg new file mode 100644 index 0000000000..2ce37653db --- /dev/null +++ b/msg/BaroThrustEstimate.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time of baro data last used for this estimate + +float32 residual # CF residual: baro minus accel-predicted altitude [m] +float32 k_estimate # estimated thrust-to-baro gain [m/unit_thrust] +float32 k_estimate_var # variance of K estimate (P[0][0]) +float32 error_var # RLS prediction error variance +float32 thrust_std # standard deviation of recent thrust excitation + +bool converged # true when all convergence criteria are met +bool estimation_active # true when RLS is updating (soft guards not active) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 06feb9a335..2fe848aa9b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -47,6 +47,7 @@ set(msg_files Airspeed.msg AirspeedWind.msg AutotuneAttitudeControlStatus.msg + BaroThrustEstimate.msg BatteryInfo.msg ButtonEvent.msg CameraCapture.msg diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 92d6f8b473..f2e6a054a3 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -259,5 +259,15 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node) } } + // 2026-03-31: SENS_BAR_AUTOCAL boolean -> bitmask (bit 0: GNSS cal, bit 1: thrust comp) + { + if (strcmp("SENS_BAR_AUTOCAL", node->name) == 0 && node->type == bson_type_t::BSON_BOOL) { + node->i32 = node->b ? 1 : 0; + node->type = bson_type_t::BSON_INT32; + PX4_INFO("migrating %s from bool to bitmask", node->name); + return param_modify_on_import_ret::PARAM_MODIFIED; + } + } + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/modules/baro_thrust_estimator/BaroThrustEstimator.cpp b/src/modules/baro_thrust_estimator/BaroThrustEstimator.cpp new file mode 100644 index 0000000000..92f701ab30 --- /dev/null +++ b/src/modules/baro_thrust_estimator/BaroThrustEstimator.cpp @@ -0,0 +1,388 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BaroThrustEstimator.cpp + * + * Online estimator for barometer thrust compensation parameters. + * + * Uses a complementary filter (accel-integrated altitude vs baro) to isolate + * thrust-induced baro pressure errors. The CF residual (baro - accel_prediction) + * captures propwash effects while rejecting real altitude changes and accel drift. + * An RLS estimator identifies the gain K using the commanded collective thrust + * magnitude from vehicle_thrust_setpoint (|xyz[2]|). Converged parameters are + * saved on disarm. + */ + +#include "BaroThrustEstimator.hpp" + +#include + +using namespace time_literals; + +ModuleBase::Descriptor BaroThrustEstimator::desc{task_spawn, custom_command, print_usage}; + +BaroThrustEstimator::BaroThrustEstimator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _estimator.reset(); + updateParams(); +} + +BaroThrustEstimator::~BaroThrustEstimator() +{ + perf_free(_cycle_perf); +} + +bool BaroThrustEstimator::init() +{ + if (!_vehicle_air_data_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void BaroThrustEstimator::updateParams() +{ + ModuleParams::updateParams(); + _estimator.setCfBandwidth(_param_sens_bar_cf_bw.get()); +} + +void BaroThrustEstimator::Run() +{ + if (should_exit()) { + _vehicle_air_data_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + perf_begin(_cycle_perf); + + // Handle parameter updates + if (_parameter_update_sub.updated()) { + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + updateParams(); + } + + // Track arm state for disarm edge detection + if (_vehicle_status_sub.updated()) { + vehicle_status_s status; + + if (_vehicle_status_sub.copy(&status)) { + const bool was_armed = _armed; + _armed = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (was_armed != _armed) { + if (was_armed && !_armed) { + saveParameters(); + } + + _estimator.reset(); + _estimation_start_time = 0; + _last_update_time = 0; + _last_publish_time = 0; + } + } + } + + // Track landed state + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s ld; + + if (_vehicle_land_detected_sub.copy(&ld)) { + _landed = ld.landed; + } + } + + // Get baro data (callback trigger) + vehicle_air_data_s air_data{}; + + if (!_vehicle_air_data_sub.copy(&air_data)) { + perf_end(_cycle_perf); + return; + } + + // Hard guards: no estimation or CF when disarmed/landed + if (!_armed || _landed) { + perf_end(_cycle_perf); + return; + } + + // Compute dt + const hrt_abstime now = air_data.timestamp_sample; + + if (_last_update_time == 0) { + _last_update_time = now; + _estimation_start_time = now; + perf_end(_cycle_perf); + return; + } + + const float dt = math::constrain(static_cast(now - _last_update_time) * 1e-6f, 0.001f, 0.5f); + _last_update_time = now; + + // Validate baro + if (!PX4_ISFINITE(air_data.baro_alt_meter)) { + perf_end(_cycle_perf); + return; + } + + // Get acceleration in body frame + vehicle_acceleration_s accel{}; + + if (!_vehicle_acceleration_sub.copy(&accel) + || hrt_elapsed_time(&accel.timestamp) > 100_ms + || !PX4_ISFINITE(accel.xyz[0]) || !PX4_ISFINITE(accel.xyz[1]) || !PX4_ISFINITE(accel.xyz[2])) { + perf_end(_cycle_perf); + return; + } + + // Get attitude quaternion + vehicle_attitude_s att{}; + + if (!_vehicle_attitude_sub.copy(&att) + || hrt_elapsed_time(&att.timestamp) > 100_ms + || !PX4_ISFINITE(att.q[0])) { + perf_end(_cycle_perf); + return; + } + + // Compute upward linear acceleration and CF residual + const float accel_up = BaroThrustCfRls::computeAccelUp( + matrix::Vector3f{accel.xyz}, matrix::Quatf{att.q}); + + const float residual = _estimator.updateCf(air_data.baro_alt_meter, accel_up, dt); + + if (!PX4_ISFINITE(residual)) { + perf_end(_cycle_perf); + return; + } + + // Soft guards: skip RLS update but keep CF current + bool estimation_active = true; + + vehicle_local_position_s local_pos{}; + + if (_vehicle_local_position_sub.copy(&local_pos)) { + if (local_pos.v_z_valid && fabsf(local_pos.vz) > MAX_VZ) { + estimation_active = false; + } + + if (local_pos.v_xy_valid + && (local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy) > MAX_VXY * MAX_VXY) { + estimation_active = false; + } + } + + // Get vertical thrust setpoint — uses latest sample rather than time-matched + // (unlike thrustCompensation). Fine for a statistical estimator; timing jitter is noise. + float thrust = 0.f; + + vehicle_thrust_setpoint_s thrust_sp{}; + + if (_vehicle_thrust_setpoint_sub.copy(&thrust_sp) + && hrt_elapsed_time(&thrust_sp.timestamp) < 500_ms + && PX4_ISFINITE(thrust_sp.xyz[2])) { + thrust = fabsf(thrust_sp.xyz[2]); + + } else { + estimation_active = false; + } + + // Once converged, freeze RLS — the estimate is good and continued + // updates during descent/landing would corrupt it with ground effect. + // Note: when updateEstimator() stops being called, all internal + // tracking state (thrust variance, K smoothed, RLS covariance) freezes, + // so the convergence criteria checked by checkConvergence() remain + // stable and converged() cannot flicker back to false. + if (estimation_active && !_estimator.converged() && !_estimator.convergedLocked()) { + _estimator.updateEstimator(residual, thrust, dt); + } + + // Convergence checking runs independently of soft guards so the hold + // timer keeps ticking during descent. With RLS frozen the checked + // values (variance, error, stability) remain stable. + if (!_estimator.convergedLocked()) { + const float elapsed_s = static_cast(now - _estimation_start_time) * 1e-6f; + _estimator.checkConvergence(elapsed_s, dt); + + if (_estimator.convergedLocked()) { + PX4_INFO("convergence locked (K=%.2f)", (double)_estimator.kEstimate()); + } + } + + if (now - _last_publish_time > 200_ms) { + publishStatus(now, residual, estimation_active); + _last_publish_time = now; + } + + perf_end(_cycle_perf); +} + +void BaroThrustEstimator::saveParameters() +{ + if (!_estimator.convergedLocked()) { + PX4_INFO("baro thrust estimator did not converge (K=%.2f, var=%.1f, err=%.2f, thr_std=%.3f)", + (double)_estimator.kEstimate(), + (double)_estimator.kEstimateVar(), + (double)_estimator.errorVar(), + (double)_estimator.thrustStd()); + return; + } + + const float K_est = _estimator.kEstimate(); + + // Only update if the remaining error is significant + if (fabsf(K_est) < MIN_K_UPDATE_THRESHOLD) { + PX4_INFO("K_est=%.2f below threshold, calibration adequate", (double)K_est); + return; + } + + // K_est is the residual gain the estimator sees *after* existing compensation, + // so the corrected PCOEF shifts by -K_est to cancel it out. + const float pcoef_new = _param_sens_baro_pcoef.get() - K_est; + + if (!PX4_ISFINITE(pcoef_new)) { + PX4_WARN("non-finite result, skipping save"); + return; + } + + if (fabsf(pcoef_new) > PCOEF_MAX) { + PX4_WARN("result out of range (pcoef=%.1f)", (double)pcoef_new); + return; + } + + PX4_INFO("saving SENS_BARO_PCOEF=%.1f (K_est=%.2f, prev_pcoef=%.1f)", + (double)pcoef_new, (double)K_est, + (double)_param_sens_baro_pcoef.get()); + + _param_sens_baro_pcoef.set(pcoef_new); + _param_sens_baro_pcoef.commit_no_notification(); +} + +void BaroThrustEstimator::publishStatus(hrt_abstime now, float residual, bool estimation_active) +{ + baro_thrust_estimate_s status{}; + status.timestamp_sample = now; + status.residual = residual; + status.k_estimate = _estimator.kEstimate(); + status.k_estimate_var = _estimator.kEstimateVar(); + status.error_var = _estimator.errorVar(); + status.thrust_std = _estimator.thrustStd(); + status.converged = _estimator.converged(); + status.estimation_active = estimation_active && !_estimator.converged(); + status.timestamp = hrt_absolute_time(); + + _baro_thrust_estimate_pub.publish(status); +} + +// --------------------------------------------------------------------------- +// Module boilerplate +// --------------------------------------------------------------------------- + +int BaroThrustEstimator::task_spawn(int argc, char *argv[]) +{ + BaroThrustEstimator *instance = new BaroThrustEstimator(); + + if (instance) { + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + desc.object.store(nullptr); + desc.task_id = -1; + + return PX4_ERROR; +} + +int BaroThrustEstimator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int BaroThrustEstimator::print_status() +{ + PX4_INFO("converged: %s", _estimator.converged() ? "yes" : "no"); + PX4_INFO("K_est: %.3f K_var: %.4f error_var: %.4f", + (double)_estimator.kEstimate(), (double)_estimator.kEstimateVar(), + (double)_estimator.errorVar()); + PX4_INFO("thrust std: %.4f", (double)_estimator.thrustStd()); + PX4_INFO("current SENS_BARO_PCOEF: %.1f", (double)_param_sens_baro_pcoef.get()); + + perf_print_counter(_cycle_perf); + return 0; +} + +int BaroThrustEstimator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Online estimator for barometer thrust compensation (SENS_BARO_PCOEF). + +Uses an accel-baro complementary filter to isolate thrust-induced baro pressure errors. +The CF residual captures propwash effects while rejecting real altitude changes and +accelerometer drift. An RLS estimator identifies the thrust-to-baro gain K using the +commanded collective thrust magnitude from vehicle_thrust_setpoint (|xyz[2]|). +Converged parameters are saved on disarm and refined over subsequent flights. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("baro_thrust_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int baro_thrust_estimator_main(int argc, char *argv[]) +{ + return ModuleBase::main(BaroThrustEstimator::desc, argc, argv); +} diff --git a/src/modules/baro_thrust_estimator/BaroThrustEstimator.hpp b/src/modules/baro_thrust_estimator/BaroThrustEstimator.hpp new file mode 100644 index 0000000000..bb3df09079 --- /dev/null +++ b/src/modules/baro_thrust_estimator/BaroThrustEstimator.hpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BaroThrustEstimator.hpp + * + * Online estimator for barometer thrust compensation parameters. + * Module wrapper around BaroThrustCfRls — handles uORB I/O, + * parameter persistence, and lifecycle. + */ + +#pragma once + +#include "baro_thrust_cf_rls.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class BaroThrustEstimator : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + static Descriptor desc; + + BaroThrustEstimator(); + ~BaroThrustEstimator() override; + + static int task_spawn(int argc, char *argv[]); + static int custom_command(int argc, char *argv[]); + static int print_usage(const char *reason = nullptr); + + bool init(); + int print_status() override; + +private: + static constexpr float MAX_VZ = 2.f; + static constexpr float MAX_VXY = 5.f; + static constexpr float PCOEF_MAX = 30.f; + static constexpr float MIN_K_UPDATE_THRESHOLD = 0.1f; + + void Run() override; + void updateParams() override; + void saveParameters(); + void publishStatus(hrt_abstime now, float residual, bool estimation_active); + + BaroThrustCfRls _estimator{}; + + // Subscriptions + uORB::SubscriptionCallbackWorkItem _vehicle_air_data_sub{this, ORB_ID(vehicle_air_data)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + // Publication + uORB::Publication _baro_thrust_estimate_pub{ORB_ID(baro_thrust_estimate)}; + + // State + bool _armed{false}; + bool _landed{true}; + hrt_abstime _estimation_start_time{0}; + hrt_abstime _last_update_time{0}; + hrt_abstime _last_publish_time{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sens_baro_pcoef, + (ParamFloat) _param_sens_bar_cf_bw + ) +}; diff --git a/src/modules/baro_thrust_estimator/CMakeLists.txt b/src/modules/baro_thrust_estimator/CMakeLists.txt new file mode 100644 index 0000000000..dbabedc861 --- /dev/null +++ b/src/modules/baro_thrust_estimator/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(baro_thrust_cf_rls + baro_thrust_cf_rls.cpp + baro_thrust_cf_rls.hpp +) + +px4_add_module( + MODULE modules__baro_thrust_estimator + MAIN baro_thrust_estimator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + BaroThrustEstimator.cpp + BaroThrustEstimator.hpp + MODULE_CONFIG + params.yaml + DEPENDS + baro_thrust_cf_rls + mathlib + px4_work_queue +) + +px4_add_unit_gtest(SRC baro_thrust_cf_rls_test.cpp LINKLIBS baro_thrust_cf_rls) diff --git a/src/modules/baro_thrust_estimator/Kconfig b/src/modules/baro_thrust_estimator/Kconfig new file mode 100644 index 0000000000..ea3a5e1c1d --- /dev/null +++ b/src/modules/baro_thrust_estimator/Kconfig @@ -0,0 +1,13 @@ +menuconfig MODULES_BARO_THRUST_ESTIMATOR + bool "baro_thrust_estimator" + default y + depends on SENSORS_VEHICLE_AIR_DATA + ---help--- + Enable support for baro_thrust_estimator + +menuconfig USER_BARO_THRUST_ESTIMATOR + bool "baro_thrust_estimator running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_BARO_THRUST_ESTIMATOR + ---help--- + Put baro_thrust_estimator in userspace memory diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp new file mode 100644 index 0000000000..ecb0c218f6 --- /dev/null +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "baro_thrust_cf_rls.hpp" + +void BaroThrustCfRls::reset() +{ + _rls.reset(RLS_P_INIT); + + _cf_alt = 0.f; + _cf_vel = 0.f; + _cf_initialized = false; + + _converged = false; + _converged_locked = false; + _converged_elapsed_s = 0.f; + _k_stable_elapsed_s = 0.f; + _k_est_smoothed.reset(0.f); + _thrust_mean.reset(0.f); + _thrust_var.reset(0.f); +} + +float BaroThrustCfRls::computeAccelUp(const matrix::Vector3f &accel_body, + const matrix::Quatf &attitude) +{ + // Rotate body-frame specific force to NED, extract Z component + const float specific_force_ned_z = (attitude.rotateVector(accel_body))(2); + + // Convert to altitude-up coordinate acceleration + // specific_force = a_inertial - g_vec, so a_inertial_z = specific_force_z + g + // altitude = -NED_z, so accel_up = -(specific_force_z + g) + return -(specific_force_ned_z + CONSTANTS_ONE_G); +} + +float BaroThrustCfRls::updateCf(float baro_alt, float accel_up, float dt) +{ + if (!_cf_initialized) { + _cf_alt = baro_alt; + _cf_vel = 0.f; + _cf_initialized = true; + return 0.f; + } + + // Predict altitude from accel integration + const float alt_pred = _cf_alt + _cf_vel * dt + 0.5f * accel_up * dt * dt; + const float vel_pred = _cf_vel + accel_up * dt; + + // CF residual: what the baro says minus what accel predicts + const float residual = baro_alt - alt_pred; + + // Correct CF state with baro at low bandwidth to prevent accel drift + // 2nd-order critically damped: K1 = 2*omega, K2 = omega^2 + const float omega = 2.f * static_cast(M_PI) * _cf_bandwidth_hz; + const float K1 = 2.f * omega; + const float K2 = omega * omega; + + _cf_alt = alt_pred + K1 * dt * residual; + _cf_vel = vel_pred + K2 * dt * residual; + + if (!std::isfinite(_cf_alt) || !std::isfinite(_cf_vel)) { + _cf_alt = baro_alt; + _cf_vel = 0.f; + return 0.f; + } + + return residual; +} + +void BaroThrustCfRls::updateEstimator(float residual, float thrust, float dt) +{ + _rls.update(residual, thrust, dt, RLS_LAMBDA); + + // Track thrust excitation (compute deviation before updating mean + // so the current sample doesn't bias the mean used for deviation) + _thrust_mean.setParameters(dt, 2.f); + const float thrust_dev = thrust - _thrust_mean.getState(); + _thrust_mean.update(thrust); + _thrust_var.setParameters(dt, 2.f); + _thrust_var.update(thrust_dev * thrust_dev); + + // Track K stability + _k_est_smoothed.setParameters(dt, 5.f); + _k_est_smoothed.update(_rls.theta[0]); +} + +void BaroThrustCfRls::checkConvergence(float elapsed_since_start_s, float dt) +{ + if (_converged_locked) { + return; + } + + const bool variance_ok = _rls.P[0][0] < CONVERGENCE_VAR_THR; + const bool excitation_ok = fmaxf(_thrust_var.getState(), 0.f) > (MIN_THRUST_EXCITATION * MIN_THRUST_EXCITATION); + + // Dual-path error check: absolute threshold works for refinement flights + // (PCOEF already set), relative threshold allows first calibration where + // the uncompensated CF residual is noisy but the model explains most of it. + const float K = _rls.theta[0]; + const float explained_var = K * K * fmaxf(_thrust_var.getState(), 0.f); + const float total_var = explained_var + _rls.error_var; + const bool error_ok = _rls.error_var < CONVERGENCE_ERR_THR + || (total_var > 1.f + && _rls.error_var / total_var < CONVERGENCE_ERR_REL_THR + && _rls.error_var < CONVERGENCE_ERR_MAX_THR); + const bool time_ok = elapsed_since_start_s > MIN_ESTIMATION_TIME_S; + + const float k_current = _k_est_smoothed.getState(); + const float k_best = _rls.theta[0]; + + if (fabsf(k_current - k_best) > K_STABILITY_DIFF_THR) { + _k_stable_elapsed_s = 0.f; + + } else { + _k_stable_elapsed_s += dt; + } + + const bool stability_ok = _k_stable_elapsed_s > K_STABILITY_TIME_S; + + _converged = variance_ok && error_ok && excitation_ok && time_ok && stability_ok; + + // Once converged, keep accumulating hold time even if excitation + // momentarily dips — the estimate quality checks (variance, error, + // stability) are sufficient to guard the hold period. + const bool hold_ok = variance_ok && error_ok && time_ok && stability_ok; + + if (_converged || (_converged_elapsed_s > 0.f && hold_ok)) { + _converged_elapsed_s += dt; + + if (_converged_elapsed_s > CONVERGENCE_HOLD_TIME_S) { + _converged_locked = true; + } + + } else { + _converged_elapsed_s = 0.f; + } +} + +// --------------------------------------------------------------------------- +// RlsEstimator +// --------------------------------------------------------------------------- + +void BaroThrustCfRls::RlsEstimator::reset(float p_init) +{ + theta[0] = 0.f; + theta[1] = 0.f; + P[0][0] = p_init; + P[0][1] = 0.f; + P[1][0] = 0.f; + P[1][1] = p_init; + error_var = ERROR_VAR_INIT; +} + +void BaroThrustCfRls::RlsEstimator::update(float residual, float thrust, float dt, float lambda) +{ + const float phi[2] = {thrust, 1.f}; + 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] + }; + + const float phiPphi = phi[0] * Pphi[0] + phi[1] * Pphi[1]; + const float denom = lambda + phiPphi; + + if (fabsf(denom) < 1e-10f) { + return; + } + + const float inv_denom = 1.f / denom; + + theta[0] += Pphi[0] * inv_denom * e; + theta[1] += Pphi[1] * inv_denom * e; + + const float inv_lambda = 1.f / lambda; + + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + P[i][j] = (P[i][j] - Pphi[i] * Pphi[j] * inv_denom) * inv_lambda; + } + } + + constexpr float alpha_err = 0.01f; + error_var = (1.f - alpha_err) * error_var + alpha_err * e * e; + + // Guard against numerical instability + if (!std::isfinite(theta[0]) || !std::isfinite(theta[1]) + || !std::isfinite(P[0][0]) || !std::isfinite(P[0][1]) + || !std::isfinite(P[1][0]) || !std::isfinite(P[1][1]) + || !std::isfinite(error_var)) { + reset(RLS_P_INIT); + } +} diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp new file mode 100644 index 0000000000..f65f069360 --- /dev/null +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file baro_thrust_cf_rls.hpp + * + * Complementary Filter + Recursive Least Squares (CF+RLS) estimator + * for barometer thrust compensation. Pure math — no uORB, no PX4 + * module infrastructure. + * + * Problem: propwash creates a static pressure error at the baro sensor + * proportional to motor output. We want to identify the gain K in: + * + * baro_error = K * mean_motor_output + bias + * + * But we can't directly observe baro_error in flight — the vehicle is + * actually moving, so baro altitude changes for real reasons too. + * + * Solution — two stages: + * + * 1. Complementary Filter (CF) — isolate the baro error signal. + * Fuses barometer altitude with double-integrated accelerometer to + * produce an altitude estimate. The CF uses a very low crossover + * frequency (0.05 Hz), trusting the accelerometer for fast changes + * and the baro for DC. The residual (baro minus CF prediction) + * contains the thrust-correlated pressure error plus noise, with + * real vehicle motion removed. + * + * 2. Recursive Least Squares (RLS) — identify K from the residual. + * Fits the linear model: residual = K * thrust + bias, updating + * the estimate with each new sample. RLS is chosen over batch + * least-squares because it runs online with O(1) memory, adapts + * to changing conditions, and provides a covariance estimate (P) + * used for convergence detection. The forgetting factor (lambda) + * down-weights old data to track slow parameter drift. + * + * Convergence requires: low parameter variance (P[0][0]), acceptable + * prediction error (absolute OR relative to explained variance), + * sufficient thrust excitation, stable K estimate for 10s, and a 10s + * hold period. The relative error path allows first-flight calibration + * (PCOEF=0) where absolute residual is high but the model explains + * most of the variance. Once locked, K is saved to SENS_BARO_PCOEF + * on disarm. + */ + +#pragma once + +#include +#include +#include + +#include +#include + +class BaroThrustCfRls +{ +public: + // --- RLS tuning --- + static constexpr float RLS_LAMBDA = 0.998f; ///< forgetting factor (1.0 = never forget, lower = adapt faster) + static constexpr float RLS_P_INIT = 100.f; ///< initial covariance diagonal (high = uncertain, learns fast) + + // --- Convergence criteria --- + static constexpr float CONVERGENCE_VAR_THR = 3.f; ///< max K variance (P[0][0]) to consider converged + static constexpr float CONVERGENCE_ERR_THR = 0.5f; ///< max prediction error variance (absolute) [m^2] + static constexpr float CONVERGENCE_ERR_REL_THR = 0.4f; ///< max error/total variance ratio (model explains >60%) + static constexpr float CONVERGENCE_ERR_MAX_THR = 4.0f; ///< absolute error cap for relative path [m^2] + static constexpr float MIN_THRUST_EXCITATION = 0.05f; ///< min thrust std dev to trust the estimate + static constexpr float MIN_ESTIMATION_TIME_S = 30.f; ///< min flight time before convergence allowed + static constexpr float K_STABILITY_TIME_S = 10.f; ///< K must be stable within threshold for this long + static constexpr float CONVERGENCE_HOLD_TIME_S = 10.f; ///< must stay converged this long before locking + 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 ERROR_VAR_INIT = 10.f; ///< initial prediction error variance + + /** + * 2-state RLS estimator: fits residual = theta[0]*thrust + theta[1] + * where theta[0] = K (the gain we want) and theta[1] = bias. + * P is the 2x2 parameter covariance matrix. + */ + struct RlsEstimator { + float theta[2] {}; ///< [K, bias] parameter estimates + float P[2][2] {}; ///< 2x2 parameter covariance + float error_var{ERROR_VAR_INIT}; ///< exponentially-weighted prediction error variance + + void reset(float p_init); + void update(float residual, float thrust, float dt, float lambda); + }; + + void reset(); + + /** + * Set CF crossover frequency. Call before first updateCf() or after reset(). + */ + void setCfBandwidth(float hz) { _cf_bandwidth_hz = hz; } + + /** + * Convert body-frame specific force (including gravity, FRD) to + * altitude-up linear acceleration. + */ + static float computeAccelUp(const matrix::Vector3f &accel_body, const matrix::Quatf &attitude); + + /** + * Update complementary filter and return residual (baro - accel prediction). + */ + float updateCf(float baro_alt, float accel_up, float dt); + + /** + * Update the RLS estimator with the CF residual and motor thrust. + */ + void updateEstimator(float residual, float thrust, float dt); + + void checkConvergence(float elapsed_since_start_s, float dt); + + bool converged() const { return _converged; } + bool convergedLocked() const { return _converged_locked; } + float kEstimate() const { return _rls.theta[0]; } + float kEstimateVar() const { return _rls.P[0][0]; } + float errorVar() const { return _rls.error_var; } + float thrustStd() const { return sqrtf(fmaxf(_thrust_var.getState(), 0.f)); } + +private: + RlsEstimator _rls{}; + + // CF state: 2nd-order (position + velocity) integrator corrected by baro + float _cf_bandwidth_hz{CF_BANDWIDTH_HZ_DEFAULT}; ///< crossover frequency [Hz] + float _cf_alt{0.f}; ///< CF altitude estimate [m] + float _cf_vel{0.f}; ///< CF vertical velocity estimate [m/s] + bool _cf_initialized{false}; + + // Convergence tracking + AlphaFilter _k_est_smoothed{}; ///< low-pass filtered K for stability check + float _k_stable_elapsed_s{0.f}; ///< time K has been within stability threshold + bool _converged{false}; ///< all convergence criteria currently met + bool _converged_locked{false}; ///< converged and held long enough — ready to save + float _converged_elapsed_s{0.f}; ///< accumulated hold time while converged + + // Thrust excitation tracking (need variation in thrust to observe K) + AlphaFilter _thrust_mean{}; ///< low-pass mean thrust + AlphaFilter _thrust_var{}; ///< low-pass thrust variance +}; diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls_test.cpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls_test.cpp new file mode 100644 index 0000000000..8e4f5f68be --- /dev/null +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls_test.cpp @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test code for BaroThrustCfRls + * Run: make tests TESTFILTER=baro_thrust_cf_rls + */ + +#include +#include +#include +#include + +#include "baro_thrust_cf_rls.hpp" + +using namespace matrix; + +class BaroThrustCfRlsTest : public ::testing::Test +{ +protected: + BaroThrustCfRls _estimator{}; + static constexpr float _dt = 0.02f; // 50 Hz + + std::default_random_engine _rng{42}; + std::normal_distribution _normal{0.f, 1.f}; +}; + +// --------------------------------------------------------------------------- +// computeAccelUp — gravity handling +// --------------------------------------------------------------------------- + +TEST_F(BaroThrustCfRlsTest, AccelUpAtRestLevel) +{ + // At rest, level: specific force in FRD body = {0, 0, -g} + const Vector3f accel_body{0.f, 0.f, -CONSTANTS_ONE_G}; + const Quatf identity{}; + + const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, identity); + + EXPECT_NEAR(accel_up, 0.f, 1e-4f); +} + +TEST_F(BaroThrustCfRlsTest, AccelUpAscending) +{ + const float a_up_true = 2.f; + const Vector3f accel_body{0.f, 0.f, -(CONSTANTS_ONE_G + a_up_true)}; + const Quatf identity{}; + + const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, identity); + + EXPECT_NEAR(accel_up, a_up_true, 1e-4f); +} + +TEST_F(BaroThrustCfRlsTest, AccelUpTilted45Hover) +{ + const float theta = 45.f * 3.14159265f / 180.f; + const Vector3f accel_body{CONSTANTS_ONE_G * sinf(theta), 0.f, -CONSTANTS_ONE_G * cosf(theta)}; + const Quatf attitude{Eulerf{0.f, theta, 0.f}}; + + const float accel_up = BaroThrustCfRls::computeAccelUp(accel_body, attitude); + + EXPECT_NEAR(accel_up, 0.f, 1e-3f); +} + +// --------------------------------------------------------------------------- +// CF residual +// --------------------------------------------------------------------------- + +TEST_F(BaroThrustCfRlsTest, CfResidualConstantAltitude) +{ + _estimator.reset(); + + const float baro_alt = 100.f; + const float accel_up = 0.f; + float residual = 0.f; + + for (int i = 0; i < 500; i++) { // 10 seconds + residual = _estimator.updateCf(baro_alt, accel_up, _dt); + } + + EXPECT_NEAR(residual, 0.f, 1e-3f); +} + +TEST_F(BaroThrustCfRlsTest, CfResidualCapturesPropwash) +{ + _estimator.reset(); + + const float true_alt = 100.f; + + // Settle CF + for (int i = 0; i < 500; i++) { + _estimator.updateCf(true_alt, 0.f, _dt); + } + + // Apply a +3 m propwash error to baro + const float propwash_error = 3.f; + float residual = _estimator.updateCf(true_alt + propwash_error, 0.f, _dt); + + EXPECT_NEAR(residual, propwash_error, 0.5f); +} + +// --------------------------------------------------------------------------- +// RLS convergence +// --------------------------------------------------------------------------- + +TEST_F(BaroThrustCfRlsTest, RlsConvergenceNoiseless) +{ + // Feed residual = K_true * thrust + bias, verify K estimate converges. + _estimator.reset(); + + const float K_true = -5.f; + const float bias_true = 0.3f; + + for (float t = 0.f; t < 30.f; t += _dt) { + const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f); + const float residual = K_true * thrust + bias_true; + _estimator.updateEstimator(residual, thrust, _dt); + } + + EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.1f); +} + +TEST_F(BaroThrustCfRlsTest, RlsConvergenceWithNoise) +{ + _estimator.reset(); + + const float K_true = -3.f; + const float bias_true = 0.5f; + const float noise_std = 0.3f; + + for (float t = 0.f; t < 60.f; t += _dt) { + const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f); + const float residual = K_true * thrust + bias_true + noise_std * _normal(_rng); + _estimator.updateEstimator(residual, thrust, _dt); + } + + EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.5f); +} + +// --------------------------------------------------------------------------- +// Reset +// --------------------------------------------------------------------------- + +TEST_F(BaroThrustCfRlsTest, ResetClearsConvergence) +{ + _estimator.reset(); + + const float K_true = -3.f; + const float bias_true = 0.1f; + + // Drive to convergence + for (float t = 0.f; t < 80.f; t += _dt) { + const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f); + const float residual = K_true * thrust + bias_true; + _estimator.updateEstimator(residual, thrust, _dt); + _estimator.checkConvergence(t, _dt); + } + + EXPECT_TRUE(_estimator.convergedLocked()); + EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.2f); + + // Reset should clear all convergence state + _estimator.reset(); + EXPECT_FALSE(_estimator.converged()); + EXPECT_FALSE(_estimator.convergedLocked()); + EXPECT_NEAR(_estimator.kEstimate(), 0.f, 1e-6f); + EXPECT_NEAR(_estimator.kEstimateVar(), BaroThrustCfRls::RLS_P_INIT, 1e-6f); +} + +// --------------------------------------------------------------------------- +// Convergence gating +// --------------------------------------------------------------------------- + +TEST_F(BaroThrustCfRlsTest, ConvergenceRequiresMinTime) +{ + _estimator.reset(); + + const float K_true = -3.f; + + for (float t = 0.f; t < 25.f; t += _dt) { // < 30s minimum + const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f); + const float residual = K_true * thrust; + _estimator.updateEstimator(residual, thrust, _dt); + _estimator.checkConvergence(t, _dt); + } + + EXPECT_FALSE(_estimator.convergedLocked()); +} + +TEST_F(BaroThrustCfRlsTest, NoExcitationDoesNotConverge) +{ + _estimator.reset(); + + // Constant thrust — no excitation. RLS can fit K but the excitation + // gate should prevent convergence since we can't trust the estimate. + for (float t = 0.f; t < 80.f; t += _dt) { + const float thrust = 0.5f; + const float residual = -3.f * thrust; + _estimator.updateEstimator(residual, thrust, _dt); + _estimator.checkConvergence(t, _dt); + } + + EXPECT_FALSE(_estimator.convergedLocked()); +} + +TEST_F(BaroThrustCfRlsTest, ConvergenceLocksAfterHoldTime) +{ + _estimator.reset(); + + const float K_true = -3.f; + const float bias_true = 0.1f; + + for (float t = 0.f; t < 80.f; t += _dt) { + const float thrust = 0.5f + 0.15f * sinf(2.f * 3.14159f * t / 5.f); + const float residual = K_true * thrust + bias_true; + _estimator.updateEstimator(residual, thrust, _dt); + _estimator.checkConvergence(t, _dt); + } + + EXPECT_TRUE(_estimator.convergedLocked()); + EXPECT_NEAR(_estimator.kEstimate(), K_true, 0.2f); +} diff --git a/src/modules/baro_thrust_estimator/params.yaml b/src/modules/baro_thrust_estimator/params.yaml new file mode 100644 index 0000000000..2a09f7e860 --- /dev/null +++ b/src/modules/baro_thrust_estimator/params.yaml @@ -0,0 +1,19 @@ +module_name: baro_thrust_estimator +parameters: +- group: Sensors + definitions: + SENS_BAR_CF_BW: + description: + short: Baro thrust estimator CF crossover frequency + long: |- + Complementary filter bandwidth for the baro-accel altitude + estimator used to isolate thrust-induced baro errors. + Lower values trust baro more at low frequencies (conservative), + 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 + min: 0.01 + max: 1.0 + unit: Hz + decimal: 2 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 091dc41171..c5d048a751 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -82,6 +82,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("heater_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); + add_topic("baro_thrust_estimate", 100); add_topic("input_rc", 500); add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 6732f85934..36c9cc9d37 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -99,6 +99,63 @@ float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, Tempera return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } +void VehicleAirData::updateThrustBuffer() +{ + vehicle_thrust_setpoint_s thrust_sp; + + while (_vehicle_thrust_setpoint_sub.update(&thrust_sp)) { + ThrustSample &sample = _thrust_buffer[_thrust_buffer_head]; + sample.timestamp = thrust_sp.timestamp; + sample.thrust_z = PX4_ISFINITE(thrust_sp.xyz[2]) ? fabsf(thrust_sp.xyz[2]) : 0.f; + + _thrust_buffer_head = (_thrust_buffer_head + 1) % THRUST_BUFFER_SIZE; + + if (_thrust_buffer_count < THRUST_BUFFER_SIZE) { + _thrust_buffer_count++; + } + } +} + +float VehicleAirData::thrustCompensation(hrt_abstime timestamp_sample) +{ + const float pcoef = _param_sens_baro_pcoef.get(); + + if (fabsf(pcoef) < FLT_EPSILON || _thrust_buffer_count == 0) { + return 0.f; + } + + // Find the thrust sample closest to the baro measurement time. + // Buffer is chronologically ordered (newest at head-1), so once + // the time delta starts increasing we've passed the closest match. + int best_idx = -1; + hrt_abstime best_dt = UINT64_MAX; + + for (int i = 0; i < _thrust_buffer_count; i++) { + int idx = (_thrust_buffer_head - 1 - i + THRUST_BUFFER_SIZE) % THRUST_BUFFER_SIZE; + const hrt_abstime ts = _thrust_buffer[idx].timestamp; + + if (ts == 0) { + continue; + } + + const hrt_abstime dt = (ts >= timestamp_sample) ? (ts - timestamp_sample) : (timestamp_sample - ts); + + if (dt < best_dt) { + best_dt = dt; + best_idx = idx; + + } else { + break; + } + } + + if (best_idx < 0 || best_dt > 500_ms) { + return 0.f; + } + + return pcoef * _thrust_buffer[best_idx].thrust_z; +} + bool VehicleAirData::ParametersUpdate(bool force) { // Check if parameters have changed @@ -144,6 +201,8 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); + updateThrustBuffer(); + estimator_status_flags_s estimator_status_flags; const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); @@ -260,7 +319,7 @@ void VehicleAirData::Run() if (!_relative_calibration_done) { _relative_calibration_done = UpdateRelativeCalibrations(time_now_us); - } else if (!_baro_gnss_calibration_done && _param_sens_baro_autocal.get() && latest_estimator_status_flags.cs_gps_hgt) { + } else if (!_baro_gnss_calibration_done && (_param_sens_baro_autocal.get() & 1) && latest_estimator_status_flags.cs_gps_hgt) { _baro_gnss_calibration_done = BaroGNSSAltitudeOffset(); } } @@ -289,7 +348,8 @@ void VehicleAirData::Run() const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); + float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); + altitude += thrustCompensation(timestamp_sample); // calculate air density const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 23a7390d71..550eacd0fe 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -57,6 +57,7 @@ #include #include #include +#include using namespace time_literals; @@ -90,6 +91,24 @@ private: bool UpdateRelativeCalibrations(hrt_abstime time_now_us); bool BaroGNSSAltitudeOffset(); + // Note: applies a single PCOEF to whichever baro the voter selects as primary. + // If baros are at different locations (e.g., internal vs external CAN), they may + // experience different propwash magnitudes — a per-instance PCOEF would be needed + // to handle that correctly. For now this assumes co-located sensors. + float thrustCompensation(hrt_abstime timestamp_sample); + void updateThrustBuffer(); + + static constexpr int THRUST_BUFFER_SIZE = 8; + + struct ThrustSample { + hrt_abstime timestamp{0}; + float thrust_z{0.f}; + }; + + ThrustSample _thrust_buffer[THRUST_BUFFER_SIZE] {}; + int _thrust_buffer_head{0}; + int _thrust_buffer_count{0}; + static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)}; @@ -111,6 +130,8 @@ private: uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + calibration::Barometer _calibration[MAX_SENSOR_COUNT]; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; @@ -147,7 +168,8 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, - (ParamBool) _param_sens_baro_autocal + (ParamInt) _param_sens_baro_autocal, + (ParamFloat) _param_sens_baro_pcoef ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_air_data/params.yaml b/src/modules/sensors/vehicle_air_data/params.yaml index 4ffbdf2e90..98c20836b8 100644 --- a/src/modules/sensors/vehicle_air_data/params.yaml +++ b/src/modules/sensors/vehicle_air_data/params.yaml @@ -13,7 +13,29 @@ parameters: SENS_BAR_AUTOCAL: description: short: Barometer auto calibration - long: Automatically calibrate barometer based on the GNSS height + long: | + Bitmask controlling automatic barometer calibrations. + Bit 0: GNSS-based altitude offset calibration. + Bit 1: Online thrust compensation estimation (identifies + SENS_BARO_PCOEF during flight and saves on disarm). category: System - type: boolean + type: bitmask + bit: + 0: GNSS altitude offset + 1: Thrust compensation default: 1 + min: 0 + max: 3 + SENS_BARO_PCOEF: + description: + short: Baro altitude correction per unit vertical thrust + long: |- + Corrects propwash-induced baro error proportional to vertical + thrust setpoint magnitude. Sign depends on sensor placement + relative to propellers. Set to 0 to disable. + type: float + default: 0.0 + min: -30.0 + max: 30.0 + unit: m + decimal: 1