libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

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