Line data    Source code

       1             : #include "AP_NavEKF2_core.h"
       2             : 
       3             : #include <AP_DAL/AP_DAL.h>
       4             : 
       5             : #include "AP_NavEKF2.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/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
      19             : */
      20      379577 : void NavEKF2_core::FuseAirspeed()
      21             : {
      22             :     // declarations
      23             :     ftype vn;
      24             :     ftype ve;
      25             :     ftype vd;
      26             :     ftype vwn;
      27             :     ftype vwe;
      28      379577 :     ftype EAS2TAS = dal.get_EAS2TAS();
      29      379577 :     const ftype R_TAS = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(EAS2TAS, 0.9f, 10.0f));
      30             :     Vector3 SH_TAS;
      31             :     ftype SK_TAS;
      32             :     Vector24 H_TAS;
      33             :     ftype VtasPred;
      34             : 
      35             :     // copy required states to local variable names
      36      379577 :     vn = stateStruct.velocity.x;
      37      379577 :     ve = stateStruct.velocity.y;
      38      379577 :     vd = stateStruct.velocity.z;
      39      379577 :     vwn = stateStruct.wind_vel.x;
      40      379577 :     vwe = stateStruct.wind_vel.y;
      41             : 
      42             :     // calculate the predicted airspeed
      43      379577 :     VtasPred = norm((ve - vwe) , (vn - vwn) , vd);
      44             :     // perform fusion of True Airspeed measurement
      45      379577 :     if (VtasPred > 1.0f)
      46             :     {
      47             :         // calculate observation jacobians
      48      376838 :         SH_TAS[0] = 1.0f/(sqrtF(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
      49      376838 :         SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2;
      50      376838 :         SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2;
      51             :         for (uint8_t i=0; i<=2; i++) H_TAS[i] = 0.0f;
      52      376838 :         H_TAS[3] = SH_TAS[2];
      53      376838 :         H_TAS[4] = SH_TAS[1];
      54      376838 :         H_TAS[5] = vd*SH_TAS[0];
      55      376838 :         H_TAS[22] = -SH_TAS[2];
      56      376838 :         H_TAS[23] = -SH_TAS[1];
      57     6406246 :         for (uint8_t i=6; i<=21; i++) H_TAS[i] = 0.0f;
      58             :         // calculate Kalman gains
      59      376838 :         ftype temp = (R_TAS + SH_TAS[2]*(P[3][3]*SH_TAS[2] + P[4][3]*SH_TAS[1] - P[22][3]*SH_TAS[2] - P[23][3]*SH_TAS[1] + P[5][3]*vd*SH_TAS[0]) + SH_TAS[1]*(P[3][4]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[5][4]*vd*SH_TAS[0]) - SH_TAS[2]*(P[3][22]*SH_TAS[2] + P[4][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[5][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[3][23]*SH_TAS[2] + P[4][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[5][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[3][5]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]));
      60      376838 :         if (temp >= R_TAS) {
      61      376838 :             SK_TAS = 1.0f / temp;
      62      376838 :             faultStatus.bad_airspeed = false;
      63             :         } else {
      64             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
      65             :             // we reset the covariance matrix and try again next measurement
      66           0 :             CovarianceInit();
      67           0 :             faultStatus.bad_airspeed = true;
      68           0 :             return;
      69             :         }
      70      376838 :         Kfusion[0] = SK_TAS*(P[0][3]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][4]*SH_TAS[1] - P[0][23]*SH_TAS[1] + P[0][5]*vd*SH_TAS[0]);
      71      376838 :         Kfusion[1] = SK_TAS*(P[1][3]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][4]*SH_TAS[1] - P[1][23]*SH_TAS[1] + P[1][5]*vd*SH_TAS[0]);
      72      376838 :         Kfusion[2] = SK_TAS*(P[2][3]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][4]*SH_TAS[1] - P[2][23]*SH_TAS[1] + P[2][5]*vd*SH_TAS[0]);
      73      376838 :         Kfusion[3] = SK_TAS*(P[3][3]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][4]*SH_TAS[1] - P[3][23]*SH_TAS[1] + P[3][5]*vd*SH_TAS[0]);
      74      376838 :         Kfusion[4] = SK_TAS*(P[4][3]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][4]*SH_TAS[1] - P[4][23]*SH_TAS[1] + P[4][5]*vd*SH_TAS[0]);
      75      376838 :         Kfusion[5] = SK_TAS*(P[5][3]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[5][23]*SH_TAS[1] + P[5][5]*vd*SH_TAS[0]);
      76      376838 :         Kfusion[6] = SK_TAS*(P[6][3]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][4]*SH_TAS[1] - P[6][23]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]);
      77      376838 :         Kfusion[7] = SK_TAS*(P[7][3]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][4]*SH_TAS[1] - P[7][23]*SH_TAS[1] + P[7][5]*vd*SH_TAS[0]);
      78      376838 :         Kfusion[8] = SK_TAS*(P[8][3]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][4]*SH_TAS[1] - P[8][23]*SH_TAS[1] + P[8][5]*vd*SH_TAS[0]);
      79      376838 :         Kfusion[9] = SK_TAS*(P[9][3]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][4]*SH_TAS[1] - P[9][23]*SH_TAS[1] + P[9][5]*vd*SH_TAS[0]);
      80      376838 :         Kfusion[10] = SK_TAS*(P[10][3]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][4]*SH_TAS[1] - P[10][23]*SH_TAS[1] + P[10][5]*vd*SH_TAS[0]);
      81      376838 :         Kfusion[11] = SK_TAS*(P[11][3]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][4]*SH_TAS[1] - P[11][23]*SH_TAS[1] + P[11][5]*vd*SH_TAS[0]);
      82      376838 :         Kfusion[12] = SK_TAS*(P[12][3]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][4]*SH_TAS[1] - P[12][23]*SH_TAS[1] + P[12][5]*vd*SH_TAS[0]);
      83      376838 :         Kfusion[13] = SK_TAS*(P[13][3]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][4]*SH_TAS[1] - P[13][23]*SH_TAS[1] + P[13][5]*vd*SH_TAS[0]);
      84      376838 :         Kfusion[14] = SK_TAS*(P[14][3]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][4]*SH_TAS[1] - P[14][23]*SH_TAS[1] + P[14][5]*vd*SH_TAS[0]);
      85      376838 :         Kfusion[15] = SK_TAS*(P[15][3]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][4]*SH_TAS[1] - P[15][23]*SH_TAS[1] + P[15][5]*vd*SH_TAS[0]);
      86      376838 :         Kfusion[22] = SK_TAS*(P[22][3]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][4]*SH_TAS[1] - P[22][23]*SH_TAS[1] + P[22][5]*vd*SH_TAS[0]);
      87      376838 :         Kfusion[23] = SK_TAS*(P[23][3]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][4]*SH_TAS[1] - P[23][23]*SH_TAS[1] + P[23][5]*vd*SH_TAS[0]);
      88             :         // zero Kalman gains to inhibit magnetic field state estimation
      89      376838 :         if (!inhibitMagStates) {
      90      373726 :             Kfusion[16] = SK_TAS*(P[16][3]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][4]*SH_TAS[1] - P[16][23]*SH_TAS[1] + P[16][5]*vd*SH_TAS[0]);
      91      373726 :             Kfusion[17] = SK_TAS*(P[17][3]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][4]*SH_TAS[1] - P[17][23]*SH_TAS[1] + P[17][5]*vd*SH_TAS[0]);
      92      373726 :             Kfusion[18] = SK_TAS*(P[18][3]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][4]*SH_TAS[1] - P[18][23]*SH_TAS[1] + P[18][5]*vd*SH_TAS[0]);
      93      373726 :             Kfusion[19] = SK_TAS*(P[19][3]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][4]*SH_TAS[1] - P[19][23]*SH_TAS[1] + P[19][5]*vd*SH_TAS[0]);
      94      373726 :             Kfusion[20] = SK_TAS*(P[20][3]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][4]*SH_TAS[1] - P[20][23]*SH_TAS[1] + P[20][5]*vd*SH_TAS[0]);
      95      373726 :             Kfusion[21] = SK_TAS*(P[21][3]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][4]*SH_TAS[1] - P[21][23]*SH_TAS[1] + P[21][5]*vd*SH_TAS[0]);
      96             :         } else {
      97       21784 :             for (uint8_t i=16; i<=21; i++) {
      98       18672 :                 Kfusion[i] = 0.0f;
      99             :             }
     100             :         }
     101             : 
     102             :         // calculate measurement innovation variance
     103      376838 :         varInnovVtas = 1.0f/SK_TAS;
     104             : 
     105             :         // calculate measurement innovation
     106      376838 :         innovVtas = VtasPred - tasDataDelayed.tas;
     107             : 
     108             :         // calculate the innovation consistency test ratio
     109      376838 :         tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
     110             : 
     111             :         // fail if the ratio is > 1, but don't fail if bad IMU data
     112      376838 :         bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
     113      376838 :         tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
     114             : 
     115      376838 :         if (!tasHealth) {
     116        1186 :             lastTasFailTime_ms = imuSampleTime_ms;
     117             :         } else {
     118      375652 :             lastTasFailTime_ms = 0;
     119             :         }
     120             :         
     121             :         // 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
     122      376838 :         if (tasHealth || (tasTimeout && posTimeout)) {
     123             : 
     124             :             // restart the counter
     125      375654 :             lastTasPassTime_ms = imuSampleTime_ms;
     126             : 
     127             : 
     128             :             // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
     129      375654 :             stateStruct.angErr.zero();
     130             : 
     131             :             // correct the state vector
     132     9391350 :             for (uint8_t j= 0; j<=stateIndexLim; j++) {
     133     9015696 :                 statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas;
     134             :             }
     135             : 
     136             :             // the first 3 states represent the angular misalignment vector.
     137             :             // This is used to correct the estimated quaternion on the current time step
     138      375654 :             stateStruct.quat.rotate(stateStruct.angErr);
     139             : 
     140             :             // correct the covariance P = (I - K*H)*P
     141             :             // take advantage of the empty columns in KH to reduce the
     142             :             // number of operations
     143     9391350 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     144    36062784 :                 for (unsigned j = 0; j<=2; j++) {
     145    27047088 :                     KH[i][j] = 0.0f;
     146             :                 }
     147    36062784 :                 for (unsigned j = 3; j<=5; j++) {
     148    27047088 :                     KH[i][j] = Kfusion[i] * H_TAS[j];
     149             :                 }
     150   153266832 :                 for (unsigned j = 6; j<=21; j++) {
     151   144251136 :                     KH[i][j] = 0.0f;
     152             :                 }
     153    27047088 :                 for (unsigned j = 22; j<=23; j++) {
     154    18031392 :                     KH[i][j] = Kfusion[i] * H_TAS[j];
     155             :                 }
     156             :             }
     157     9391350 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     158   225392400 :                 for (unsigned i = 0; i<=stateIndexLim; i++) {
     159             :                     ftype res = 0;
     160   216376704 :                     res += KH[i][3] * P[3][j];
     161   216376704 :                     res += KH[i][4] * P[4][j];
     162   216376704 :                     res += KH[i][5] * P[5][j];
     163   216376704 :                     res += KH[i][22] * P[22][j];
     164   216376704 :                     res += KH[i][23] * P[23][j];
     165   216376704 :                     KHP[i][j] = res;
     166             :                 }
     167             :             }
     168     9391350 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     169   225392400 :                 for (unsigned j = 0; j<=stateIndexLim; j++) {
     170   216376704 :                     P[i][j] = P[i][j] - KHP[i][j];
     171             :                 }
     172             :             }
     173             :         }
     174             :     }
     175             : 
     176             :     // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
     177      379577 :     ForceSymmetry();
     178      379577 :     ConstrainVariances();
     179             : }
     180             : 
     181             : // select fusion of true airspeed measurements
     182     3248540 : void NavEKF2_core::SelectTasFusion()
     183             : {
     184             :     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     185             :     // If so, don't fuse measurements on this time step to reduce frame over-runs
     186             :     // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements
     187     3248540 :     if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) {
     188       18168 :         airSpdFusionDelayed = true;
     189       18168 :         return;
     190             :     } else {
     191     3230372 :         airSpdFusionDelayed = false;
     192             :     }
     193             : 
     194             :     // get true airspeed measurement
     195     3230372 :     readAirSpdData();
     196             : 
     197             :     // If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
     198     3230372 :     if (imuSampleTime_ms - tasDataNew.time_ms > frontend->tasRetryTime_ms) {
     199      408044 :         tasTimeout = true;
     200             :     }
     201             : 
     202             :     // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
     203     3230372 :     if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
     204      379577 :         FuseAirspeed();
     205      379577 :         prevTasStep_ms = imuSampleTime_ms;
     206             :     }
     207             : }
     208             : 
     209             : 
     210             : // select fusion of synthetic sideslip measurements
     211             : // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
     212             : // it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc)
     213     3248540 : void NavEKF2_core::SelectBetaFusion()
     214             : {
     215             :     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
     216             :     // If so, don't fuse measurements on this time step to reduce frame over-runs
     217             :     // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
     218     3248540 :     if (magFusePerformed && dtIMUavg < 0.005f && !sideSlipFusionDelayed) {
     219       18168 :         sideSlipFusionDelayed = true;
     220       18168 :         return;
     221             :     } else {
     222     3230372 :         sideSlipFusionDelayed = false;
     223             :     }
     224             : 
     225             :     // set true when the fusion time interval has triggered
     226     3230372 :     bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend->betaAvg_ms);
     227             :     // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
     228     3230372 :     bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms));
     229             :     // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
     230     3230372 :     bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
     231             :     // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
     232     3230372 :     if (f_feasible && f_required && f_timeTrigger) {
     233       31082 :         FuseSideslip();
     234       31082 :         prevBetaStep_ms = imuSampleTime_ms;
     235             :     }
     236             : }
     237             : 
     238             : /*
     239             :  * Fuse synthetic sideslip measurement of zero using explicit algebraic equations generated with Matlab symbolic toolbox.
     240             :  * The script file used to generate these and other equations in this filter can be found here:
     241             :  * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
     242             : */
     243       31082 : void NavEKF2_core::FuseSideslip()
     244             : {
     245             :     // declarations
     246             :     ftype q0;
     247             :     ftype q1;
     248             :     ftype q2;
     249             :     ftype q3;
     250             :     ftype vn;
     251             :     ftype ve;
     252             :     ftype vd;
     253             :     ftype vwn;
     254             :     ftype vwe;
     255             :     const ftype R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
     256             :     Vector10 SH_BETA;
     257             :     Vector5 SK_BETA;
     258       31082 :     Vector3F vel_rel_wind;
     259             :     Vector24 H_BETA;
     260             :     ftype innovBeta;
     261             : 
     262             :     // copy required states to local variable names
     263       31082 :     q0 = stateStruct.quat[0];
     264       31082 :     q1 = stateStruct.quat[1];
     265       31082 :     q2 = stateStruct.quat[2];
     266       31082 :     q3 = stateStruct.quat[3];
     267       31082 :     vn = stateStruct.velocity.x;
     268       31082 :     ve = stateStruct.velocity.y;
     269       31082 :     vd = stateStruct.velocity.z;
     270       31082 :     vwn = stateStruct.wind_vel.x;
     271       31082 :     vwe = stateStruct.wind_vel.y;
     272             : 
     273             :     // calculate predicted wind relative velocity in NED
     274       31082 :     vel_rel_wind.x = vn - vwn;
     275       31082 :     vel_rel_wind.y = ve - vwe;
     276       31082 :     vel_rel_wind.z = vd;
     277             : 
     278             :     // rotate into body axes
     279       31082 :     vel_rel_wind = prevTnb * vel_rel_wind;
     280             : 
     281             :     // perform fusion of assumed sideslip  = 0
     282       31082 :     if (vel_rel_wind.x > 5.0f)
     283             :     {
     284             :         // Calculate observation jacobians
     285       30707 :         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);
     286       30707 :         if (fabsF(SH_BETA[0]) <= 1e-9f) {
     287           0 :             faultStatus.bad_sideslip = true;
     288         492 :             return;
     289             :         } else {
     290       30707 :             faultStatus.bad_sideslip = false;
     291             :         }
     292       30707 :         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);
     293       30707 :         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);
     294       30707 :         SH_BETA[2] = vd*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) - (ve - vwe)*(2*q0*q1 - 2*q2*q3) + (vn - vwn)*(2*q0*q2 + 2*q1*q3);
     295       30707 :         SH_BETA[3] = 1/sq(SH_BETA[0]);
     296       30707 :         SH_BETA[4] = (sq(q0) - sq(q1) + sq(q2) - sq(q3))/SH_BETA[0];
     297       30707 :         SH_BETA[5] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
     298       30707 :         SH_BETA[6] = 1/SH_BETA[0];
     299             :         SH_BETA[7] = 2*q0*q3;
     300             :         SH_BETA[8] = SH_BETA[7] + 2*q1*q2;
     301             :         SH_BETA[9] = SH_BETA[7] - 2*q1*q2;
     302       30707 :         H_BETA[0] = SH_BETA[2]*SH_BETA[6];
     303       30707 :         H_BETA[1] = SH_BETA[1]*SH_BETA[2]*SH_BETA[3];
     304       30707 :         H_BETA[2] = - sq(SH_BETA[1])*SH_BETA[3] - 1;
     305       30707 :         H_BETA[3] = - SH_BETA[6]*SH_BETA[9] - SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
     306       30707 :         H_BETA[4] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
     307       30707 :         H_BETA[5] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
     308      522019 :         for (uint8_t i=6; i<=21; i++) {
     309      491312 :             H_BETA[i] = 0.0f;
     310             :         }
     311       30707 :         H_BETA[22] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
     312       30707 :         H_BETA[23] = SH_BETA[1]*SH_BETA[3]*SH_BETA[8] - SH_BETA[4];
     313             : 
     314             :         // Calculate Kalman gains
     315       30707 :         ftype temp = (R_BETA + (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][4]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][4]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][4]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][4]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][4]*SH_BETA[2]*SH_BETA[6] + P[1][4]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8])*(P[22][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][23]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][23]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][23]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][23]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][23]*SH_BETA[2]*SH_BETA[6] + P[1][23]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][3]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][3]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][3]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][3]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][3]*SH_BETA[2]*SH_BETA[6] + P[1][3]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5])*(P[22][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][22]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][22]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][22]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][22]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][22]*SH_BETA[2]*SH_BETA[6] + P[1][22]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) - (sq(SH_BETA[1])*SH_BETA[3] + 1)*(P[22][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][2]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][2]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][2]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][2]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][2]*SH_BETA[2]*SH_BETA[6] + P[1][2]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + (SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3))*(P[22][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][5]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][5]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][5]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][5]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][5]*SH_BETA[2]*SH_BETA[6] + P[1][5]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[2]*SH_BETA[6]*(P[22][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][0]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][0]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][0]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][0]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][0]*SH_BETA[2]*SH_BETA[6] + P[1][0]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]) + SH_BETA[1]*SH_BETA[2]*SH_BETA[3]*(P[22][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[3][1]*(SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5]) - P[2][1]*(sq(SH_BETA[1])*SH_BETA[3] + 1) + P[5][1]*(SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3)) + P[4][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) - P[23][1]*(SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8]) + P[0][1]*SH_BETA[2]*SH_BETA[6] + P[1][1]*SH_BETA[1]*SH_BETA[2]*SH_BETA[3]));
     316       30707 :         if (temp >= R_BETA) {
     317       30707 :             SK_BETA[0] = 1.0f / temp;
     318       30707 :             faultStatus.bad_sideslip = false;
     319             :         } else {
     320             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
     321             :             // we reset the covariance matrix and try again next measurement
     322           0 :             CovarianceInit();
     323           0 :             faultStatus.bad_sideslip = true;
     324           0 :             return;
     325             :         }
     326             :         SK_BETA[1] = SH_BETA[6]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[3]*(2*q0*q2 - 2*q1*q3);
     327             :         SK_BETA[2] = SH_BETA[6]*SH_BETA[9] + SH_BETA[1]*SH_BETA[3]*SH_BETA[5];
     328             :         SK_BETA[3] = SH_BETA[4] - SH_BETA[1]*SH_BETA[3]*SH_BETA[8];
     329       30707 :         SK_BETA[4] = sq(SH_BETA[1])*SH_BETA[3] + 1;
     330       30707 :         Kfusion[0] = SK_BETA[0]*(P[0][5]*SK_BETA[1] - P[0][2]*SK_BETA[4] - P[0][3]*SK_BETA[2] + P[0][4]*SK_BETA[3] + P[0][22]*SK_BETA[2] - P[0][23]*SK_BETA[3] + P[0][0]*SH_BETA[6]*SH_BETA[2] + P[0][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     331       30707 :         Kfusion[1] = SK_BETA[0]*(P[1][5]*SK_BETA[1] - P[1][2]*SK_BETA[4] - P[1][3]*SK_BETA[2] + P[1][4]*SK_BETA[3] + P[1][22]*SK_BETA[2] - P[1][23]*SK_BETA[3] + P[1][0]*SH_BETA[6]*SH_BETA[2] + P[1][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     332       30707 :         Kfusion[2] = SK_BETA[0]*(P[2][5]*SK_BETA[1] - P[2][2]*SK_BETA[4] - P[2][3]*SK_BETA[2] + P[2][4]*SK_BETA[3] + P[2][22]*SK_BETA[2] - P[2][23]*SK_BETA[3] + P[2][0]*SH_BETA[6]*SH_BETA[2] + P[2][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     333       30707 :         Kfusion[3] = SK_BETA[0]*(P[3][5]*SK_BETA[1] - P[3][2]*SK_BETA[4] - P[3][3]*SK_BETA[2] + P[3][4]*SK_BETA[3] + P[3][22]*SK_BETA[2] - P[3][23]*SK_BETA[3] + P[3][0]*SH_BETA[6]*SH_BETA[2] + P[3][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     334       30707 :         Kfusion[4] = SK_BETA[0]*(P[4][5]*SK_BETA[1] - P[4][2]*SK_BETA[4] - P[4][3]*SK_BETA[2] + P[4][4]*SK_BETA[3] + P[4][22]*SK_BETA[2] - P[4][23]*SK_BETA[3] + P[4][0]*SH_BETA[6]*SH_BETA[2] + P[4][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     335       30707 :         Kfusion[5] = SK_BETA[0]*(P[5][5]*SK_BETA[1] - P[5][2]*SK_BETA[4] - P[5][3]*SK_BETA[2] + P[5][4]*SK_BETA[3] + P[5][22]*SK_BETA[2] - P[5][23]*SK_BETA[3] + P[5][0]*SH_BETA[6]*SH_BETA[2] + P[5][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     336       30707 :         Kfusion[6] = SK_BETA[0]*(P[6][5]*SK_BETA[1] - P[6][2]*SK_BETA[4] - P[6][3]*SK_BETA[2] + P[6][4]*SK_BETA[3] + P[6][22]*SK_BETA[2] - P[6][23]*SK_BETA[3] + P[6][0]*SH_BETA[6]*SH_BETA[2] + P[6][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     337       30707 :         Kfusion[7] = SK_BETA[0]*(P[7][5]*SK_BETA[1] - P[7][2]*SK_BETA[4] - P[7][3]*SK_BETA[2] + P[7][4]*SK_BETA[3] + P[7][22]*SK_BETA[2] - P[7][23]*SK_BETA[3] + P[7][0]*SH_BETA[6]*SH_BETA[2] + P[7][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     338       30707 :         Kfusion[8] = SK_BETA[0]*(P[8][5]*SK_BETA[1] - P[8][2]*SK_BETA[4] - P[8][3]*SK_BETA[2] + P[8][4]*SK_BETA[3] + P[8][22]*SK_BETA[2] - P[8][23]*SK_BETA[3] + P[8][0]*SH_BETA[6]*SH_BETA[2] + P[8][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     339       30707 :         Kfusion[9] = SK_BETA[0]*(P[9][5]*SK_BETA[1] - P[9][2]*SK_BETA[4] - P[9][3]*SK_BETA[2] + P[9][4]*SK_BETA[3] + P[9][22]*SK_BETA[2] - P[9][23]*SK_BETA[3] + P[9][0]*SH_BETA[6]*SH_BETA[2] + P[9][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     340       30707 :         Kfusion[10] = SK_BETA[0]*(P[10][5]*SK_BETA[1] - P[10][2]*SK_BETA[4] - P[10][3]*SK_BETA[2] + P[10][4]*SK_BETA[3] + P[10][22]*SK_BETA[2] - P[10][23]*SK_BETA[3] + P[10][0]*SH_BETA[6]*SH_BETA[2] + P[10][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     341       30707 :         Kfusion[11] = SK_BETA[0]*(P[11][5]*SK_BETA[1] - P[11][2]*SK_BETA[4] - P[11][3]*SK_BETA[2] + P[11][4]*SK_BETA[3] + P[11][22]*SK_BETA[2] - P[11][23]*SK_BETA[3] + P[11][0]*SH_BETA[6]*SH_BETA[2] + P[11][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     342       30707 :         Kfusion[12] = SK_BETA[0]*(P[12][5]*SK_BETA[1] - P[12][2]*SK_BETA[4] - P[12][3]*SK_BETA[2] + P[12][4]*SK_BETA[3] + P[12][22]*SK_BETA[2] - P[12][23]*SK_BETA[3] + P[12][0]*SH_BETA[6]*SH_BETA[2] + P[12][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     343       30707 :         Kfusion[13] = SK_BETA[0]*(P[13][5]*SK_BETA[1] - P[13][2]*SK_BETA[4] - P[13][3]*SK_BETA[2] + P[13][4]*SK_BETA[3] + P[13][22]*SK_BETA[2] - P[13][23]*SK_BETA[3] + P[13][0]*SH_BETA[6]*SH_BETA[2] + P[13][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     344       30707 :         Kfusion[14] = SK_BETA[0]*(P[14][5]*SK_BETA[1] - P[14][2]*SK_BETA[4] - P[14][3]*SK_BETA[2] + P[14][4]*SK_BETA[3] + P[14][22]*SK_BETA[2] - P[14][23]*SK_BETA[3] + P[14][0]*SH_BETA[6]*SH_BETA[2] + P[14][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     345       30707 :         Kfusion[15] = SK_BETA[0]*(P[15][5]*SK_BETA[1] - P[15][2]*SK_BETA[4] - P[15][3]*SK_BETA[2] + P[15][4]*SK_BETA[3] + P[15][22]*SK_BETA[2] - P[15][23]*SK_BETA[3] + P[15][0]*SH_BETA[6]*SH_BETA[2] + P[15][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     346       30707 :         Kfusion[22] = SK_BETA[0]*(P[22][5]*SK_BETA[1] - P[22][2]*SK_BETA[4] - P[22][3]*SK_BETA[2] + P[22][4]*SK_BETA[3] + P[22][22]*SK_BETA[2] - P[22][23]*SK_BETA[3] + P[22][0]*SH_BETA[6]*SH_BETA[2] + P[22][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     347       30707 :         Kfusion[23] = SK_BETA[0]*(P[23][5]*SK_BETA[1] - P[23][2]*SK_BETA[4] - P[23][3]*SK_BETA[2] + P[23][4]*SK_BETA[3] + P[23][22]*SK_BETA[2] - P[23][23]*SK_BETA[3] + P[23][0]*SH_BETA[6]*SH_BETA[2] + P[23][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     348             :         // zero Kalman gains to inhibit magnetic field state estimation
     349       30707 :         if (!inhibitMagStates) {
     350       27988 :             Kfusion[16] = SK_BETA[0]*(P[16][5]*SK_BETA[1] - P[16][2]*SK_BETA[4] - P[16][3]*SK_BETA[2] + P[16][4]*SK_BETA[3] + P[16][22]*SK_BETA[2] - P[16][23]*SK_BETA[3] + P[16][0]*SH_BETA[6]*SH_BETA[2] + P[16][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     351       27988 :             Kfusion[17] = SK_BETA[0]*(P[17][5]*SK_BETA[1] - P[17][2]*SK_BETA[4] - P[17][3]*SK_BETA[2] + P[17][4]*SK_BETA[3] + P[17][22]*SK_BETA[2] - P[17][23]*SK_BETA[3] + P[17][0]*SH_BETA[6]*SH_BETA[2] + P[17][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     352       27988 :             Kfusion[18] = SK_BETA[0]*(P[18][5]*SK_BETA[1] - P[18][2]*SK_BETA[4] - P[18][3]*SK_BETA[2] + P[18][4]*SK_BETA[3] + P[18][22]*SK_BETA[2] - P[18][23]*SK_BETA[3] + P[18][0]*SH_BETA[6]*SH_BETA[2] + P[18][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     353       27988 :             Kfusion[19] = SK_BETA[0]*(P[19][5]*SK_BETA[1] - P[19][2]*SK_BETA[4] - P[19][3]*SK_BETA[2] + P[19][4]*SK_BETA[3] + P[19][22]*SK_BETA[2] - P[19][23]*SK_BETA[3] + P[19][0]*SH_BETA[6]*SH_BETA[2] + P[19][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     354       27988 :             Kfusion[20] = SK_BETA[0]*(P[20][5]*SK_BETA[1] - P[20][2]*SK_BETA[4] - P[20][3]*SK_BETA[2] + P[20][4]*SK_BETA[3] + P[20][22]*SK_BETA[2] - P[20][23]*SK_BETA[3] + P[20][0]*SH_BETA[6]*SH_BETA[2] + P[20][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     355       27988 :             Kfusion[21] = SK_BETA[0]*(P[21][5]*SK_BETA[1] - P[21][2]*SK_BETA[4] - P[21][3]*SK_BETA[2] + P[21][4]*SK_BETA[3] + P[21][22]*SK_BETA[2] - P[21][23]*SK_BETA[3] + P[21][0]*SH_BETA[6]*SH_BETA[2] + P[21][1]*SH_BETA[1]*SH_BETA[3]*SH_BETA[2]);
     356             :         } else {
     357       19033 :             for (uint8_t i=16; i<=21; i++) {
     358       16314 :                 Kfusion[i] = 0.0f;
     359             :             }
     360             :         }
     361             : 
     362             :         // calculate predicted sideslip angle and innovation using small angle approximation
     363       30707 :         innovBeta = vel_rel_wind.y / vel_rel_wind.x;
     364             : 
     365             :         // reject measurement if greater than 3-sigma inconsistency
     366       30707 :         if (innovBeta > 0.5f) {
     367             :             return;
     368             :         }
     369             : 
     370             :         // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
     371       30215 :         stateStruct.angErr.zero();
     372             : 
     373             :         // correct the state vector
     374      755375 :         for (uint8_t j= 0; j<=stateIndexLim; j++) {
     375      725160 :             statesArray[j] = statesArray[j] - Kfusion[j] * innovBeta;
     376             :         }
     377             : 
     378             :         // the first 3 states represent the angular misalignment vector.
     379             :         //  This is used to correct the estimated quaternion on the current time step
     380       30215 :         stateStruct.quat.rotate(stateStruct.angErr);
     381             : 
     382             :         // correct the covariance P = (I - K*H)*P
     383             :         // take advantage of the empty columns in KH to reduce the
     384             :         // number of operations
     385      755375 :         for (unsigned i = 0; i<=stateIndexLim; i++) {
     386     5076120 :             for (unsigned j = 0; j<=5; j++) {
     387     4350960 :                 KH[i][j] = Kfusion[i] * H_BETA[j];
     388             :             }
     389    12327720 :             for (unsigned j = 6; j<=21; j++) {
     390    11602560 :                 KH[i][j] = 0.0f;
     391             :             }
     392     2175480 :             for (unsigned j = 22; j<=23; j++) {
     393     1450320 :                 KH[i][j] = Kfusion[i] * H_BETA[j];
     394             :             }
     395             :         }
     396      755375 :         for (unsigned j = 0; j<=stateIndexLim; j++) {
     397    18129000 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     398             :                 ftype res = 0;
     399    17403840 :                 res += KH[i][0] * P[0][j];
     400    17403840 :                 res += KH[i][1] * P[1][j];
     401    17403840 :                 res += KH[i][2] * P[2][j];
     402    17403840 :                 res += KH[i][3] * P[3][j];
     403    17403840 :                 res += KH[i][4] * P[4][j];
     404    17403840 :                 res += KH[i][5] * P[5][j];
     405    17403840 :                 res += KH[i][22] * P[22][j];
     406    17403840 :                 res += KH[i][23] * P[23][j];
     407    17403840 :                 KHP[i][j] = res;
     408             :             }
     409             :         }
     410      755375 :         for (unsigned i = 0; i<=stateIndexLim; i++) {
     411    18129000 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     412    17403840 :                 P[i][j] = P[i][j] - KHP[i][j];
     413             :             }
     414             :         }
     415             :     }
     416             : 
     417             :     // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
     418       30590 :     ForceSymmetry();
     419       30590 :     ConstrainVariances();
     420             : }
     421             : 
     422             : /********************************************************
     423             : *                   MISC FUNCTIONS                      *
     424             : ********************************************************/
     425             :