Draft: feat(ekf2): airspeed fusion for wind estimation in rotary-wing flight

[JonasPerolini]

Estimating the wind in MC mode is important to avoid flying out of specs as soon as possible to prevent flying out of specs and attempting a front transition. (There is not always an anemometer on site, and wind can change with altitude).

There is a multicopter wind estimator: https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf#mc_wind_estimation_using_drag, however, it did not work for our VTOL platform, the wind estimated was not precise enough.

Solution

Allow airspeed fusion for wind estimation in multicopter mode, mainly for VTOL flight with pusher. Fusion is gated by lateral specific force (must be aligned with the wind) and only updates the wind states, so it cannot act as navigation aiding.

Params:

• EKF2_ASPD_MC enable MC wind estimation
• EKF2_ASPD_MC_LIM Lateral specific force limit for multicopter airspeed fusion

Tests

Added tests for parameter gating, wind convergence, alignment rejection, and dead-reckoning behavior.

To discuss:

How do we know that the UAV is aligned with the wind (zero sideslip assumption)?

Sideslip fusion assumes that the UAV is aligned with the wind and sets the observation to zero. This works well in FW but in MC we need a way to know whether the uav is aligned with the wind or not. This PR reuses the lateral specific force as defined in the drag-based MC wind estimator https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf#mc_wind_estimation_using_drag. One limitation is when flying sideways in the direction of the wind, in that case the y-specific force is small.

