From a14677c37909c00cf7b3e6164f0754f04a7f0cd6 Mon Sep 17 00:00:00 2001 From: Raffi1202 Date: Tue, 22 Sep 2026 23:18:18 +0200 Subject: [PATCH] Propose automatic fixed-wing tuning and add model-fitting prototype --- .../fixed-wing-automatic-tuning.md | 325 ++++++++++++++++++ src/main/flight/fw_tune_model.c | 202 +++++++++++ src/main/flight/fw_tune_model.h | 61 ++++ src/test/unit/CMakeLists.txt | 2 + src/test/unit/fw_tune_model_unittest.cc | 206 +++++++++++ 5 files changed, 796 insertions(+) create mode 100644 docs/development/fixed-wing-automatic-tuning.md create mode 100644 src/main/flight/fw_tune_model.c create mode 100644 src/main/flight/fw_tune_model.h create mode 100644 src/test/unit/fw_tune_model_unittest.cc diff --git a/docs/development/fixed-wing-automatic-tuning.md b/docs/development/fixed-wing-automatic-tuning.md new file mode 100644 index 00000000000..247a9703f3f --- /dev/null +++ b/docs/development/fixed-wing-automatic-tuning.md @@ -0,0 +1,325 @@ +# Automatic fixed-wing tuning: implementation proposal for INAV 10.x + +**Status: design proposal, not an implemented flight mode.** This document +specifies an automatic commissioning sequence for conventional fixed-wing +aircraft, including two-servo flying wings. It does not enable excitation, +change gains, or declare a vehicle safe to fly. + +## Objective and boundaries + +After takeoff and establishing stable flight, the pilot starts one sequence +with one switch. The firmware coordinates servo trim, roll/pitch identification, +rate-controller tuning, attitude-controller tuning, level trim, and validation. +Optional extensions cover gain scheduling and navigation altitude/throttle +control. The pilot must not have to switch between AUTOTUNE, AUTOTRIM and +AUTOLEVELTRIM to advance the sequence. + +The initial supported mixer is a conventional fixed-wing mixer with independent +roll and pitch authority. A flying wing does not require a yaw actuator: the +default axis mask is roll/pitch, and yaw identification is not attempted. +VTOL transitions, tailsitters, rudder-only roll control, flaps, and changing +mixer geometry are outside the first implementation's supported envelope. + +The aircraft must already fly and stabilize with its initial settings. This +is not a first-launch recovery system. Sensor calibration, control direction, +mechanical travel, mixer configuration, and centre of gravity remain preflight +work. No stall search, maximum-speed discovery, battery-capacity estimation, +power-consumption survey, or range-estimator tuning is part of this proposal. + +## Existing implementation and related work + +The baseline is `maintenance-10.x`, inspected at +`86a0441c0003466d54a7711e7240d8297c8d067f`. Integrations must be checked again +against the target branch when code is submitted. + +| Component | Existing behaviour | Required new work | +| --- | --- | --- | +| `flight/pid_autotune.c` | Learns FF and, depending on mode/settings, maximum rates from pilot manoeuvres. Does not tune P/I/D. | Automated excitation, model/measurement acceptance, controller synthesis and independent validation. | +| `flight/servos.c` | Continuous servo trim can move neutral positions using stable I-term contributions; it saves on disarm. | Phase gating, bounded candidate trims and a transaction-aware persistence path. | +| `flight/pid.c` | Auto-level trim adjusts the pitch datum from vertical velocity when altitude control is inactive. | Eligibility/convergence reporting and explicit commit/rollback of its runtime state. | +| `flight/pid.c` / `fc/control_profile.*` | APA/TPA already scale gains; the level controller has a shared P gain and a setpoint filter. | Reuse scheduling; validate both axes before changing the shared level gain. | +| `navigation/navigation_fixedwing.c` | Existing altitude, speed and pitch-to-throttle controllers. | A separately validated navigation-tuning stage, not an import of another autopilot's energy controller. | +| `fc/rc_modes.c` | Gives existing tuning modes priority: AUTOTUNE, then AUTOTRIM, then AUTOLEVELTRIM. | One owner for the automatic session, without fabricating receiver mode bits. | + +Related INAV work: + +- [#7461](https://github.com/iNavFlight/inav/pull/7461) removed P/I/D values + derived from FF because they gave poor results, particularly on flying wings. + The new tuner must not reintroduce that method. +- [#7056](https://github.com/iNavFlight/inav/pull/7056) proposed discovering + fixed-wing acceleration limits. Recheck its implementation and status before + adding acceleration discovery; it is not a prerequisite of this sequence. +- [#11042](https://github.com/iNavFlight/inav/pull/11042) and + [#11222](https://github.com/iNavFlight/inav/pull/11222) cover airspeed-dependent + attenuation and validation. Do not add a competing speed-scaling path. +- [#12010](https://github.com/iNavFlight/inav/pull/12010) is a multirotor + measurement proposal. Its excitation frequencies, controller assumptions, + and flight adapter are not suitable fixed-wing defaults. + +The open PR search on 2026-09-22 did not find an equivalent combined automatic +fixed-wing tuning sequence. This is a search result, not a claim that every +external branch or experiment has been examined. + +## Pilot workflow + +1. Configure the aircraft normally and select a documented tuning envelope. + The configurator should explain missing prerequisites before flight. +2. Take off and establish stable flight inside that envelope. +3. Activate the sequence switch. The OSD shows the current phase, progress, + and the reason for any wait, failure, or unavailable optional stage. +4. The sequence runs its tests and checks the candidate settings. Stick input, + switching the sequence off, or a higher-priority flight action ends the + experiment immediately. +5. A successful sequence retains candidates temporarily for the remainder of + the flight. After landing, the pilot explicitly accepts or discards them. + Accepting is one action for the entire result; it is not a series of + separate mode changes. + +Suggested OSD messages are `TUNE: SERVO TRIM`, `TUNE: ROLL`, `TUNE: PITCH`, +`TUNE: ANGLE`, `TUNE: LEVEL`, `TUNE: CHECK`, and `TUNE: RESULT READY`. +The existing settings-save gesture must not accidentally commit an incomplete +session. A timeout means failure or an incomplete stage, never success. + +An outcome must distinguish completed required stages, skipped optional stages, +and failed stages. An altitude-tuning stage that was not run must not be +reported as a fully completed tune. + +## Session ownership and integration + +Use an explicit session state machine rather than toggling existing RC modes +on a timer. The session owns its candidate parameters and temporary excitation; +normal navigation, failsafe and pilot control retain higher priority. + +Conceptual states: + +``` +IDLE -> PREFLIGHT_CHECK -> CAPTURE_BASELINE -> ESTABLISH_FLIGHT + -> SERVO_TRIM -> IDENTIFY_ROLL -> IDENTIFY_PITCH + -> DESIGN_RATE_GAINS -> CHECK_RATE_GAINS + -> TUNE_ATTITUDE -> LEVEL_TRIM -> RECHECK_SERVO_TRIM + -> OPTIONAL_SPEED_CHECK -> OPTIONAL_NAV_TUNE -> FINAL_CHECK + -> RESULT_READY -> LAND_AND_ACCEPT -> COMMITTED + +Any active state -> ABORT -> RESTORE -> IDLE (after switch reset) +``` + +Each measurement phase can return to ESTABLISH_FLIGHT between test blocks. +There must be a finite retry/session limit. Rechecking servo trim after level +trim is necessary because the equilibrium angle and neutral surface positions +are coupled. Cap the number of passes and reject non-convergence. + +Separate the following responsibilities so they can be tested independently: + +- A numerical estimator and controller-design module with no parameter writes, + receiver reads, motor writes, or implicit clock calls. +- A sequence module that accepts timestamped observations and returns bounded + test requests, phase/status, and candidate updates. +- A flight adapter that verifies eligibility, arbitrates requests, gathers + actual applied signals, and cancels excitation before control output. +- A parameter transaction that owns snapshot, trial, restore and commit. + +The adapter must use actual controller/mixer signals, not assume that a +requested test reached the aircraft unchanged. Integration points must expose +controller saturation, servo headroom, rate/angle limiting, and the effective +APA/TPA factors. Mixer sums and asymmetric servo travel can saturate a flying +wing even when each axis PID output is individually below its limit. + +Changing control, battery or mixer profiles, live adjustments, CMS/MSP writes, +or programming overrides during an active session must abort or be rejected +through an explicit ownership mechanism. Do not silently overwrite unrelated +settings during rollback. + +## Flight path and eligibility + +There are two different kinds of phase: + +- Identification and gain-validation phases can run with qualified navigation + containment, provided that its commands are accounted for in the signal path. +- Servo and level-trim phases need qualified straight flight. In particular, + the level-trim learner must not compete with altitude hold. + +The navigation adapter must therefore provide bounded straight measurement +legs and recovery/turn segments. It must stop a measurement and recover before +the configured altitude or flight-area boundary is reached. Do not merely +disable altitude control and allow an unbounded straight run. The particular +navigation interface and available margin must be demonstrated in closed-loop +simulation before enabling an autonomous sequence. + +Eligibility includes valid attitude and gyro observations, valid navigation +position/velocity, adequate altitude and containment margin, the intended +fixed-wing mixer, usable control headroom, valid receiver input, and stable +flight conditions. Test amplitude, bank/pitch limits and minimum/maximum +airspeed must be explicit parts of the approved test envelope. They are not +values to discover by deliberately approaching loss of control. + +Airspeed validity needs special treatment. Ground speed alone is not proof of +adequate airspeed. A real validated pitot sensor is preferred. A sensorless +path requires a demonstrated wind/airspeed estimate with freshness and quality +checks. Until such a path is validated, the adapter must reject unsupported +speed-dependent stages rather than silently use GPS speed as airspeed. + +Pilot takeover, receiver loss, failsafe/RTH/landing, navigation validity loss, +containment loss, excessive attitude/rate, output saturation, observation +gaps, or non-finite numbers must remove excitation on the first control update +that observes the condition. Aborting must not suppress the requested failsafe +or pilot action. A switch held high must not restart the session after abort, +completion, disarm/rearm, or a temporary loss of eligibility. + +## Rate-controller identification and synthesis + +Roll and pitch are identified separately with bounded, smooth excitation. +Choose the frequency band and amplitude from servo/airframe response and +simulation results; do not reuse a multirotor sweep. Excitation must produce +enough information without exhausting attitude or actuator headroom. + +The measured plant input is the actual applied control signal, including +relevant mixing/filtering/limiting effects; the output is the matching gyro +rate with known timing. The estimator must account for trim offsets, sample +timing, actuator delay, closed-loop measurement bias, and airspeed changes. +Commanded rate divided by measured rate is not a plant transfer function. + +Before producing candidate gains, require: + +- sufficient independent excitation and signal-to-noise ratio; +- finite, numerically well-conditioned model coefficients; +- plausible sign, gain, delay and dynamics; +- prediction performance on measurements not used to fit the model; +- rejection of saturation, flexible-airframe modes and unmodelled coupling + that invalidate the chosen model class; +- controller stability/robustness checks using the actual INAV signal path. + +Controller synthesis must respect INAV's additive FF, P, I and derivative-on- +measurement structure, gain units, filters, I-term lock, integrator limiting, +and gain scheduling. A proven algorithm from another flight stack is a useful +reference, but its coefficients are not directly transferable. + +Candidate gains must be bounded, introduced without an output discontinuity, +and evaluated against the baseline using comparable held-out manoeuvres. +Reject a candidate that fails tracking, damping, actuator-use or robustness +criteria. A four-second quiet segment alone is not proof of a good tune. +Do not increase configured maximum rates merely because a small-signal model +predicts that the aircraft could rotate faster. + +## Attitude-controller tuning + +Auto-level trim and attitude tuning solve different problems. Trim finds the +level-flight datum; attitude tuning sets how the aircraft approaches a target +angle. + +In INAV, `fw_p_level` is shared by roll and pitch. `fw_i_level` is a filter +cutoff, and `fw_d_level` controls the HORIZON transition; these are not three +ordinary attitude PID gains. Keep the filter and HORIZON behaviour fixed in +the first implementation. Derive a candidate shared P from both accepted rate +responses and validate it on both axes with bounded angle commands. The slower +axis must constrain the result. Never tune it from roll alone and assume the +pitch response will also be acceptable. + +## Scheduling and oscillation monitoring + +Keep the existing APA/TPA machinery as the single gain-scheduling mechanism. +A first tune at one operating point does not identify a speed schedule. An +optional extension can validate the result at more than one already-approved +airspeed. Automatic changes to schedule parameters require repeatable results +at those operating points and a validated speed source. + +Oscillation monitoring is a distinct runtime feature, not another trim phase. +Its detector must distinguish commanded excitation, turbulence, sensor noise, +and sustained control-loop oscillation. Specify which terms are reduced, the +minimum gain, release/recovery behaviour, and its interaction with I-term lock +and integrator state. It must never raise gains above the nominal tune. + +During a tuning trial, activation of compression invalidates the trial unless +the estimator explicitly models that intervention. A tune must not be marked +successful because compression concealed unstable nominal gains. Recovery to +the baseline and routine pilot/failsafe takeover still take precedence. + +## Optional altitude and throttle tuning + +This is a separate stage after rate and attitude validation. INAV's navigation +controller is not ArduPilot TECS, so porting TECS parameter rules is not valid. + +Test bounded altitude/climb-rate changes within the approved flight envelope. +Evaluate pitch response, height/vertical-speed tracking, speed preservation, +throttle response and saturation. Candidate altitude gains and pitch-to- +throttle feedforward must be identified separately enough to avoid one +controller hiding errors in the other. A pitch trim measurement must never run +concurrently with these tests. + +No automatic search for stall speed or maximum climb capability is required. +The stage may only operate inside previously configured limits. Sensorless +airspeed support and behaviour with voltage sag, wind, delayed barometric +measurements and weak propulsion require explicit validation before support +is claimed. Cruise power and electrical consumption remain out of scope. + +## Parameter persistence + +Capture the affected settings and the identity of their profiles at session +start. Keep candidate results distinct from persistent settings. Cover servo +midpoints, runtime level trim and its integrator, rate gains, shared level gain, +and any enabled navigation/scheduling candidates. + +Existing servo-trim saves on disarm and level-trim updates on disarm must be +made transaction-aware. Handle every configuration-save path: an unrelated +save must not persist trial gains. A power loss or reset before acceptance must +load the last accepted configuration. + +Rollback restores the affected settings to their original profiles and resets +or transfers controller state in a tested way. Restoring coefficients alone +does not remove a contaminated integrator or filter state. On final acceptance, +commit the result using the existing configuration persistence mechanism, +without introducing an in-flight flash write. + +## Validation and release gates + +The following are requirements, not results already obtained: + +| Layer | Required evidence | +| --- | --- | +| Numerical tests | Known plants, independent validation data, noise, offsets, sample jitter, delay, weak excitation, ill-conditioning, reversed response, non-finite inputs and unstable/inadequate models. | +| Controller tests | Actual gain units and filtering, I-term lock, APA/TPA, derivative convention, integrator limits, slew limits and saturation; compare baseline and candidate under identical disturbances. | +| Sequence/adapter tests | Every transition, timeout and retry; pilot/failsafe preemption; timestamp wrap; receiver/startup edge cases; sensor loss; mixer/profile changes; no yaw command on a two-servo wing. | +| Persistence tests | Successful accept, discard, abort, disarm, reset/power loss, unrelated save and changed-profile cases; no partial candidate settings after restart. | +| Closed-loop simulation | At least a flying wing and conventional plane; different servo speeds, asymmetric travel, coupled pitch/roll, wind/turbulence, actuator saturation, navigation containment and sensor failures. Use independent flight dynamics, not only the estimator's own model equations. | +| Target builds | Representative F4, small-flash F7, H7 and AT32 targets; flash/RAM/ITCM and worst-case execution cost. Unsupported targets must explicitly omit the feature. | +| Hardware/flight validation | Bench output/preemption tests, then staged identification-only and tuning trials with logs and baseline comparison. Simulator success is not a physical flight test. | + +No phase may be reported complete solely because its timer expired. Define +and publish measurable acceptance thresholds alongside the implementing code, +with evidence for those thresholds. Thresholds and excitation envelopes are +intentionally not presented here as flight-proven constants. + +## Implementation series + +Keep code submissions focused and independently reviewable: + +1. Measurement/estimator and replay tests, including rejected-model cases. +2. Controller synthesis and validation using the actual INAV rate/angle loops. +3. Session ownership, reversible trim/gain transactions, and persistence tests. +4. Flight-sequence/navigation integration, OSD status and one-switch workflow. +5. Optional speed-envelope validation and scheduling changes. +6. Optional navigation altitude/throttle tuning. +7. Independently tested oscillation monitoring/compression. + +The first usable automatic sequence requires items 1-4 together and the +applicable validation gates. Merging an estimator alone must not expose a +mode that claims to complete the full tune. Optional stages need explicit +support/status reporting, not placeholder success paths. + +## External references + +- [PX4 fixed-wing autotune](https://docs.px4.io/main/en/config/autotune_fw): + automatic excitation, selectable axes, model-based tuning and trial rollback. +- [PX4 fixed-wing implementation](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules/fw_autotune_attitude_control) + and [controller synthesis](https://github.com/PX4/PX4-Autopilot/tree/main/src/lib/pid_design): + useful algorithm references; the examined fixed-wing adapter applies PI/FF, + not a directly transferable INAV PID parameter set. +- [ArduPilot Plane autotune](https://ardupilot.org/plane/docs/automatic-tuning-with-autotune.html): + FF/P/I/D tuning, configurable response and airspeed scaling. +- [PX4 gain compression](https://docs.px4.io/main/en/features_fw/gain_compression) + and [ArduPilot limit-cycle detection](https://ardupilot.org/plane/docs/common-servo-limit-cycle-detection.html): + runtime oscillation handling. +- [ArduPilot TECS tuning](https://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html): + a reference for separating attitude-loop tuning from altitude/speed control, + not evidence that TECS parameters map to INAV navigation gains. + +Any adapted source must preserve its required copyright and licence notices. diff --git a/src/main/flight/fw_tune_model.c b/src/main/flight/fw_tune_model.c new file mode 100644 index 00000000000..70801cec769 --- /dev/null +++ b/src/main/flight/fw_tune_model.c @@ -0,0 +1,202 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ + +#include +#include + +#include "flight/fw_tune_model.h" + +#define FW_TUNE_MODEL_MIN_SAMPLES 100 +#define FW_TUNE_MODEL_MAX_SAMPLES 10000 + +void fwTuneModelInit(fwTuneModelFit_t *fit, float samplePeriod) +{ + memset(fit, 0, sizeof(*fit)); + fit->samplePeriod = samplePeriod; + fit->invalid = !isfinite(samplePeriod) || samplePeriod <= 0; +} + +bool fwTuneModelAdd(fwTuneModelFit_t *fit, float input, float output) +{ + if (fit->invalid || !isfinite(input) || !isfinite(output) || + fabsf(input) > 10000.0f || fabsf(output) > 10000.0f || + fit->samples >= FW_TUNE_MODEL_MAX_SAMPLES) { + fit->invalid = true; + return false; + } + + if (fit->history >= 3) { + float row[FW_TUNE_MODEL_TERMS] = { + -fit->y[0], -fit->y[1], fit->u[0], fit->u[1], fit->u[2], 1.0f, + }; + float residual = output; + + // Incremental QR avoids squaring the condition number through X'X. + // The intercept fits a constant trim offset without altering the dynamics. + for (unsigned i = 0; i < FW_TUNE_MODEL_TERMS; i++) { + const float magnitude = hypotf(fit->r[i][i], row[i]); + if (magnitude == 0) { + continue; + } + if (!isfinite(magnitude)) { + fit->invalid = true; + return false; + } + const float c = fit->r[i][i] / magnitude; + const float s = row[i] / magnitude; + fit->r[i][i] = magnitude; + for (unsigned j = i + 1; j < FW_TUNE_MODEL_TERMS; j++) { + const float value = c * fit->r[i][j] + s * row[j]; + row[j] = -s * fit->r[i][j] + c * row[j]; + fit->r[i][j] = value; + } + const float value = c * fit->z[i] + s * residual; + residual = -s * fit->z[i] + c * residual; + fit->z[i] = value; + } + fit->samples++; + } else { + fit->history++; + } + + fit->u[2] = fit->u[1]; + fit->u[1] = fit->u[0]; + fit->u[0] = input; + fit->y[1] = fit->y[0]; + fit->y[0] = output; + return true; +} + +static bool modelIsFinite(const fwTuneModel_t *model) +{ + return isfinite(model->a[0]) && isfinite(model->a[1]) && + isfinite(model->b[0]) && isfinite(model->b[1]) && isfinite(model->b[2]) && + isfinite(model->offset) && isfinite(model->samplePeriod) && model->samplePeriod > 0; +} + +bool fwTuneModelIsStable(const fwTuneModel_t *model) +{ + // Strict second-order Jury conditions; marginal poles are not accepted. + return modelIsFinite(model) && fabsf(model->a[1]) < 1.0f && + 1.0f + model->a[0] + model->a[1] > 0.00001f && + 1.0f - model->a[0] + model->a[1] > 0.00001f; +} + +bool fwTuneModelStaticGain(const fwTuneModel_t *model, float *gain) +{ + if (!fwTuneModelIsStable(model)) { + return false; + } + const float value = (model->b[0] + model->b[1] + model->b[2]) / + (1.0f + model->a[0] + model->a[1]); + if (!isfinite(value) || value <= 0) { + return false; + } + *gain = value; + return true; +} + +bool fwTuneModelSolve(const fwTuneModelFit_t *fit, fwTuneModel_t *model) +{ + if (fit->invalid || fit->samples < FW_TUNE_MODEL_MIN_SAMPLES) { + return false; + } + float coefficients[FW_TUNE_MODEL_TERMS] = {0}; + for (int i = FW_TUNE_MODEL_TERMS - 1; i >= 0; i--) { + float rowMagnitude = 0; + for (unsigned j = 0; j <= (unsigned)i; j++) { + rowMagnitude += fabsf(fit->r[j][i]); + } + // Reject insufficiently independent columns, including constant/no input. + if (!isfinite(rowMagnitude) || fabsf(fit->r[i][i]) <= 0.0001f * rowMagnitude || + fabsf(fit->r[i][i]) < 0.00001f) { + return false; + } + float value = fit->z[i]; + for (unsigned j = i + 1; j < FW_TUNE_MODEL_TERMS; j++) { + value -= fit->r[i][j] * coefficients[j]; + } + coefficients[i] = value / fit->r[i][i]; + if (!isfinite(coefficients[i])) { + return false; + } + } + + const fwTuneModel_t result = { + .a = {coefficients[0], coefficients[1]}, + .b = {coefficients[2], coefficients[3], coefficients[4]}, + .offset = coefficients[5], + .samplePeriod = fit->samplePeriod, + }; + float gain; + if (!fwTuneModelStaticGain(&result, &gain)) { + return false; + } + *model = result; + return true; +} + +float fwTuneModelPredict(const fwTuneModel_t *model, fwTuneModelState_t *state, float input) +{ + const float output = -model->a[0] * state->y[0] - model->a[1] * state->y[1] + + model->b[0] * state->u[0] + model->b[1] * state->u[1] + model->b[2] * state->u[2] + model->offset; + state->y[1] = state->y[0]; + state->y[0] = output; + state->u[2] = state->u[1]; + state->u[1] = state->u[0]; + state->u[0] = input; + return output; +} + +void fwTuneModelValidationInit(fwTuneModelValidation_t *validation, const fwTuneModel_t *model) +{ + memset(validation, 0, sizeof(*validation)); + validation->model = *model; + float gain; + validation->invalid = !fwTuneModelStaticGain(model, &gain); +} + +bool fwTuneModelValidateSample(fwTuneModelValidation_t *validation, float input, float output) +{ + if (validation->invalid || !isfinite(input) || !isfinite(output) || + fabsf(input) > 10000.0f || fabsf(output) > 10000.0f || + validation->samples >= FW_TUNE_MODEL_MAX_SAMPLES) { + validation->invalid = true; + return false; + } + const float prediction = fwTuneModelPredict(&validation->model, &validation->state, input); + if (!isfinite(prediction) || fabsf(prediction) > 10000.0f) { + validation->invalid = true; + return false; + } + if (validation->history < 3) { + validation->state.y[0] = output; + validation->history++; + return true; + } + + const float error = prediction - output; + validation->squaredError += error * error; + validation->samples++; + const float delta = output - validation->mean; + validation->mean += delta / validation->samples; + validation->squaredDeviation += delta * (output - validation->mean); + return true; +} + +bool fwTuneModelValidationError(const fwTuneModelValidation_t *validation, float *normalisedError) +{ + if (validation->invalid || validation->samples < FW_TUNE_MODEL_MIN_SAMPLES || + !isfinite(validation->squaredError) || !isfinite(validation->squaredDeviation) || + validation->squaredDeviation < 0.000001f) { + return false; + } + const float value = sqrtf(validation->squaredError / validation->squaredDeviation); + if (!isfinite(value)) { + return false; + } + *normalisedError = value; + return true; +} diff --git a/src/main/flight/fw_tune_model.h b/src/main/flight/fw_tune_model.h new file mode 100644 index 00000000000..a8dcbeb82e6 --- /dev/null +++ b/src/main/flight/fw_tune_model.h @@ -0,0 +1,61 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ +#pragma once + +#include +#include + +// y[k] = -a1*y[k-1] - a2*y[k-2] + b1*u[k-1] + b2*u[k-2] + b3*u[k-3] + offset. +// Samples must be synchronous, uniformly spaced, and expressed in consistent units. +// The model includes the actuator delay and the measurement/filter path at that rate. +#define FW_TUNE_MODEL_TERMS 6 + +typedef struct { + float a[2]; + float b[3]; + float offset; + float samplePeriod; +} fwTuneModel_t; + +typedef struct { + float r[FW_TUNE_MODEL_TERMS][FW_TUNE_MODEL_TERMS]; + float z[FW_TUNE_MODEL_TERMS]; + float u[3]; + float y[2]; + float samplePeriod; + uint32_t samples; + uint8_t history; + bool invalid; +} fwTuneModelFit_t; + +typedef struct { + float y[2]; + float u[3]; +} fwTuneModelState_t; + +typedef struct { + fwTuneModel_t model; + fwTuneModelState_t state; + float squaredError; + float mean; + float squaredDeviation; + uint32_t samples; + uint8_t history; + bool invalid; +} fwTuneModelValidation_t; + +void fwTuneModelInit(fwTuneModelFit_t *fit, float samplePeriod); +bool fwTuneModelAdd(fwTuneModelFit_t *fit, float input, float output); +bool fwTuneModelSolve(const fwTuneModelFit_t *fit, fwTuneModel_t *model); +bool fwTuneModelIsStable(const fwTuneModel_t *model); +bool fwTuneModelStaticGain(const fwTuneModel_t *model, float *gain); +// Uses previous inputs: input is the command that will be held after this observation. +float fwTuneModelPredict(const fwTuneModel_t *model, fwTuneModelState_t *state, float input); + +// Held-out, free-running prediction: measured outputs initialise the history only. +// A low prediction error is necessary but not sufficient for controller synthesis. +void fwTuneModelValidationInit(fwTuneModelValidation_t *validation, const fwTuneModel_t *model); +bool fwTuneModelValidateSample(fwTuneModelValidation_t *validation, float input, float output); +bool fwTuneModelValidationError(const fwTuneModelValidation_t *validation, float *normalisedError); diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 60bde216173..3b631a8256e 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -26,6 +26,8 @@ set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c" "drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c" "sensors/gyro.c") +set_property(SOURCE fw_tune_model_unittest.cc PROPERTY depends "flight/fw_tune_model.c") + set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") set_property(SOURCE navigation_fixedwing_turn_math_unittest.cc PROPERTY depends diff --git a/src/test/unit/fw_tune_model_unittest.cc b/src/test/unit/fw_tune_model_unittest.cc new file mode 100644 index 00000000000..3b666c8a607 --- /dev/null +++ b/src/test/unit/fw_tune_model_unittest.cc @@ -0,0 +1,206 @@ +/* + * This file is part of INAV. + * SPDX-License-Identifier: GPL-3.0-or-later + */ +#include +#include +#include + +extern "C" { +#include "flight/fw_tune_model.h" +} + +namespace { +// Independent continuous-time two-lag plant integrated at 1 kHz. +// The estimator sees only 50 Hz actuator commands and observations. +struct Plant { + double servo = 0; + double rate = 0; + double update(double command) { + for (unsigned i = 0; i < 20; i++) { + servo += (command - servo) * (1.0 - std::exp(-0.001 / 0.055)); + rate += (2.0 * servo - rate) * (1.0 - std::exp(-0.001 / 0.22)); + } + return rate; + } +}; + +float excitation(unsigned sample) { + return 0.2f * std::sin(sample * 0.13f) + 0.12f * std::sin(sample * 0.61f) + + 0.06f * std::sin(sample * 1.23f); +} +} + +TEST(FwTuneModel, IdentifiesIndependentContinuousPlantAndPredictsHeldOutInput) +{ + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + Plant plant; + float observation = 0; + for (unsigned i = 0; i < 1500; i++) { + const float input = excitation(i); + ASSERT_TRUE(fwTuneModelAdd(&fit, input, observation)); + observation = plant.update(input); + } + fwTuneModel_t model = {}; + ASSERT_TRUE(fwTuneModelSolve(&fit, &model)); + float gain = 0; + ASSERT_TRUE(fwTuneModelStaticGain(&model, &gain)); + EXPECT_NEAR(gain, 2.0f, 0.005f); + + plant = Plant(); + observation = 0; + fwTuneModelState_t state = {}; + for (unsigned i = 0; i < 1000; i++) { + const float input = i % 150 < 75 ? 0.3f : -0.2f; + const float prediction = fwTuneModelPredict(&model, &state, input); + ASSERT_NEAR(prediction, observation, 0.005f) << "sample " << i; + observation = plant.update(input); + } +} + +TEST(FwTuneModel, FitsConstantObservationOffsetWithoutChangingStaticGain) +{ + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + Plant plant; + float observation = 0; + for (unsigned i = 0; i < 2000; i++) { + const float input = excitation(i); + ASSERT_TRUE(fwTuneModelAdd(&fit, input, observation + 0.4f)); + observation = plant.update(input); + } + fwTuneModel_t model = {}; + ASSERT_TRUE(fwTuneModelSolve(&fit, &model)); + float gain = 0; + ASSERT_TRUE(fwTuneModelStaticGain(&model, &gain)); + EXPECT_NEAR(gain, 2.0f, 0.01f); + EXPECT_NEAR(model.offset / (1 + model.a[0] + model.a[1]), 0.4f, 0.005f); +} + +TEST(FwTuneModel, RejectsInsufficientAndConstantExcitationWithoutWritingResult) +{ + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + fwTuneModel_t model = {}; + model.offset = 42; + EXPECT_FALSE(fwTuneModelSolve(&fit, &model)); + for (unsigned i = 0; i < 500; i++) { + ASSERT_TRUE(fwTuneModelAdd(&fit, 0.2f, 0.4f)); + } + EXPECT_FALSE(fwTuneModelSolve(&fit, &model)); + EXPECT_EQ(model.offset, 42); +} + +TEST(FwTuneModel, InvalidSamplesLatchFailureUntilReset) +{ + for (float value : {std::numeric_limits::quiet_NaN(), + std::numeric_limits::infinity(), -10001.0f}) { + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + EXPECT_FALSE(fwTuneModelAdd(&fit, value, 0)); + EXPECT_FALSE(fwTuneModelAdd(&fit, 0, 0)); + fwTuneModelInit(&fit, 0.02f); + EXPECT_TRUE(fwTuneModelAdd(&fit, 0, 0)); + EXPECT_FALSE(fwTuneModelAdd(&fit, 0, value)); + } +} + +TEST(FwTuneModel, RejectsInvalidSamplePeriod) +{ + for (float period : {0.0f, -0.02f, std::numeric_limits::quiet_NaN()}) { + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, period); + EXPECT_FALSE(fwTuneModelAdd(&fit, 0, 0)); + } +} + +TEST(FwTuneModel, RejectsMarginalUnstableAndReversedPlants) +{ + fwTuneModel_t model = {{-1.5f, 0.56f}, {0.03f, 0.03f, 0}, 0, 0.02f}; + EXPECT_TRUE(fwTuneModelIsStable(&model)); + float gain = 42; + EXPECT_TRUE(fwTuneModelStaticGain(&model, &gain)); + EXPECT_NEAR(gain, 1, 0.00001f); + model.b[0] = -0.1f; + EXPECT_FALSE(fwTuneModelStaticGain(&model, &gain)); + model.a[0] = -1.6f; + model.a[1] = 0.6f; + EXPECT_FALSE(fwTuneModelIsStable(&model)); + model.a[0] = -2.0f; + EXPECT_FALSE(fwTuneModelIsStable(&model)); + model.a[0] = 2.0f; + EXPECT_FALSE(fwTuneModelIsStable(&model)); + model.a[0] = 0; + model.a[1] = 1; + EXPECT_FALSE(fwTuneModelIsStable(&model)); +} + +TEST(FwTuneModel, IndependentValidationDetectsWrongGainAndLongDelay) +{ + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + Plant plant; + float observation = 0; + for (unsigned i = 0; i < 1500; i++) { + const float input = excitation(i); + ASSERT_TRUE(fwTuneModelAdd(&fit, input, observation)); + observation = plant.update(input); + } + fwTuneModel_t model = {}; + ASSERT_TRUE(fwTuneModelSolve(&fit, &model)); + + for (unsigned scenario = 0; scenario < 3; scenario++) { + fwTuneModelValidation_t validation; + fwTuneModelValidationInit(&validation, &model); + plant = Plant(); + observation = 0; + float history[10] = {}; + for (unsigned i = 0; i < 1000; i++) { + const float input = excitation(i + 3000); + const float measured = scenario == 1 ? observation * 1.5f : observation; + ASSERT_TRUE(fwTuneModelValidateSample(&validation, input, measured)); + const float delayed = history[i % 10]; + history[i % 10] = input; + observation = plant.update(scenario == 2 ? delayed : input); + } + float error = -1; + ASSERT_TRUE(fwTuneModelValidationError(&validation, &error)); + if (scenario == 0) { + EXPECT_LT(error, 0.005f); + } else { + EXPECT_GT(error, 0.2f); + } + } +} + +TEST(FwTuneModel, ValidationNeedsIndependentVariationAndFiniteObservations) +{ + const fwTuneModel_t model = {{-1.5f, 0.56f}, {0.03f, 0.03f, 0}, 0, 0.02f}; + fwTuneModelValidation_t validation; + fwTuneModelValidationInit(&validation, &model); + float error = 42; + EXPECT_FALSE(fwTuneModelValidationError(&validation, &error)); + for (unsigned i = 0; i < 500; i++) { + ASSERT_TRUE(fwTuneModelValidateSample(&validation, 0, 0)); + } + EXPECT_FALSE(fwTuneModelValidationError(&validation, &error)); + EXPECT_EQ(error, 42); + EXPECT_FALSE(fwTuneModelValidateSample(&validation, 0, std::numeric_limits::infinity())); + EXPECT_FALSE(fwTuneModelValidateSample(&validation, 0, 0)); +} + +TEST(FwTuneModel, ReversedResponseDoesNotProduceUsableModel) +{ + fwTuneModelFit_t fit; + fwTuneModelInit(&fit, 0.02f); + Plant plant; + float observation = 0; + for (unsigned i = 0; i < 1500; i++) { + const float input = excitation(i); + ASSERT_TRUE(fwTuneModelAdd(&fit, input, -observation)); + observation = plant.update(input); + } + fwTuneModel_t model = {}; + EXPECT_FALSE(fwTuneModelSolve(&fit, &model)); +}