Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : 
       3             : #include "AP_NavEKF3.h"
       4             : #include "AP_NavEKF3_core.h"
       5             : #include <AP_DAL/AP_DAL.h>
       6             : 
       7             : /********************************************************
       8             : *                   RESET FUNCTIONS                     *
       9             : ********************************************************/
      10             : 
      11             : /********************************************************
      12             : *                   FUSE MEASURED_DATA                  *
      13             : ********************************************************/
      14             : 
      15             : /*
      16             :  * Fuse true airspeed measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
      17             :  * The script file used to generate these and other equations in this filter can be found here:
      18             :  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
      19             : */
      20      583622 : void NavEKF3_core::FuseAirspeed()
      21             : {
      22             :     // declarations
      23             :     ftype SH_TAS[3];
      24             :     ftype SK_TAS[2];
      25             :     Vector24 H_TAS = {};
      26             : 
      27             :     // copy required states to local variable names
      28      583622 :     const ftype vn = stateStruct.velocity.x;
      29      583622 :     const ftype ve = stateStruct.velocity.y;
      30      583622 :     const ftype vd = stateStruct.velocity.z;
      31      583622 :     const ftype vwn = stateStruct.wind_vel.x;
      32      583622 :     const ftype vwe = stateStruct.wind_vel.y;
      33             : 
      34             :     // calculate the predicted airspeed
      35      583622 :     const ftype VtasPred = norm((ve - vwe) , (vn - vwn) , vd);
      36             :     // perform fusion of True Airspeed measurement
      37      583622 :     if (VtasPred > 1.0f)
      38             :     {
      39             :         // calculate observation innovation and variance
      40      574689 :         innovVtas = VtasPred - tasDataDelayed.tas;
      41             : 
      42             :         // calculate observation jacobians
      43      574689 :         SH_TAS[0] = 1.0f/VtasPred;
      44      574689 :         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
      45      574689 :         SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
      46             :         H_TAS[4] = SH_TAS[2];
      47             :         H_TAS[5] = SH_TAS[1];
      48      574689 :         H_TAS[6] = vd*SH_TAS[0];
      49             :         H_TAS[22] = -SH_TAS[2];
      50             :         H_TAS[23] = -SH_TAS[1];
      51             :         // calculate Kalman gains
      52      574689 :         ftype temp = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
      53      574689 :         if (temp >= tasDataDelayed.tasVariance) {
      54      574689 :             SK_TAS[0] = 1.0f / temp;
      55      574689 :             faultStatus.bad_airspeed = false;
      56             :         } else {
      57             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
      58             :             // we reset the covariance matrix and try again next measurement
      59           0 :             CovarianceInit();
      60           0 :             faultStatus.bad_airspeed = true;
      61             :             return;
      62             :         }
      63             :         SK_TAS[1] = SH_TAS[1];
      64             : 
      65             :         uint32_t kalman_mask = 0; // values to calculate in Kfusion (others are set to zero)
      66             : 
      67      574689 :         if (tasDataDelayed.allowFusion && !airDataFusionWindOnly) {
      68             :             kalman_mask = (1<<10)-1;
      69             :         }
      70             : 
      71      574689 :         if (tasDataDelayed.allowFusion && !inhibitDelAngBiasStates && !airDataFusionWindOnly) {
      72       11157 :             kalman_mask |= (1<<10) | (1<<11) | (1<<12);
      73             :         }
      74             : 
      75      574689 :         if (tasDataDelayed.allowFusion && !inhibitDelVelBiasStates && !airDataFusionWindOnly) {
      76       44628 :             for (uint8_t index = 0; index < 3; index++) {
      77       33471 :                 const uint8_t stateIndex = index + 13;
      78       33471 :                 if (!dvelBiasAxisInhibit[index]) {
      79       33471 :                     kalman_mask |= (1<<stateIndex);
      80             :                 }
      81             :             }
      82             :         }
      83             : 
      84             :         // zero Kalman gains to inhibit magnetic field state estimation
      85      574689 :         if (tasDataDelayed.allowFusion && !inhibitMagStates && !airDataFusionWindOnly) {
      86       11155 :             kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
      87             :         }
      88             : 
      89      574689 :         if (tasDataDelayed.allowFusion && !inhibitWindStates && !treatWindStatesAsTruth) {
      90      564546 :             kalman_mask |= (1<<22) | (1<<23);
      91             :         }
      92             : 
      93    14367225 :         for (auto i=0; i<24; i++) {
      94             :             ftype res = 0;
      95    13792536 :             if (kalman_mask & (1<<i)) {
      96     1374534 :                 res = SK_TAS[0]*(P[i][4]*SH_TAS[2] - P[i][22]*SH_TAS[2] + P[i][5]*SK_TAS[1] - P[i][23]*SK_TAS[1] + P[i][6]*vd*SH_TAS[0]);
      97             :             }
      98    13792536 :             Kfusion[i] = res;
      99             :         }
     100             : 
     101             :         // calculate measurement innovation variance
     102      574689 :         varInnovVtas = 1.0f/SK_TAS[0];
     103             : 
     104             :         // calculate the innovation consistency test ratio
     105      574689 :         tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
     106             : 
     107             :         // fail if the ratio is > 1, but don't fail if bad IMU data
     108      574689 :         const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata;
     109      574689 :         tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
     110      574689 :         if (!isConsistent) {
     111        1427 :             lastTasFailTime_ms = imuSampleTime_ms;
     112             :         } else {
     113      573262 :             lastTasFailTime_ms = 0;
     114             :         }
     115             : 
     116             :         // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
     117      574689 :         if (tasDataDelayed.allowFusion && (isConsistent || (tasTimeout && posTimeout))) {
     118             :             // restart the counter
     119      573263 :             lastTasPassTime_ms = imuSampleTime_ms;
     120             : 
     121             :             // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
     122             :             // the zero elements of H to reduce the number of operations.
     123    14331575 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     124             :                 // j as the inner loop allows the compiler to hoist the KH product
     125             :                 // to save computation, and do the inner indexing more efficiently.
     126   343957800 :                 for (unsigned j = 0; j<=stateIndexLim; j++) {
     127             :                     ftype res = 0;
     128   330199488 :                     res += (Kfusion[i] * H_TAS[4]) * P[4][j];
     129   330199488 :                     res += (Kfusion[i] * H_TAS[5]) * P[5][j];
     130   330199488 :                     res += (Kfusion[i] * H_TAS[6]) * P[6][j];
     131   330199488 :                     res += (Kfusion[i] * H_TAS[22]) * P[22][j];
     132   330199488 :                     res += (Kfusion[i] * H_TAS[23]) * P[23][j];
     133   330199488 :                     KHP[i][j] = res;
     134             :                 }
     135             :             }
     136             : 
     137             :             // finish fusion from KHP and Kfusion
     138      573263 :             FinishFusion(innovVtas, true); // forcing fusion is probably a bug
     139             :         }
     140             :     }
     141             : }
     142             : 
     143             : // select fusion of true airspeed measurements
     144    19965274 : void NavEKF3_core::SelectTasFusion()
     145             : {
     146             :     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     147             :     // If so, don't fuse measurements on this time step to reduce frame over-runs
     148             :     // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements
     149    19965274 :     if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) {
     150     1204036 :         airSpdFusionDelayed = true;
     151     1204036 :         return;
     152             :     } else {
     153    18761238 :         airSpdFusionDelayed = false;
     154             :     }
     155             : 
     156             :     // get true airspeed measurement
     157    18761238 :     readAirSpdData();
     158             : 
     159             :     // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
     160             : 
     161    18761238 :     if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
     162      583622 :         FuseAirspeed();
     163      583622 :         tasDataToFuse = false;
     164      583622 :         prevTasStep_ms = imuSampleTime_ms;
     165             :     }
     166             : }
     167             : 
     168             : 
     169             : // select fusion of synthetic sideslip measurements or body frame drag
     170             : // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
     171             : // body frame drag only works for bluff body multi rotor vehices with thrust forces aligned with the Z axis
     172             : // it requires a stable wind for best results and should not be used for aerobatic flight
     173    19965274 : void NavEKF3_core::SelectBetaDragFusion()
     174             : {
     175             :     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     176             :     // If so, don't fuse measurements on this time step to reduce frame over-runs
     177             :     // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
     178    19965274 :     if (magFusePerformed && dtIMUavg < 0.005f && !sideSlipFusionDelayed) {
     179     1204036 :         sideSlipFusionDelayed = true;
     180     1204036 :         return;
     181             :     } else {
     182    18761238 :         sideSlipFusionDelayed = false;
     183             :     }
     184             : 
     185             :     // set true when the fusion time interval has triggered
     186    18761238 :     bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms);
     187             : 
     188             :     // use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning
     189    18761238 :     bool is_dead_reckoning = ((imuSampleTime_ms - lastGpsPosPassTime_ms) > frontend->deadReckonDeclare_ms) &&
     190      229081 :                              ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms);
     191    18761238 :     const bool noYawSensor = !use_compass() && !using_noncompass_for_yaw();
     192    18761238 :     const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning;
     193             : 
     194             :     // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
     195    18761238 :     const bool f_beta_feasible = (assume_zero_sideslip() && !inhibitWindStates);
     196             : 
     197             :     // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
     198    18761238 :     if (f_beta_feasible && f_timeTrigger) {
     199             :         // unless air data is required to constrain drift, it is only used to update wind state estimates
     200      595859 :         if (f_required || (frontend->_betaMask & (1<<0))) {
     201             :             // we are required to correct all states
     202        5929 :             airDataFusionWindOnly = false;
     203             :         } else {
     204             :             // we are required to correct only wind states
     205      589930 :             airDataFusionWindOnly = true;
     206             :         }
     207      595859 :         FuseSideslip();
     208      595859 :         prevBetaDragStep_ms = imuSampleTime_ms;
     209             :     }
     210             : 
     211             : #if EK3_FEATURE_DRAG_FUSION
     212             :     // fusion of XY body frame aero specific forces is done at a slower rate and only if alternative methods of wind estimation are not available
     213    18761238 :     if (!inhibitWindStates && storedDrag.recall(dragSampleDelayed,imuDataDelayed.time_ms)) {
     214        5366 :         FuseDragForces();
     215             :     }
     216    18761238 :     dragTimeout = (imuSampleTime_ms - lastDragPassTime_ms) > frontend->dragFailTimeLimit_ms;
     217             : #endif
     218             : }
     219             : 
     220             : /*
     221             :  * Fuse sythetic sideslip measurement of zero using explicit algebraic equations generated with Matlab symbolic toolbox.
     222             :  * The script file used to generate these and other equations in this filter can be found here:
     223             :  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
     224             : */
     225      595859 : void NavEKF3_core::FuseSideslip()
     226             : {
     227             :     // declarations
     228             :     const ftype R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
     229             :     Vector13 SH_BETA;
     230             :     Vector8 SK_BETA;
     231      595859 :     Vector3F vel_rel_wind;
     232             :     Vector24 H_BETA;
     233             : 
     234             :     // copy required states to local variable names
     235      595859 :     const ftype q0 = stateStruct.quat[0];
     236      595859 :     const ftype q1 = stateStruct.quat[1];
     237      595859 :     const ftype q2 = stateStruct.quat[2];
     238      595859 :     const ftype q3 = stateStruct.quat[3];
     239      595859 :     const ftype vn = stateStruct.velocity.x;
     240      595859 :     const ftype ve = stateStruct.velocity.y;
     241      595859 :     const ftype vd = stateStruct.velocity.z;
     242      595859 :     const ftype vwn = stateStruct.wind_vel.x;
     243      595859 :     const ftype vwe = stateStruct.wind_vel.y;
     244             : 
     245             :     // calculate predicted wind relative velocity in NED
     246      595859 :     vel_rel_wind.x = vn - vwn;
     247      595859 :     vel_rel_wind.y = ve - vwe;
     248      595859 :     vel_rel_wind.z = vd;
     249             : 
     250             :     // rotate into body axes
     251      595859 :     vel_rel_wind = prevTnb * vel_rel_wind;
     252             : 
     253             :     // perform fusion of assumed sideslip  = 0
     254      595859 :     if (vel_rel_wind.x > 5.0f)
     255             :     {
     256             :         // Calculate observation jacobians
     257      575267 :         SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
     258      575267 :         if (fabsF(SH_BETA[0]) <= 1e-9f) {
     259           0 :             faultStatus.bad_sideslip = true;
     260           0 :             return;
     261             :         } else {
     262      575267 :             faultStatus.bad_sideslip = false;
     263             :         }
     264      575267 :         SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
     265             :         SH_BETA[2] = vn - vwn;
     266             :         SH_BETA[3] = ve - vwe;
     267      575267 :         SH_BETA[4] = 1/sq(SH_BETA[0]);
     268      575267 :         SH_BETA[5] = 1/SH_BETA[0];
     269      575267 :         SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
     270      575267 :         SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
     271      575267 :         SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd;
     272      575267 :         SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd;
     273      575267 :         SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd;
     274      575267 :         SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd;
     275             :         SH_BETA[12] = 2*q0*q3;
     276             : 
     277      575267 :         H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
     278      575267 :         H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
     279      575267 :         H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
     280      575267 :         H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
     281      575267 :         H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
     282      575267 :         H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
     283      575267 :         H_BETA[6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
     284             :         for (uint8_t i=7; i<=21; i++) {
     285             :             H_BETA[i] = 0.0f;
     286             :         }
     287      575267 :         H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
     288      575267 :         H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6];
     289             : 
     290             :         // Calculate Kalman gains
     291      575267 :         ftype temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))));
     292      575267 :         if (temp >= R_BETA) {
     293      575267 :             SK_BETA[0] = 1.0f / temp;
     294      575267 :             faultStatus.bad_sideslip = false;
     295             :         } else {
     296             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
     297             :             // we reset the covariance matrix and try again next measurement
     298           0 :             CovarianceInit();
     299           0 :             faultStatus.bad_sideslip = true;
     300           0 :             return;
     301             :         }
     302             :         SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
     303             :         SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
     304             :         SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
     305             :         SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
     306             :         SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
     307             :         SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
     308             :         SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
     309             : 
     310             :         uint32_t kalman_mask = 0; // values to calculate in Kfusion (others are set to zero)
     311             : 
     312      575267 :         if (!airDataFusionWindOnly) {
     313             :             kalman_mask = (1<<10)-1;
     314             :         }
     315             : 
     316      575267 :         if (!inhibitDelAngBiasStates && !airDataFusionWindOnly) {
     317        5924 :             kalman_mask |= (1<<10) | (1<<11) | (1<<12);
     318             :         }
     319             : 
     320      575267 :         if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
     321       23696 :             for (uint8_t index = 0; index < 3; index++) {
     322       17772 :                 const uint8_t stateIndex = index + 13;
     323       17772 :                 if (!dvelBiasAxisInhibit[index]) {
     324       17772 :                     kalman_mask |= (1<<stateIndex);
     325             :                 }
     326             :             }
     327             :         }
     328             : 
     329             :         // zero Kalman gains to inhibit magnetic field state estimation
     330      575267 :         if (!inhibitMagStates && !airDataFusionWindOnly) {
     331        5924 :             kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
     332             :         }
     333             : 
     334      575267 :         if (!inhibitWindStates && !treatWindStatesAsTruth) {
     335      570573 :             kalman_mask |= (1<<22) | (1<<23);
     336             :         }
     337             : 
     338    14381675 :         for (auto i=0; i<24; i++) {
     339             :             ftype res = 0;
     340    13806408 :             if (kalman_mask & (1<<i)) {
     341     1271474 :                 res = SK_BETA[0]*(P[i][0]*SK_BETA[5] + P[i][1]*SK_BETA[4] - P[i][4]*SK_BETA[1] + P[i][5]*SK_BETA[2] + P[i][2]*SK_BETA[6] + P[i][6]*SK_BETA[3] - P[i][3]*SK_BETA[7] + P[i][22]*SK_BETA[1] - P[i][23]*SK_BETA[2]);
     342             :             }
     343    13806408 :             Kfusion[i] = res;
     344             :         }
     345             : 
     346             :         // calculate predicted sideslip angle and innovation using small angle approximation
     347      575267 :         innovBeta = constrain_ftype(vel_rel_wind.y / vel_rel_wind.x, -0.5f, 0.5f);
     348             : 
     349             :         // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
     350             :         // the zero elements of H to reduce the number of operations.
     351    14381675 :         for (unsigned i = 0; i<=stateIndexLim; i++) {
     352             :             // j as the inner loop allows the compiler to hoist the KH product
     353             :             // to save computation, and do the inner indexing more efficiently.
     354   345160200 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     355             :                 ftype res = 0;
     356   331353792 :                 res += (Kfusion[i] * H_BETA[0]) * P[0][j];
     357   331353792 :                 res += (Kfusion[i] * H_BETA[1]) * P[1][j];
     358   331353792 :                 res += (Kfusion[i] * H_BETA[2]) * P[2][j];
     359   331353792 :                 res += (Kfusion[i] * H_BETA[3]) * P[3][j];
     360   331353792 :                 res += (Kfusion[i] * H_BETA[4]) * P[4][j];
     361   331353792 :                 res += (Kfusion[i] * H_BETA[5]) * P[5][j];
     362   331353792 :                 res += (Kfusion[i] * H_BETA[6]) * P[6][j];
     363   331353792 :                 res += (Kfusion[i] * H_BETA[22]) * P[22][j];
     364   331353792 :                 res += (Kfusion[i] * H_BETA[23]) * P[23][j];
     365   331353792 :                 KHP[i][j] = res;
     366             :             }
     367             :         }
     368             : 
     369             :         // finish fusion from KHP and Kfusion
     370      575267 :         FinishFusion(innovBeta, true); // forcing fusion is probably a bug
     371             :     }
     372             : }
     373             : 
     374             : #if EK3_FEATURE_DRAG_FUSION
     375             : /*
     376             :  * Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy.
     377             :  * See derivation/generate_2.py for derivation
     378             :  * Output for change reference: derivation/generated/acc_bf_generated.cpp
     379             : */
     380        5366 : void NavEKF3_core::FuseDragForces()
     381             : {
     382             :     // drag model parameters
     383        5366 :     const ftype bcoef_x = frontend->_ballisticCoef_x.get();
     384        5366 :     const ftype bcoef_y = frontend->_ballisticCoef_y.get();
     385        5366 :     const ftype mcoef = frontend->_momentumDragCoef.get();
     386        5366 :     const bool using_bcoef_x = bcoef_x > 1.0f;
     387        5366 :     const bool using_bcoef_y = bcoef_y > 1.0f;
     388        5366 :     const bool using_mcoef = mcoef > 0.001f;
     389             : 
     390        5366 :     ZERO_FARRAY(Kfusion);
     391             :     Vector24 Hfusion; // Observation Jacobians
     392        5366 :     const ftype R_ACC = sq(fmaxF(frontend->_dragObsNoise, 0.5f));
     393        5366 :     const ftype density_ratio = 1.0f/sq(dal.get_EAS2TAS());
     394        5366 :     const ftype rho = fmaxF(1.225f * density_ratio, 0.1f); // air density
     395             : 
     396             :     // get latest estimated orientation
     397        5366 :     const ftype &q0 = stateStruct.quat[0];
     398        5366 :     const ftype &q1 = stateStruct.quat[1];
     399        5366 :     const ftype &q2 = stateStruct.quat[2];
     400        5366 :     const ftype &q3 = stateStruct.quat[3];
     401             : 
     402             :     // get latest velocity in earth frame
     403             :     const ftype &vn = stateStruct.velocity.x;
     404             :     const ftype &ve = stateStruct.velocity.y;
     405             :     const ftype &vd = stateStruct.velocity.z;
     406             : 
     407             :     // get latest wind velocity in earth frame
     408             :     const ftype &vwn = stateStruct.wind_vel.x;
     409             :     const ftype &vwe = stateStruct.wind_vel.y;
     410             : 
     411             :     // predicted specific forces
     412             :     // calculate relative wind velocity in earth frame and rotate into body frame
     413        5366 :     const Vector3F rel_wind_earth(vn - vwn, ve - vwe, vd);
     414        5366 :     const Vector3F rel_wind_body = prevTnb * rel_wind_earth;
     415             : 
     416             :     // perform sequential fusion of XY specific forces
     417       16098 :     for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
     418             :         // correct accel data for bias
     419       10732 :         const ftype mea_acc = dragSampleDelayed.accelXY[axis_index]  - stateStruct.accel_bias[axis_index] / dtEkfAvg;
     420             : 
     421             :         // Acceleration in m/s/s predicted using vehicle and wind velocity estimates
     422             :         // Initialised to measured value and updated later using available drag model
     423             :         ftype predAccel = mea_acc;
     424             : 
     425             :         // predicted sign of drag force
     426       10732 :         const ftype dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f;
     427             : 
     428       10732 :         if (axis_index == 0) {
     429             :             // drag can be modelled as an arbitrary  combination of bluff body drag that proportional to
     430             :             // speed squared, and rotor momentum drag that is proportional to speed.
     431             :             ftype Kaccx; // Derivative of specific force wrt airspeed
     432        5366 :             if (using_mcoef && using_bcoef_x) {
     433             :                 // mixed bluff body and propeller momentum drag
     434        5366 :                 const ftype airSpd = (bcoef_x / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_x) * fabsF(mea_acc)));
     435        5366 :                 Kaccx = fmaxF(1e-1f, (rho / bcoef_x) * airSpd + mcoef * density_ratio);
     436        5366 :                 predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
     437           0 :             } else if (using_mcoef) {
     438             :                 // propeller momentum drag only
     439           0 :                 Kaccx = fmaxF(1e-1f, mcoef * density_ratio);
     440           0 :                 predAccel = - rel_wind_body[0] * mcoef * density_ratio;
     441           0 :             } else if (using_bcoef_x) {
     442             :                 // bluff body drag only
     443           0 :                 const ftype airSpd = sqrtF((2.0f * bcoef_x * fabsF(mea_acc)) / rho);
     444           0 :                 Kaccx = fmaxF(1e-1f, (rho / bcoef_x) * airSpd);
     445           0 :                 predAccel = (0.5f / bcoef_x) * rho * sq(rel_wind_body[0]) * dragForceSign;
     446             :             } else {
     447             :                 // skip this axis
     448           0 :                 continue;
     449             :             }
     450             : 
     451             :             // intermediate variables
     452        5366 :             const ftype HK0 = vn - vwn;
     453        5366 :             const ftype HK1 = ve - vwe;
     454        5366 :             const ftype HK2 = HK0*q0 + HK1*q3 - q2*vd;
     455        5366 :             const ftype HK3 = 2*Kaccx;
     456        5366 :             const ftype HK4 = HK0*q1 + HK1*q2 + q3*vd;
     457        5366 :             const ftype HK5 = HK0*q2 - HK1*q1 + q0*vd;
     458        5366 :             const ftype HK6 = -HK0*q3 + HK1*q0 + q1*vd;
     459        5366 :             const ftype HK7 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
     460        5366 :             const ftype HK8 = HK7*Kaccx;
     461        5366 :             const ftype HK9 = q0*q3 + q1*q2;
     462        5366 :             const ftype HK10 = HK3*HK9;
     463        5366 :             const ftype HK11 = q0*q2 - q1*q3;
     464        5366 :             const ftype HK12 = 2*HK9;
     465        5366 :             const ftype HK13 = 2*HK11;
     466        5366 :             const ftype HK14 = 2*HK4;
     467        5366 :             const ftype HK15 = 2*HK2;
     468        5366 :             const ftype HK16 = 2*HK5;
     469        5366 :             const ftype HK17 = 2*HK6;
     470        5366 :             const ftype HK18 = -HK12*P[0][23] + HK12*P[0][5] - HK13*P[0][6] + HK14*P[0][1] + HK15*P[0][0] - HK16*P[0][2] + HK17*P[0][3] - HK7*P[0][22] + HK7*P[0][4];
     471        5366 :             const ftype HK19 = HK12*P[5][23];
     472        5366 :             const ftype HK20 = -HK12*P[23][23] - HK13*P[6][23] + HK14*P[1][23] + HK15*P[0][23] - HK16*P[2][23] + HK17*P[3][23] + HK19 - HK7*P[22][23] + HK7*P[4][23];
     473        5366 :             const ftype HK21 = sq(Kaccx);
     474        5366 :             const ftype HK22 = HK12*HK21;
     475        5366 :             const ftype HK23 = HK12*P[5][5] - HK13*P[5][6] + HK14*P[1][5] + HK15*P[0][5] - HK16*P[2][5] + HK17*P[3][5] - HK19 + HK7*P[4][5] - HK7*P[5][22];
     476        5366 :             const ftype HK24 = HK12*P[5][6] - HK12*P[6][23] - HK13*P[6][6] + HK14*P[1][6] + HK15*P[0][6] - HK16*P[2][6] + HK17*P[3][6] + HK7*P[4][6] - HK7*P[6][22];
     477        5366 :             const ftype HK25 = HK7*P[4][22];
     478        5366 :             const ftype HK26 = -HK12*P[4][23] + HK12*P[4][5] - HK13*P[4][6] + HK14*P[1][4] + HK15*P[0][4] - HK16*P[2][4] + HK17*P[3][4] - HK25 + HK7*P[4][4];
     479        5366 :             const ftype HK27 = HK21*HK7;
     480        5366 :             const ftype HK28 = -HK12*P[22][23] + HK12*P[5][22] - HK13*P[6][22] + HK14*P[1][22] + HK15*P[0][22] - HK16*P[2][22] + HK17*P[3][22] + HK25 - HK7*P[22][22];
     481        5366 :             const ftype HK29 = -HK12*P[1][23] + HK12*P[1][5] - HK13*P[1][6] + HK14*P[1][1] + HK15*P[0][1] - HK16*P[1][2] + HK17*P[1][3] - HK7*P[1][22] + HK7*P[1][4];
     482        5366 :             const ftype HK30 = -HK12*P[2][23] + HK12*P[2][5] - HK13*P[2][6] + HK14*P[1][2] + HK15*P[0][2] - HK16*P[2][2] + HK17*P[2][3] - HK7*P[2][22] + HK7*P[2][4];
     483        5366 :             const ftype HK31 = -HK12*P[3][23] + HK12*P[3][5] - HK13*P[3][6] + HK14*P[1][3] + HK15*P[0][3] - HK16*P[2][3] + HK17*P[3][3] - HK7*P[3][22] + HK7*P[3][4];
     484             :             // const ftype HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
     485             : 
     486             :             // calculate innovation variance and exit if badly conditioned
     487        5366 :             innovDragVar.x = (-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC);
     488        5366 :             if (innovDragVar.x < R_ACC) {
     489           0 :                 return;
     490             :             }
     491        5366 :             const ftype HK32 = Kaccx / innovDragVar.x;
     492             : 
     493             :             // Observation Jacobians
     494        5366 :             Hfusion[0] = -HK2*HK3;
     495        5366 :             Hfusion[1] = -HK3*HK4;
     496        5366 :             Hfusion[2] = HK3*HK5;
     497        5366 :             Hfusion[3] = -HK3*HK6;
     498        5366 :             Hfusion[4] = -HK8;
     499        5366 :             Hfusion[5] = -HK10;
     500        5366 :             Hfusion[6] = HK11*HK3;
     501             :             Hfusion[22] = HK8;
     502             :             Hfusion[23] = HK10;
     503             : 
     504             :             // Kalman gains
     505             :             // Don't allow modification of any states other than wind velocity - we only need a wind estimate.
     506             :             // See derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations.
     507        5366 :             Kfusion[22] = -HK28*HK32;
     508        5366 :             Kfusion[23] = -HK20*HK32;
     509             : 
     510             : 
     511             :         } else if (axis_index == 1) {
     512             :             // drag can be modelled as an arbitrary combination of bluff body drag that proportional to
     513             :             // speed squared, and rotor momentum drag that is proportional to speed.
     514             :             ftype Kaccy; // Derivative of specific force wrt airspeed
     515        5366 :             if (using_mcoef && using_bcoef_y) {
     516             :                 // mixed bluff body and propeller momentum drag
     517        5366 :                 const ftype airSpd = (bcoef_y / rho) * (- mcoef + sqrtF(sq(mcoef) + 2.0f * (rho / bcoef_y) * fabsF(mea_acc)));
     518        5366 :                 Kaccy = fmaxF(1e-1f, (rho / bcoef_y) * airSpd + mcoef * density_ratio);
     519        5366 :                 predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
     520           0 :             } else if (using_mcoef) {
     521             :                 // propeller momentum drag only
     522           0 :                 Kaccy = fmaxF(1e-1f, mcoef * density_ratio);
     523           0 :                 predAccel = - rel_wind_body[1] * mcoef * density_ratio;
     524           0 :             } else if (using_bcoef_y) {
     525             :                 // bluff body drag only
     526           0 :                 const ftype airSpd = sqrtF((2.0f * bcoef_y * fabsF(mea_acc)) / rho);
     527           0 :                 Kaccy = fmaxF(1e-1f, (rho / bcoef_y) * airSpd);
     528           0 :                 predAccel = (0.5f / bcoef_y) * rho * sq(rel_wind_body[1]) * dragForceSign;
     529             :             } else {
     530             :                 // nothing more to do
     531             :                 return;
     532             :             }
     533             : 
     534             :             // intermediate variables
     535        5366 :             const ftype HK0 = ve - vwe;
     536        5366 :             const ftype HK1 = vn - vwn;
     537        5366 :             const ftype HK2 = HK0*q0 - HK1*q3 + q1*vd;
     538        5366 :             const ftype HK3 = 2*Kaccy;
     539        5366 :             const ftype HK4 = -HK0*q1 + HK1*q2 + q0*vd;
     540        5366 :             const ftype HK5 = HK0*q2 + HK1*q1 + q3*vd;
     541        5366 :             const ftype HK6 = HK0*q3 + HK1*q0 - q2*vd;
     542        5366 :             const ftype HK7 = q0*q3 - q1*q2;
     543        5366 :             const ftype HK8 = HK3*HK7;
     544        5366 :             const ftype HK9 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
     545        5366 :             const ftype HK10 = HK9*Kaccy;
     546        5366 :             const ftype HK11 = q0*q1 + q2*q3;
     547        5366 :             const ftype HK12 = 2*HK11;
     548        5366 :             const ftype HK13 = 2*HK7;
     549        5366 :             const ftype HK14 = 2*HK5;
     550        5366 :             const ftype HK15 = 2*HK2;
     551        5366 :             const ftype HK16 = 2*HK4;
     552        5366 :             const ftype HK17 = 2*HK6;
     553        5366 :             const ftype HK18 = HK12*P[0][6] + HK13*P[0][22] - HK13*P[0][4] + HK14*P[0][2] + HK15*P[0][0] + HK16*P[0][1] - HK17*P[0][3] - HK9*P[0][23] + HK9*P[0][5];
     554        5366 :             const ftype HK19 = sq(Kaccy);
     555        5366 :             const ftype HK20 = HK12*P[6][6] - HK13*P[4][6] + HK13*P[6][22] + HK14*P[2][6] + HK15*P[0][6] + HK16*P[1][6] - HK17*P[3][6] + HK9*P[5][6] - HK9*P[6][23];
     556        5366 :             const ftype HK21 = HK13*P[4][22];
     557        5366 :             const ftype HK22 = HK12*P[6][22] + HK13*P[22][22] + HK14*P[2][22] + HK15*P[0][22] + HK16*P[1][22] - HK17*P[3][22] - HK21 - HK9*P[22][23] + HK9*P[5][22];
     558        5366 :             const ftype HK23 = HK13*HK19;
     559        5366 :             const ftype HK24 = HK12*P[4][6] - HK13*P[4][4] + HK14*P[2][4] + HK15*P[0][4] + HK16*P[1][4] - HK17*P[3][4] + HK21 - HK9*P[4][23] + HK9*P[4][5];
     560        5366 :             const ftype HK25 = HK9*P[5][23];
     561        5366 :             const ftype HK26 = HK12*P[5][6] - HK13*P[4][5] + HK13*P[5][22] + HK14*P[2][5] + HK15*P[0][5] + HK16*P[1][5] - HK17*P[3][5] - HK25 + HK9*P[5][5];
     562        5366 :             const ftype HK27 = HK19*HK9;
     563        5366 :             const ftype HK28 = HK12*P[6][23] + HK13*P[22][23] - HK13*P[4][23] + HK14*P[2][23] + HK15*P[0][23] + HK16*P[1][23] - HK17*P[3][23] + HK25 - HK9*P[23][23];
     564        5366 :             const ftype HK29 = HK12*P[2][6] + HK13*P[2][22] - HK13*P[2][4] + HK14*P[2][2] + HK15*P[0][2] + HK16*P[1][2] - HK17*P[2][3] - HK9*P[2][23] + HK9*P[2][5];
     565        5366 :             const ftype HK30 = HK12*P[1][6] + HK13*P[1][22] - HK13*P[1][4] + HK14*P[1][2] + HK15*P[0][1] + HK16*P[1][1] - HK17*P[1][3] - HK9*P[1][23] + HK9*P[1][5];
     566        5366 :             const ftype HK31 = HK12*P[3][6] + HK13*P[3][22] - HK13*P[3][4] + HK14*P[2][3] + HK15*P[0][3] + HK16*P[1][3] - HK17*P[3][3] - HK9*P[3][23] + HK9*P[3][5];
     567             :             // const ftype HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
     568             : 
     569        5366 :             innovDragVar.y = (HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC);
     570        5366 :             if (innovDragVar.y < R_ACC) {
     571             :                 // calculation is badly conditioned
     572             :                 return;
     573             :             }
     574        5366 :             const ftype HK32 = Kaccy / innovDragVar.y;
     575             : 
     576             :             // Observation Jacobians
     577        5366 :             Hfusion[0] = -HK2*HK3;
     578        5366 :             Hfusion[1] = -HK3*HK4;
     579        5366 :             Hfusion[2] = -HK3*HK5;
     580        5366 :             Hfusion[3] = HK3*HK6;
     581             :             Hfusion[4] = HK8;
     582        5366 :             Hfusion[5] = -HK10;
     583        5366 :             Hfusion[6] = -HK11*HK3;
     584        5366 :             Hfusion[22] = -HK8;
     585             :             Hfusion[23] = HK10;
     586             : 
     587             :             // Kalman gains
     588             :             // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
     589             :             // See derivation/generated/acc_bf_generated.cpp for un-implemented Kalman gain equations.
     590        5366 :             Kfusion[22] = -HK22*HK32;
     591        5366 :             Kfusion[23] = -HK28*HK32;
     592             :         }
     593             : 
     594       10732 :         innovDrag[axis_index] = predAccel - mea_acc;
     595       10732 :         dragTestRatio[axis_index] = sq(innovDrag[axis_index]) / (25.0f * innovDragVar[axis_index]);
     596             : 
     597             :         // if the innovation consistency check fails then don't fuse the sample
     598       10732 :         if (dragTestRatio[axis_index] > 1.0f) {
     599             :             return;
     600             :         }
     601             : 
     602             :         // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
     603             :         // the zero elements of H to reduce the number of operations.
     604      268300 :         for (unsigned i = 0; i<=stateIndexLim; i++) {
     605             :             // j as the inner loop allows the compiler to hoist the KH product
     606             :             // to save computation, and do the inner indexing more efficiently.
     607     6439200 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     608             :                 ftype res = 0;
     609     6181632 :                 res += (Kfusion[i] * Hfusion[0]) * P[0][j];
     610     6181632 :                 res += (Kfusion[i] * Hfusion[1]) * P[1][j];
     611     6181632 :                 res += (Kfusion[i] * Hfusion[2]) * P[2][j];
     612     6181632 :                 res += (Kfusion[i] * Hfusion[3]) * P[3][j];
     613     6181632 :                 res += (Kfusion[i] * Hfusion[4]) * P[4][j];
     614     6181632 :                 res += (Kfusion[i] * Hfusion[5]) * P[5][j];
     615     6181632 :                 res += (Kfusion[i] * Hfusion[6]) * P[6][j];
     616     6181632 :                 res += (Kfusion[i] * Hfusion[22]) * P[22][j];
     617     6181632 :                 res += (Kfusion[i] * Hfusion[23]) * P[23][j];
     618     6181632 :                 KHP[i][j] = res;
     619             :             }
     620             :         }
     621             : 
     622             :         // finish fusion from KHP and Kfusion
     623       10732 :         FinishFusion(innovDrag[axis_index], true); // forcing fusion is probably a bug
     624             :     }
     625             : 
     626             :     // record time of successful fusion
     627        5366 :     lastDragPassTime_ms = imuSampleTime_ms;
     628             : }
     629             : #endif // EK3_FEATURE_DRAG_FUSION
     630             : 
     631             : /********************************************************
     632             : *                   MISC FUNCTIONS                      *
     633             : ********************************************************/
     634             :