@bresch could you have a look please? Do you think there is a better way to determine whether the UAV is aligned with the wind?

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 640 byte (0.03 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +624  +0.0%    +624    .text
    +0.1%    +136  +0.1%    +136    g_cromfs_image
   -99.8%     +76 -99.8%     +76    [68 Others]
    +1.1%     +64  +1.1%     +64    EKF2::EKF2()
    +4.3%     +64  +4.3%     +64    Ekf::predictState()
    +5.7%     +60  +5.7%     +60    Ekf::controlAirDataFusion()
     +14%     +48   +14%     +48    Ekf::fuseSideslip()
    +1.2%     +44  +1.2%     +44    Ekf::Ekf()
    +6.7%     +36  +6.7%     +36    Ekf::fuseAirspeed()
    +1.2%     +24  +1.2%     +24    EKF2::PublishAidSourceStatus()
    +1.4%     +24  +1.4%     +24    EKF2::updateParamsImpl()
    +3.1%     +16  +3.1%     +16    Ekf::controlFakePosFusion()
    +2.3%     +16  +2.3%     +16    Ekf::updateHorizontalDeadReckoningstatus()
    +0.0%     +16  +0.0%     +16    [section .text]
    +0.1%     +16  +0.1%     +16    px4::parameters
    +1.3%     +12  +1.3%     +12    EKF2::PublishStatusFlags()
    +0.7%     +12  +0.7%     +12    Ekf::controlExternalVisionFusion()
    +3.1%     +12  +3.1%     +12    Ekf::controlFakeHgtFusion()
    +0.7%     +12  +0.7%     +12    Ekf::controlGnssHeightFusion()
    -0.6%     -16  -0.6%     -16    Ekf::controlRangeHaglFusion()
    -0.2%     -24  -0.2%     -24    Ekf::predictCovariance()
    -7.7%     -24  -7.7%     -24    Ekf::stopGnssFusion()
  +0.1%     +16  +0.1%     +16    .ramfunc
    +1.2%      +4  +1.2%      +4    Ekf::measurementUpdate()
    +1.9%      +4  +1.9%      +4    param_get
     +33%      +4   +33%      +4    param_get_index
    +9.1%      +4  +9.1%      +4    param_get_system_default_value
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +20  [ = ]       0    .debug_frame
  +0.0% +5.22Ki  [ = ]       0    .debug_info
  +0.0%    +712  [ = ]       0    .debug_line
   -50.0%      -3  [ = ]       0    [Unmapped]
    +0.0%    +715  [ = ]       0    [section .debug_line]
  -0.0%    -144  [ = ]       0    .debug_loclists
  +0.0%    +289  [ = ]       0    .debug_rnglists
  +0.1% +2.06Ki  [ = ]       0    .debug_str
  +0.0%     +16  [ = ]       0    .symtab
   -33.3%     -16  [ = ]       0    ConstLayer::contains()
     +50%     +16  [ = ]       0    ConstLayer::store()
   -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
     +33%     +16  [ = ]       0    EKF2::PublishEventFlags()
   -50.0%     -16  [ = ]       0    EKF2::PublishGpsStatus()
    +100%     +16  [ = ]       0    EKF2::PublishWindEstimate()
    +100%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
     +11%     +16  [ = ]       0    EKF2::handleSensorFusionCommand()
   -50.0%     -16  [ = ]       0    EKF2::updateParamsImpl()
   -25.0%     -16  [ = ]       0    Ekf::controlEvPosFusion()
   -50.0%     -16  [ = ]       0    Ekf::controlGnssPosFusion()
     +25%     +16  [ = ]       0    Ekf::controlGpsFusion()
    +100%     +16  [ = ]       0    Ekf::resetMagBiasCov()
    +100%     +16  [ = ]       0    Ekf::resetMagEarthCov()
   -25.0%     -16  [ = ]       0    Ekf::updateHorizontalDeadReckoningstatus()
    +100%     +16  [ = ]       0    Ekf::updateVerticalDeadReckoningStatus()
     +33%     +16  [ = ]       0    EstimatorInterface::setSystemFlagData()
    -0.3%     -32  [ = ]       0    [section .symtab]
   -11.8%     -32  [ = ]       0    ___ZL19param_get_cplusplustPf.isra.0_veneer
     +67%     +32  [ = ]       0    __strcmp_veneer
   +43% +3.38Ki  [ = ]       0    [Unmapped]
  +0.0% +12.2Ki  +0.0%    +640    TOTAL

px4_fmu-v6x [Total VM Diff: 616 byte (0.03 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +616  +0.0%    +616    .text
    +0.1%    +160  +0.1%    +160    g_cromfs_image
    +1.1%     +64  +1.1%     +64    EKF2::EKF2()
    +4.3%     +64  +4.3%     +64    Ekf::predictState()
    +5.7%     +60  +5.7%     +60    Ekf::controlAirDataFusion()
     +14%     +48   +14%     +48    Ekf::fuseSideslip()
    +1.2%     +44  +1.2%     +44    Ekf::Ekf()
    +0.0%     +40  +0.0%     +40    [section .text]
    +6.7%     +36  +6.7%     +36    Ekf::fuseAirspeed()
    +1.2%     +24  +1.2%     +24    EKF2::PublishAidSourceStatus()
    +1.4%     +24  +1.4%     +24    EKF2::updateParamsImpl()
   -99.9%     +20 -99.9%     +20    [52 Others]
    +3.1%     +16  +3.1%     +16    Ekf::controlFakePosFusion()
    +2.3%     +16  +2.3%     +16    Ekf::updateHorizontalDeadReckoningstatus()
    +0.1%     +16  +0.1%     +16    px4::parameters
    +1.3%     +12  +1.3%     +12    EKF2::PublishStatusFlags()
    +0.7%     +12  +0.7%     +12    Ekf::controlExternalVisionFusion()
    +3.1%     +12  +3.1%     +12    Ekf::controlFakeHgtFusion()
    +0.7%     +12  +0.7%     +12    Ekf::controlGnssHeightFusion()
    -0.6%     -16  -0.6%     -16    Ekf::controlRangeHaglFusion()
    -0.2%     -24  -0.2%     -24    Ekf::predictCovariance()
    -7.7%     -24  -7.7%     -24    Ekf::stopGnssFusion()
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +20  [ = ]       0    .debug_frame
  +0.0% +5.09Ki  [ = ]       0    .debug_info
  +0.0%    +694  [ = ]       0    .debug_line
   -50.0%      -1  [ = ]       0    [Unmapped]
    +0.0%    +695  [ = ]       0    [section .debug_line]
  +0.0%     +77  [ = ]       0    .debug_loclists
  +0.1%    +284  [ = ]       0    .debug_rnglists
   -33.3%      -1  [ = ]       0    [Unmapped]
    +0.1%    +285  [ = ]       0    [section .debug_rnglists]
  +0.1% +2.03Ki  [ = ]       0    .debug_str
  +0.0%     +16  [ = ]       0    .symtab
   -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
     +33%     +16  [ = ]       0    EKF2::PublishEventFlags()
   -50.0%     -16  [ = ]       0    EKF2::PublishGpsStatus()
    +100%     +16  [ = ]       0    EKF2::PublishWindEstimate()
    +100%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
     +11%     +16  [ = ]       0    EKF2::handleSensorFusionCommand()
   -50.0%     -16  [ = ]       0    EKF2::updateParamsImpl()
   -25.0%     -16  [ = ]       0    Ekf::controlEvPosFusion()
   -50.0%     -16  [ = ]       0    Ekf::controlGnssPosFusion()
     +25%     +16  [ = ]       0    Ekf::controlGpsFusion()
    +100%     +16  [ = ]       0    Ekf::resetMagBiasCov()
    +100%     +16  [ = ]       0    Ekf::resetMagEarthCov()
   -25.0%     -16  [ = ]       0    Ekf::updateHorizontalDeadReckoningstatus()
    +100%     +16  [ = ]       0    Ekf::updateVerticalDeadReckoningStatus()
     +33%     +16  [ = ]       0    EstimatorInterface::setSystemFlagData()
    -0.3%     -32  [ = ]       0    [section .symtab]
 -12.6%    -616  [ = ]       0    [Unmapped]
  +0.0% +8.24Ki  +0.0%    +616    TOTAL

Updated: 2026-07-13T15:50:46

[mbjd]

Not sure if that would be much harder implementation wise, but if we are already using the lateral specific force to check the zero-sideslip assumption (and not fusing any airspeed / sideslip at all if too much sideslip) would it not make more sense to:

• always fuse airspeed only in the longitudinal direction (so we don't need any zero sideslip assumption)
  • maybe not if airspeed negative, or only with higher noise variance
• laterally, directly fuse a wind-relative velocity derived from specific force + drag model?

Laterally the measurement might still be quite inaccurate but if heading changes a bit we could inform the wind estimate from airspeed in both directions without relying on zero-sideslip

[ThomasRigi]

I think we should keep the no-sideslip condition when fusing airspeed measurements from a pitot. Most (basic) pitots only work properly when they're well aligned with the airflow. Anything else messes with the measured pressure.

I therefore think we should also add a check on max angular rates. In our VTOL for example the pitot is on the nose. Whenever the drone yaws or pitches, we get unusable readings. Most of the time the readings are below what we would set for the threshold, but I think it's safer to have a dedicated mechanism to reject phases where we know we don't read the correct values.
Depending on where the pitot is placed on the drone, it might be more affected by roll rather than pitch (e.g. a pitot on the wingtip). So I think it would be best to have separate threshold parameters for the three axes.

I like the idea of using airspeed measurements only for the x-body direction of the wind (when aligned), and use the specific drag estimation for the y-direction. I would rather do it in a separate PR though, so as to not overload this one. What do you think ?

[mbjd]

Makes sense. @RomanBapst is also experimenting with a pure MC version of airspeed-deadreckoning, with airspeed derived from lateral specific force + drag model. Whoever is first ask each other for review :)