Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       3             : 
       4             : #include "AP_NavEKF2.h"
       5             : #include "AP_NavEKF2_core.h"
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9             : /********************************************************
      10             : *                   RESET FUNCTIONS                     *
      11             : ********************************************************/
      12             : 
      13             : /********************************************************
      14             : *                   FUSE MEASURED_DATA                  *
      15             : ********************************************************/
      16             : 
      17             : // select fusion of optical flow measurements
      18     3248540 : void NavEKF2_core::SelectFlowFusion()
      19             : {
      20             :     // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
      21             :     // If so, don't fuse measurements on this time step to reduce frame over-runs
      22             :     // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
      23     3248540 :     if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
      24       18168 :         optFlowFusionDelayed = true;
      25       18168 :         return;
      26             :     } else {
      27     3230372 :         optFlowFusionDelayed = false;
      28             :     }
      29             : 
      30             :     // Perform Data Checks
      31             :     // Check if the optical flow data is still valid
      32     3230372 :     flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
      33             :     // check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0
      34     3230372 :     gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == HGT_SOURCE_RNG);
      35             :     // Perform tilt check
      36     3230372 :     bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
      37             :     // Constrain measurements to zero if takeoff is not detected and the height above ground
      38             :     // is insufficient to achieve acceptable focus. This allows the vehicle to be picked up
      39             :     // and carried to test optical flow operation
      40     3230372 :     if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
      41      895507 :         ofDataDelayed.flowRadXYcomp.zero();
      42      895507 :         ofDataDelayed.flowRadXY.zero();
      43      895507 :         flowDataValid = true;
      44             :     }
      45             : 
      46             :     // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
      47     3230372 :     if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
      48             :         // Estimate the terrain offset (runs a one state EKF)
      49        2798 :         EstimateTerrainOffset();
      50             :     }
      51             : 
      52             :     // Fuse optical flow data into the main filter
      53     3230372 :     if (flowDataToFuse && tiltOK) {
      54           0 :         if (frontend->_flowUse == FLOW_USE_NAV) {
      55             :             // Set the flow noise used by the fusion processes
      56           0 :             R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
      57             :             // Fuse the optical flow X and Y axis data into the main filter sequentially
      58           0 :             FuseOptFlow();
      59             :         }
      60             :         // reset flag to indicate that no new flow data is available for fusion
      61           0 :         flowDataToFuse = false;
      62             :     }
      63             : }
      64             : 
      65             : /*
      66             : Estimation of terrain offset using a single state EKF
      67             : The filter can fuse motion compensated optical flow rates and range finder measurements
      68             : Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator
      69             : */
      70        2798 : void NavEKF2_core::EstimateTerrainOffset()
      71             : {
      72             :     // horizontal velocity squared
      73        2798 :     ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
      74             : 
      75             :     // don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
      76             :     // don't fuse flow data if it exceeds validity limits
      77             :     // don't update terrain offset if ground is being used as the zero height datum in the main filter
      78        2798 :     bool cantFuseFlowData = ((frontend->_flowUse != FLOW_USE_TERRAIN)
      79        1686 :     || gpsNotAvailable 
      80        1466 :     || PV_AidingMode == AID_RELATIVE 
      81        1466 :     || velHorizSq < 25.0f 
      82        4052 :     || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
      83             : 
      84        2798 :     if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) {
      85             :         // skip update
      86           0 :         inhibitGndState = true;
      87             :     } else {
      88        2798 :         inhibitGndState = false;
      89             :         // record the time we last updated the terrain offset state
      90        2798 :         gndHgtValidTime_ms = imuSampleTime_ms;
      91             : 
      92             :         // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
      93             :         // limit distance to prevent initialisation after bad gps causing bad numerical conditioning
      94        2798 :         ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
      95        2798 :         distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
      96        2798 :         prevPosN = stateStruct.position[0];
      97        2798 :         prevPosE = stateStruct.position[1];
      98             : 
      99             :         // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
     100        2798 :         ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
     101        2798 :         ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[5][5];
     102        2798 :         Popt += Pincrement;
     103        2798 :         timeAtLastAuxEKF_ms = imuSampleTime_ms;
     104             : 
     105             :         // fuse range finder data
     106        2798 :         if (rangeDataToFuse) {
     107             :             // predict range
     108        2798 :             ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
     109             : 
     110             :             // Copy required states to local variable names
     111        2798 :             ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
     112        2798 :             ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
     113        2798 :             ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
     114        2798 :             ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
     115             : 
     116             :             // Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
     117        2798 :             ftype R_RNG = frontend->_rngNoise.get();
     118             : 
     119             :             // calculate Kalman gain
     120        2798 :             ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
     121        2798 :             ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
     122             : 
     123             :             // Calculate the innovation variance for data logging
     124        2798 :             varInnovRng = (R_RNG + Popt/sq(SK_RNG));
     125             : 
     126             :             // constrain terrain height to be below the vehicle
     127        2798 :             terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
     128             : 
     129             :             // Calculate the measurement innovation
     130        2798 :             innovRng = predRngMeas - rangeDataDelayed.rng;
     131             : 
     132             :             // calculate the innovation consistency test ratio
     133        2798 :             auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
     134             : 
     135             :             // Check the innovation test ratio and don't fuse if too large
     136        2798 :             if (auxRngTestRatio < 1.0f) {
     137             :                 // correct the state
     138        2796 :                 terrainState -= K_RNG * innovRng;
     139             : 
     140             :                 // constrain the state
     141        2796 :                 terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
     142             : 
     143             :                 // correct the covariance
     144        2796 :                 Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
     145             : 
     146             :                 // prevent the state variance from becoming negative
     147        2796 :                 Popt = MAX(Popt,0.0f);
     148             : 
     149             :             }
     150             :         }
     151             : 
     152        2798 :         if (!cantFuseFlowData) {
     153             : 
     154             :             Vector3F relVelSensor;          // velocity of sensor relative to ground in sensor axes
     155        1254 :             Vector2F losPred;               // predicted optical flow angular rate measurement
     156        1254 :             ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
     157        1254 :             ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
     158        1254 :             ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
     159        1254 :             ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
     160             :             ftype K_OPT;
     161             :             ftype H_OPT;
     162             :             Vector2F auxFlowObsInnovVar;
     163             : 
     164             :             // predict range to centre of image
     165        1254 :             ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
     166             : 
     167             :             // constrain terrain height to be below the vehicle
     168        1254 :             terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
     169             : 
     170             :             // calculate relative velocity in sensor frame
     171        1254 :             relVelSensor = prevTnb*stateStruct.velocity;
     172             : 
     173             :             // divide velocity by range, subtract body rates and apply scale factor to
     174             :             // get predicted sensed angular optical rates relative to X and Y sensor axes
     175        1254 :             losPred.x =   relVelSensor.y / flowRngPred;
     176        1254 :             losPred.y = - relVelSensor.x / flowRngPred;
     177             : 
     178             :             // calculate innovations
     179        1254 :             auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
     180             : 
     181             :             // calculate observation jacobians 
     182        1254 :             ftype t2 = q0*q0;
     183        1254 :             ftype t3 = q1*q1;
     184        1254 :             ftype t4 = q2*q2;
     185        1254 :             ftype t5 = q3*q3;
     186        1254 :             ftype t6 = stateStruct.position.z - terrainState;
     187        1254 :             ftype t7 = 1.0f / (t6*t6);
     188        1254 :             ftype t8 = q0*q3*2.0f;
     189        1254 :             ftype t9 = t2-t3-t4+t5;
     190             : 
     191             :             // prevent the state variances from becoming badly conditioned
     192        1254 :             Popt = MAX(Popt,1E-6f);
     193             : 
     194             :             // calculate observation noise variance from parameter
     195        1254 :             ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
     196             : 
     197             :             // Fuse Y axis data
     198             : 
     199             :             // Calculate observation partial derivative
     200        1254 :             H_OPT = t7*t9*(-stateStruct.velocity.z*(q0*q2*2.0-q1*q3*2.0)+stateStruct.velocity.x*(t2+t3-t4-t5)+stateStruct.velocity.y*(t8+q1*q2*2.0));
     201             : 
     202             :             // calculate innovation variance
     203        1254 :             auxFlowObsInnovVar.y = H_OPT * Popt * H_OPT + flow_noise_variance;
     204             : 
     205             :             // calculate Kalman gain
     206        1254 :             K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
     207             : 
     208             :             // calculate the innovation consistency test ratio
     209        1254 :             auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
     210             : 
     211             :             // don't fuse if optical flow data is outside valid range
     212        1254 :             if (auxFlowTestRatio.y < 1.0f) {
     213             : 
     214             :                 // correct the state
     215           8 :                 terrainState -= K_OPT * auxFlowObsInnov.y;
     216             : 
     217             :                 // constrain the state
     218           8 :                 terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
     219             : 
     220             :                 // update intermediate variables used when fusing the X axis
     221           8 :                 t6 = stateStruct.position.z - terrainState;
     222           8 :                 t7 = 1.0f / (t6*t6);
     223             : 
     224             :                 // correct the covariance
     225           8 :                 Popt = Popt - K_OPT * H_OPT * Popt;
     226             : 
     227             :                 // prevent the state variances from becoming badly conditioned
     228           8 :                 Popt = MAX(Popt,1E-6f);
     229             :             }
     230             : 
     231             :             // fuse X axis data
     232        1254 :             H_OPT = -t7*t9*(stateStruct.velocity.z*(q0*q1*2.0+q2*q3*2.0)+stateStruct.velocity.y*(t2-t3+t4-t5)-stateStruct.velocity.x*(t8-q1*q2*2.0));
     233             : 
     234             :             // calculate innovation variances
     235        1254 :             auxFlowObsInnovVar.x = H_OPT * Popt * H_OPT + flow_noise_variance;
     236             : 
     237             :             // calculate Kalman gain
     238        1254 :             K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
     239             : 
     240             :             // calculate the innovation consistency test ratio
     241        1254 :             auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
     242             : 
     243             :             // don't fuse if optical flow data is outside valid range
     244        1254 :             if (auxFlowTestRatio.x < 1.0f) {
     245             : 
     246             :                 // correct the state
     247        1150 :                 terrainState -= K_OPT * auxFlowObsInnov.x;
     248             : 
     249             :                 // constrain the state
     250        1150 :                 terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
     251             : 
     252             :                 // correct the covariance
     253        1150 :                 Popt = Popt - K_OPT * H_OPT * Popt;
     254             : 
     255             :                 // prevent the state variances from becoming badly conditioned
     256        1150 :                 Popt = MAX(Popt,1E-6f);
     257             :             }
     258             :         }
     259             :     }
     260        2798 : }
     261             : 
     262             : /*
     263             :  * Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox.
     264             :  * The script file used to generate these and other equations in this filter can be found here:
     265             :  * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
     266             :  * Requires a valid terrain height estimate.
     267             : */
     268           0 : void NavEKF2_core::FuseOptFlow()
     269             : {
     270             :     Vector24 H_LOS;
     271             :     Vector3F relVelSensor;
     272             :     Vector14 SH_LOS;
     273             :     Vector2 losPred;
     274             : 
     275             :     // Copy required states to local variable names
     276           0 :     ftype q0  = stateStruct.quat[0];
     277           0 :     ftype q1 = stateStruct.quat[1];
     278           0 :     ftype q2 = stateStruct.quat[2];
     279           0 :     ftype q3 = stateStruct.quat[3];
     280           0 :     ftype vn = stateStruct.velocity.x;
     281           0 :     ftype ve = stateStruct.velocity.y;
     282           0 :     ftype vd = stateStruct.velocity.z;
     283           0 :     ftype pd = stateStruct.position.z;
     284             : 
     285             :     // constrain height above ground to be above range measured on ground
     286           0 :     ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
     287           0 :     ftype ptd = pd + heightAboveGndEst;
     288             : 
     289             :     // Calculate common expressions for observation jacobians
     290           0 :     SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
     291           0 :     SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
     292           0 :     SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
     293           0 :     SH_LOS[3] = 1/(pd - ptd);
     294           0 :     SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
     295             :     SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
     296             :     SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
     297           0 :     SH_LOS[7] = q0*q0;
     298           0 :     SH_LOS[8] = q1*q1;
     299           0 :     SH_LOS[9] = q2*q2;
     300           0 :     SH_LOS[10] = q3*q3;
     301           0 :     SH_LOS[11] = q0*q3*2.0f;
     302             :     SH_LOS[12] = pd-ptd;
     303           0 :     SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
     304             : 
     305             :     // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
     306           0 :     for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
     307             :         // calculate range from ground plane to centre of sensor fov assuming flat earth
     308           0 :         ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
     309             : 
     310             :         // correct range for flow sensor offset body frame position offset
     311             :         // the corrected value is the predicted range from the sensor focal point to the
     312             :         // centre of the image on the ground assuming flat terrain
     313           0 :         Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
     314           0 :         if (!posOffsetBody.is_zero()) {
     315           0 :             Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
     316           0 :             range -= posOffsetEarth.z / prevTnb.c.z;
     317             :         }
     318             : 
     319             :         // override with user specified height (if given, for rover)
     320             : #if APM_BUILD_TYPE(APM_BUILD_Rover)
     321           0 :         if (ofDataDelayed.heightOverride > 0) {
     322           0 :             range = ofDataDelayed.heightOverride;
     323             :         }
     324             : #endif
     325             : 
     326             :         // calculate relative velocity in sensor frame including the relative motion due to rotation
     327           0 :         relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody;
     328             : 
     329             :         // divide velocity by range  to get predicted angular LOS rates relative to X and Y axes
     330           0 :         losPred[0] =  relVelSensor.y/range;
     331           0 :         losPred[1] = -relVelSensor.x/range;
     332             : 
     333             :         // calculate observation jacobians and Kalman gains
     334           0 :         memset(&H_LOS[0], 0, sizeof(H_LOS));
     335           0 :         if (obsIndex == 0) {
     336           0 :             H_LOS[0] = SH_LOS[3]*SH_LOS[2]*SH_LOS[6]-SH_LOS[3]*SH_LOS[0]*SH_LOS[4];
     337           0 :             H_LOS[1] = SH_LOS[3]*SH_LOS[2]*SH_LOS[5];
     338           0 :             H_LOS[2] = SH_LOS[3]*SH_LOS[0]*SH_LOS[1];
     339           0 :             H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]-q1*q2*2.0f);
     340           0 :             H_LOS[4] = -SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]-SH_LOS[8]+SH_LOS[9]-SH_LOS[10]);
     341           0 :             H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
     342           0 :             H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
     343             : 
     344             :             ftype t2 = SH_LOS[3];
     345             :             ftype t3 = SH_LOS[0];
     346             :             ftype t4 = SH_LOS[2];
     347             :             ftype t5 = SH_LOS[6];
     348           0 :             ftype t100 = t2 * t3 * t5;
     349             :             ftype t6 = SH_LOS[4];
     350           0 :             ftype t7 = t2*t3*t6;
     351           0 :             ftype t9 = t2*t4*t5;
     352           0 :             ftype t8 = t7-t9;
     353             :             ftype t10 = q0*q3*2.0f;
     354           0 :             ftype t21 = q1*q2*2.0f;
     355           0 :             ftype t11 = t10-t21;
     356           0 :             ftype t101 = t2 * t3 * t11;
     357             :             ftype t12 = pd-ptd;
     358             :             ftype t13 = 1.0f/(t12*t12);
     359             :             ftype t104 = t3 * t4 * t13;
     360             :             ftype t14 = SH_LOS[5];
     361             :             ftype t102 = t2 * t4 * t14;
     362             :             ftype t15 = SH_LOS[1];
     363             :             ftype t103 = t2 * t3 * t15;
     364             :             ftype t16 = q0*q0;
     365             :             ftype t17 = q1*q1;
     366             :             ftype t18 = q2*q2;
     367             :             ftype t19 = q3*q3;
     368             :             ftype t20 = t16-t17+t18-t19;
     369           0 :             ftype t105 = t2 * t3 * t20;
     370           0 :             ftype t22 = P[1][1]*t102;
     371           0 :             ftype t23 = P[3][0]*t101;
     372           0 :             ftype t24 = P[8][0]*t104;
     373           0 :             ftype t25 = P[1][0]*t102;
     374           0 :             ftype t26 = P[2][0]*t103;
     375           0 :             ftype t63 = P[0][0]*t8;
     376           0 :             ftype t64 = P[5][0]*t100;
     377           0 :             ftype t65 = P[4][0]*t105;
     378           0 :             ftype t27 = t23+t24+t25+t26-t63-t64-t65;
     379           0 :             ftype t28 = P[3][3]*t101;
     380           0 :             ftype t29 = P[8][3]*t104;
     381           0 :             ftype t30 = P[1][3]*t102;
     382           0 :             ftype t31 = P[2][3]*t103;
     383           0 :             ftype t67 = P[0][3]*t8;
     384           0 :             ftype t68 = P[5][3]*t100;
     385           0 :             ftype t69 = P[4][3]*t105;
     386           0 :             ftype t32 = t28+t29+t30+t31-t67-t68-t69;
     387           0 :             ftype t33 = t101*t32;
     388           0 :             ftype t34 = P[3][8]*t101;
     389           0 :             ftype t35 = P[8][8]*t104;
     390           0 :             ftype t36 = P[1][8]*t102;
     391           0 :             ftype t37 = P[2][8]*t103;
     392           0 :             ftype t70 = P[0][8]*t8;
     393           0 :             ftype t71 = P[5][8]*t100;
     394           0 :             ftype t72 = P[4][8]*t105;
     395           0 :             ftype t38 = t34+t35+t36+t37-t70-t71-t72;
     396           0 :             ftype t39 = t104*t38;
     397           0 :             ftype t40 = P[3][1]*t101;
     398           0 :             ftype t41 = P[8][1]*t104;
     399           0 :             ftype t42 = P[2][1]*t103;
     400           0 :             ftype t73 = P[0][1]*t8;
     401           0 :             ftype t74 = P[5][1]*t100;
     402           0 :             ftype t75 = P[4][1]*t105;
     403           0 :             ftype t43 = t22+t40+t41+t42-t73-t74-t75;
     404           0 :             ftype t44 = t102*t43;
     405           0 :             ftype t45 = P[3][2]*t101;
     406           0 :             ftype t46 = P[8][2]*t104;
     407           0 :             ftype t47 = P[1][2]*t102;
     408           0 :             ftype t48 = P[2][2]*t103;
     409           0 :             ftype t76 = P[0][2]*t8;
     410           0 :             ftype t77 = P[5][2]*t100;
     411           0 :             ftype t78 = P[4][2]*t105;
     412           0 :             ftype t49 = t45+t46+t47+t48-t76-t77-t78;
     413           0 :             ftype t50 = t103*t49;
     414           0 :             ftype t51 = P[3][5]*t101;
     415           0 :             ftype t52 = P[8][5]*t104;
     416           0 :             ftype t53 = P[1][5]*t102;
     417           0 :             ftype t54 = P[2][5]*t103;
     418           0 :             ftype t79 = P[0][5]*t8;
     419           0 :             ftype t80 = P[5][5]*t100;
     420           0 :             ftype t81 = P[4][5]*t105;
     421           0 :             ftype t55 = t51+t52+t53+t54-t79-t80-t81;
     422           0 :             ftype t56 = P[3][4]*t101;
     423           0 :             ftype t57 = P[8][4]*t104;
     424           0 :             ftype t58 = P[1][4]*t102;
     425           0 :             ftype t59 = P[2][4]*t103;
     426           0 :             ftype t83 = P[0][4]*t8;
     427           0 :             ftype t84 = P[5][4]*t100;
     428           0 :             ftype t85 = P[4][4]*t105;
     429           0 :             ftype t60 = t56+t57+t58+t59-t83-t84-t85;
     430           0 :             ftype t66 = t8*t27;
     431           0 :             ftype t82 = t100*t55;
     432           0 :             ftype t86 = t105*t60;
     433           0 :             ftype t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86;
     434           0 :             ftype t62 = 1.0f/t61;
     435             : 
     436             :             // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
     437           0 :             if (t61 > R_LOS) {
     438             :                 t62 = 1.0f/t61;
     439           0 :                 faultStatus.bad_yflow = false;
     440             :             } else {
     441             :                 t61 = 0.0f;
     442             :                 t62 = 1.0f/R_LOS;
     443           0 :                 faultStatus.bad_yflow = true;
     444           0 :                 return;
     445             :             }
     446           0 :             varInnovOptFlow[0] = t61;
     447             : 
     448             :             // calculate innovation for X axis observation
     449           0 :             innovOptFlow[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
     450             : 
     451             :             // calculate Kalman gains for X-axis observation
     452           0 :             Kfusion[0] = t62*(-P[0][0]*t8-P[0][5]*t100+P[0][3]*t101+P[0][1]*t102+P[0][2]*t103+P[0][8]*t104-P[0][4]*t105);
     453           0 :             Kfusion[1] = t62*(t22-P[1][0]*t8-P[1][5]*t100+P[1][3]*t101+P[1][2]*t103+P[1][8]*t104-P[1][4]*t105);
     454           0 :             Kfusion[2] = t62*(t48-P[2][0]*t8-P[2][5]*t100+P[2][3]*t101+P[2][1]*t102+P[2][8]*t104-P[2][4]*t105);
     455           0 :             Kfusion[3] = t62*(t28-P[3][0]*t8-P[3][5]*t100+P[3][1]*t102+P[3][2]*t103+P[3][8]*t104-P[3][4]*t105);
     456           0 :             Kfusion[4] = t62*(-t85-P[4][0]*t8-P[4][5]*t100+P[4][3]*t101+P[4][1]*t102+P[4][2]*t103+P[4][8]*t104);
     457           0 :             Kfusion[5] = t62*(-t80-P[5][0]*t8+P[5][3]*t101+P[5][1]*t102+P[5][2]*t103+P[5][8]*t104-P[5][4]*t105);
     458           0 :             Kfusion[6] = t62*(-P[6][0]*t8-P[6][5]*t100+P[6][3]*t101+P[6][1]*t102+P[6][2]*t103+P[6][8]*t104-P[6][4]*t105);
     459           0 :             Kfusion[7] = t62*(-P[7][0]*t8-P[7][5]*t100+P[7][3]*t101+P[7][1]*t102+P[7][2]*t103+P[7][8]*t104-P[7][4]*t105);
     460           0 :             Kfusion[8] = t62*(t35-P[8][0]*t8-P[8][5]*t100+P[8][3]*t101+P[8][1]*t102+P[8][2]*t103-P[8][4]*t105);
     461           0 :             Kfusion[9] = t62*(-P[9][0]*t8-P[9][5]*t100+P[9][3]*t101+P[9][1]*t102+P[9][2]*t103+P[9][8]*t104-P[9][4]*t105);
     462           0 :             Kfusion[10] = t62*(-P[10][0]*t8-P[10][5]*t100+P[10][3]*t101+P[10][1]*t102+P[10][2]*t103+P[10][8]*t104-P[10][4]*t105);
     463           0 :             Kfusion[11] = t62*(-P[11][0]*t8-P[11][5]*t100+P[11][3]*t101+P[11][1]*t102+P[11][2]*t103+P[11][8]*t104-P[11][4]*t105);
     464           0 :             Kfusion[12] = t62*(-P[12][0]*t8-P[12][5]*t100+P[12][3]*t101+P[12][1]*t102+P[12][2]*t103+P[12][8]*t104-P[12][4]*t105);
     465           0 :             Kfusion[13] = t62*(-P[13][0]*t8-P[13][5]*t100+P[13][3]*t101+P[13][1]*t102+P[13][2]*t103+P[13][8]*t104-P[13][4]*t105);
     466           0 :             Kfusion[14] = t62*(-P[14][0]*t8-P[14][5]*t100+P[14][3]*t101+P[14][1]*t102+P[14][2]*t103+P[14][8]*t104-P[14][4]*t105);
     467           0 :             Kfusion[15] = t62*(-P[15][0]*t8-P[15][5]*t100+P[15][3]*t101+P[15][1]*t102+P[15][2]*t103+P[15][8]*t104-P[15][4]*t105);
     468           0 :             if (!inhibitWindStates) {
     469           0 :                 Kfusion[22] = t62*(-P[22][0]*t8-P[22][5]*t100+P[22][3]*t101+P[22][1]*t102+P[22][2]*t103+P[22][8]*t104-P[22][4]*t105);
     470           0 :                 Kfusion[23] = t62*(-P[23][0]*t8-P[23][5]*t100+P[23][3]*t101+P[23][1]*t102+P[23][2]*t103+P[23][8]*t104-P[23][4]*t105);
     471             :             } else {
     472           0 :                 Kfusion[22] = 0.0f;
     473           0 :                 Kfusion[23] = 0.0f;
     474             :             }
     475           0 :             if (!inhibitMagStates) {
     476           0 :                 Kfusion[16] = t62*(-P[16][0]*t8-P[16][5]*t100+P[16][3]*t101+P[16][1]*t102+P[16][2]*t103+P[16][8]*t104-P[16][4]*t105);
     477           0 :                 Kfusion[17] = t62*(-P[17][0]*t8-P[17][5]*t100+P[17][3]*t101+P[17][1]*t102+P[17][2]*t103+P[17][8]*t104-P[17][4]*t105);
     478           0 :                 Kfusion[18] = t62*(-P[18][0]*t8-P[18][5]*t100+P[18][3]*t101+P[18][1]*t102+P[18][2]*t103+P[18][8]*t104-P[18][4]*t105);
     479           0 :                 Kfusion[19] = t62*(-P[19][0]*t8-P[19][5]*t100+P[19][3]*t101+P[19][1]*t102+P[19][2]*t103+P[19][8]*t104-P[19][4]*t105);
     480           0 :                 Kfusion[20] = t62*(-P[20][0]*t8-P[20][5]*t100+P[20][3]*t101+P[20][1]*t102+P[20][2]*t103+P[20][8]*t104-P[20][4]*t105);
     481           0 :                 Kfusion[21] = t62*(-P[21][0]*t8-P[21][5]*t100+P[21][3]*t101+P[21][1]*t102+P[21][2]*t103+P[21][8]*t104-P[21][4]*t105);
     482             :             } else {
     483           0 :                 for (uint8_t i = 16; i <= 21; i++) {
     484           0 :                     Kfusion[i] = 0.0f;
     485             :                 }
     486             :             }
     487             : 
     488             :         } else {
     489             : 
     490           0 :             H_LOS[0] = -SH_LOS[3]*SH_LOS[6]*SH_LOS[1];
     491           0 :             H_LOS[1] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[4]-SH_LOS[3]*SH_LOS[1]*SH_LOS[5];
     492           0 :             H_LOS[2] = SH_LOS[3]*SH_LOS[2]*SH_LOS[0];
     493           0 :             H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]+SH_LOS[8]-SH_LOS[9]-SH_LOS[10]);
     494           0 :             H_LOS[4] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]+q1*q2*2.0f);
     495           0 :             H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
     496           0 :             H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
     497             : 
     498             :             ftype t2 = SH_LOS[3];
     499             :             ftype t3 = SH_LOS[0];
     500             :             ftype t4 = SH_LOS[1];
     501             :             ftype t5 = SH_LOS[5];
     502           0 :             ftype t100 = t2 * t3 * t5;
     503             :             ftype t6 = SH_LOS[4];
     504           0 :             ftype t7 = t2*t3*t6;
     505             :             ftype t8 = t2*t4*t5;
     506           0 :             ftype t9 = t7+t8;
     507             :             ftype t10 = q0*q3*2.0f;
     508             :             ftype t11 = q1*q2*2.0f;
     509             :             ftype t12 = t10+t11;
     510             :             ftype t101 = t2 * t3 * t12;
     511             :             ftype t13 = pd-ptd;
     512             :             ftype t14 = 1.0f/(t13*t13);
     513           0 :             ftype t104 = t3 * t4 * t14;
     514             :             ftype t15 = SH_LOS[6];
     515           0 :             ftype t105 = t2 * t4 * t15;
     516             :             ftype t16 = SH_LOS[2];
     517           0 :             ftype t102 = t2 * t3 * t16;
     518             :             ftype t17 = q0*q0;
     519             :             ftype t18 = q1*q1;
     520             :             ftype t19 = q2*q2;
     521             :             ftype t20 = q3*q3;
     522             :             ftype t21 = t17+t18-t19-t20;
     523             :             ftype t103 = t2 * t3 * t21;
     524           0 :             ftype t22 = P[0][0]*t105;
     525           0 :             ftype t23 = P[1][1]*t9;
     526           0 :             ftype t24 = P[8][1]*t104;
     527           0 :             ftype t25 = P[0][1]*t105;
     528           0 :             ftype t26 = P[5][1]*t100;
     529           0 :             ftype t64 = P[4][1]*t101;
     530           0 :             ftype t65 = P[2][1]*t102;
     531           0 :             ftype t66 = P[3][1]*t103;
     532           0 :             ftype t27 = t23+t24+t25+t26-t64-t65-t66;
     533           0 :             ftype t28 = t9*t27;
     534           0 :             ftype t29 = P[1][4]*t9;
     535           0 :             ftype t30 = P[8][4]*t104;
     536           0 :             ftype t31 = P[0][4]*t105;
     537           0 :             ftype t32 = P[5][4]*t100;
     538           0 :             ftype t67 = P[4][4]*t101;
     539           0 :             ftype t68 = P[2][4]*t102;
     540           0 :             ftype t69 = P[3][4]*t103;
     541           0 :             ftype t33 = t29+t30+t31+t32-t67-t68-t69;
     542           0 :             ftype t34 = P[1][8]*t9;
     543           0 :             ftype t35 = P[8][8]*t104;
     544           0 :             ftype t36 = P[0][8]*t105;
     545           0 :             ftype t37 = P[5][8]*t100;
     546           0 :             ftype t71 = P[4][8]*t101;
     547           0 :             ftype t72 = P[2][8]*t102;
     548           0 :             ftype t73 = P[3][8]*t103;
     549           0 :             ftype t38 = t34+t35+t36+t37-t71-t72-t73;
     550           0 :             ftype t39 = t104*t38;
     551           0 :             ftype t40 = P[1][0]*t9;
     552           0 :             ftype t41 = P[8][0]*t104;
     553           0 :             ftype t42 = P[5][0]*t100;
     554           0 :             ftype t74 = P[4][0]*t101;
     555           0 :             ftype t75 = P[2][0]*t102;
     556           0 :             ftype t76 = P[3][0]*t103;
     557           0 :             ftype t43 = t22+t40+t41+t42-t74-t75-t76;
     558           0 :             ftype t44 = t105*t43;
     559           0 :             ftype t45 = P[1][2]*t9;
     560           0 :             ftype t46 = P[8][2]*t104;
     561           0 :             ftype t47 = P[0][2]*t105;
     562           0 :             ftype t48 = P[5][2]*t100;
     563           0 :             ftype t63 = P[2][2]*t102;
     564           0 :             ftype t77 = P[4][2]*t101;
     565           0 :             ftype t78 = P[3][2]*t103;
     566           0 :             ftype t49 = t45+t46+t47+t48-t63-t77-t78;
     567           0 :             ftype t50 = P[1][5]*t9;
     568           0 :             ftype t51 = P[8][5]*t104;
     569           0 :             ftype t52 = P[0][5]*t105;
     570           0 :             ftype t53 = P[5][5]*t100;
     571           0 :             ftype t80 = P[4][5]*t101;
     572           0 :             ftype t81 = P[2][5]*t102;
     573           0 :             ftype t82 = P[3][5]*t103;
     574           0 :             ftype t54 = t50+t51+t52+t53-t80-t81-t82;
     575           0 :             ftype t55 = t100*t54;
     576           0 :             ftype t56 = P[1][3]*t9;
     577           0 :             ftype t57 = P[8][3]*t104;
     578           0 :             ftype t58 = P[0][3]*t105;
     579           0 :             ftype t59 = P[5][3]*t100;
     580           0 :             ftype t83 = P[4][3]*t101;
     581           0 :             ftype t84 = P[2][3]*t102;
     582           0 :             ftype t85 = P[3][3]*t103;
     583           0 :             ftype t60 = t56+t57+t58+t59-t83-t84-t85;
     584           0 :             ftype t70 = t101*t33;
     585           0 :             ftype t79 = t102*t49;
     586           0 :             ftype t86 = t103*t60;
     587           0 :             ftype t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86;
     588           0 :             ftype t62 = 1.0f/t61;
     589             : 
     590             :             // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
     591           0 :             if (t61 > R_LOS) {
     592             :                 t62 = 1.0f/t61;
     593           0 :                 faultStatus.bad_yflow = false;
     594             :             } else {
     595             :                 t61 = 0.0f;
     596             :                 t62 = 1.0f/R_LOS;
     597           0 :                 faultStatus.bad_yflow = true;
     598           0 :                 return;
     599             :             }
     600           0 :             varInnovOptFlow[1] = t61;
     601             : 
     602             :             // calculate innovation for Y observation
     603           0 :             innovOptFlow[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
     604             : 
     605             :             // calculate Kalman gains for the Y-axis observation
     606           0 :             Kfusion[0] = -t62*(t22+P[0][1]*t9+P[0][5]*t100-P[0][4]*t101-P[0][2]*t102-P[0][3]*t103+P[0][8]*t104);
     607           0 :             Kfusion[1] = -t62*(t23+P[1][5]*t100+P[1][0]*t105-P[1][4]*t101-P[1][2]*t102-P[1][3]*t103+P[1][8]*t104);
     608           0 :             Kfusion[2] = -t62*(-t63+P[2][1]*t9+P[2][5]*t100+P[2][0]*t105-P[2][4]*t101-P[2][3]*t103+P[2][8]*t104);
     609           0 :             Kfusion[3] = -t62*(-t85+P[3][1]*t9+P[3][5]*t100+P[3][0]*t105-P[3][4]*t101-P[3][2]*t102+P[3][8]*t104);
     610           0 :             Kfusion[4] = -t62*(-t67+P[4][1]*t9+P[4][5]*t100+P[4][0]*t105-P[4][2]*t102-P[4][3]*t103+P[4][8]*t104);
     611           0 :             Kfusion[5] = -t62*(t53+P[5][1]*t9+P[5][0]*t105-P[5][4]*t101-P[5][2]*t102-P[5][3]*t103+P[5][8]*t104);
     612           0 :             Kfusion[6] = -t62*(P[6][1]*t9+P[6][5]*t100+P[6][0]*t105-P[6][4]*t101-P[6][2]*t102-P[6][3]*t103+P[6][8]*t104);
     613           0 :             Kfusion[7] = -t62*(P[7][1]*t9+P[7][5]*t100+P[7][0]*t105-P[7][4]*t101-P[7][2]*t102-P[7][3]*t103+P[7][8]*t104);
     614           0 :             Kfusion[8] = -t62*(t35+P[8][1]*t9+P[8][5]*t100+P[8][0]*t105-P[8][4]*t101-P[8][2]*t102-P[8][3]*t103);
     615           0 :             Kfusion[9] = -t62*(P[9][1]*t9+P[9][5]*t100+P[9][0]*t105-P[9][4]*t101-P[9][2]*t102-P[9][3]*t103+P[9][8]*t104);
     616           0 :             Kfusion[10] = -t62*(P[10][1]*t9+P[10][5]*t100+P[10][0]*t105-P[10][4]*t101-P[10][2]*t102-P[10][3]*t103+P[10][8]*t104);
     617           0 :             Kfusion[11] = -t62*(P[11][1]*t9+P[11][5]*t100+P[11][0]*t105-P[11][4]*t101-P[11][2]*t102-P[11][3]*t103+P[11][8]*t104);
     618           0 :             Kfusion[12] = -t62*(P[12][1]*t9+P[12][5]*t100+P[12][0]*t105-P[12][4]*t101-P[12][2]*t102-P[12][3]*t103+P[12][8]*t104);
     619           0 :             Kfusion[13] = -t62*(P[13][1]*t9+P[13][5]*t100+P[13][0]*t105-P[13][4]*t101-P[13][2]*t102-P[13][3]*t103+P[13][8]*t104);
     620           0 :             Kfusion[14] = -t62*(P[14][1]*t9+P[14][5]*t100+P[14][0]*t105-P[14][4]*t101-P[14][2]*t102-P[14][3]*t103+P[14][8]*t104);
     621           0 :             Kfusion[15] = -t62*(P[15][1]*t9+P[15][5]*t100+P[15][0]*t105-P[15][4]*t101-P[15][2]*t102-P[15][3]*t103+P[15][8]*t104);
     622           0 :             if (!inhibitWindStates) {
     623           0 :                 Kfusion[22] = -t62*(P[22][1]*t9+P[22][5]*t100+P[22][0]*t105-P[22][4]*t101-P[22][2]*t102-P[22][3]*t103+P[22][8]*t104);
     624           0 :                 Kfusion[23] = -t62*(P[23][1]*t9+P[23][5]*t100+P[23][0]*t105-P[23][4]*t101-P[23][2]*t102-P[23][3]*t103+P[23][8]*t104);
     625             :             } else {
     626           0 :                 Kfusion[22] = 0.0f;
     627           0 :                 Kfusion[23] = 0.0f;
     628             :             }
     629           0 :             if (!inhibitMagStates) {
     630           0 :                 Kfusion[16] = -t62*(P[16][1]*t9+P[16][5]*t100+P[16][0]*t105-P[16][4]*t101-P[16][2]*t102-P[16][3]*t103+P[16][8]*t104);
     631           0 :                 Kfusion[17] = -t62*(P[17][1]*t9+P[17][5]*t100+P[17][0]*t105-P[17][4]*t101-P[17][2]*t102-P[17][3]*t103+P[17][8]*t104);
     632           0 :                 Kfusion[18] = -t62*(P[18][1]*t9+P[18][5]*t100+P[18][0]*t105-P[18][4]*t101-P[18][2]*t102-P[18][3]*t103+P[18][8]*t104);
     633           0 :                 Kfusion[19] = -t62*(P[19][1]*t9+P[19][5]*t100+P[19][0]*t105-P[19][4]*t101-P[19][2]*t102-P[19][3]*t103+P[19][8]*t104);
     634           0 :                 Kfusion[20] = -t62*(P[20][1]*t9+P[20][5]*t100+P[20][0]*t105-P[20][4]*t101-P[20][2]*t102-P[20][3]*t103+P[20][8]*t104);
     635           0 :                 Kfusion[21] = -t62*(P[21][1]*t9+P[21][5]*t100+P[21][0]*t105-P[21][4]*t101-P[21][2]*t102-P[21][3]*t103+P[21][8]*t104);
     636             :             } else {
     637           0 :                 for (uint8_t i = 16; i <= 21; i++) {
     638           0 :                     Kfusion[i] = 0.0f;
     639             :                 }
     640             :             }
     641             :         }
     642             : 
     643             :         // calculate the innovation consistency test ratio
     644           0 :         flowTestRatio[obsIndex] = sq(innovOptFlow[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * varInnovOptFlow[obsIndex]);
     645             : 
     646             :         // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
     647           0 :         if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
     648             :             // record the last time observations were accepted for fusion
     649           0 :             prevFlowFuseTime_ms = imuSampleTime_ms;
     650             : 
     651             :             // correct the covariance P = (I - K*H)*P
     652             :             // take advantage of the empty columns in KH to reduce the
     653             :             // number of operations
     654           0 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     655           0 :                 for (unsigned j = 0; j<=5; j++) {
     656           0 :                     KH[i][j] = Kfusion[i] * H_LOS[j];
     657             :                 }
     658           0 :                 for (unsigned j = 6; j<=7; j++) {
     659           0 :                     KH[i][j] = 0.0f;
     660             :                 }
     661           0 :                 KH[i][8] = Kfusion[i] * H_LOS[8];
     662           0 :                 for (unsigned j = 9; j<=23; j++) {
     663           0 :                     KH[i][j] = 0.0f;
     664             :                 }
     665             :             }
     666           0 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     667           0 :                 for (unsigned i = 0; i<=stateIndexLim; i++) {
     668             :                     ftype res = 0;
     669           0 :                     res += KH[i][0] * P[0][j];
     670           0 :                     res += KH[i][1] * P[1][j];
     671           0 :                     res += KH[i][2] * P[2][j];
     672           0 :                     res += KH[i][3] * P[3][j];
     673           0 :                     res += KH[i][4] * P[4][j];
     674           0 :                     res += KH[i][5] * P[5][j];
     675           0 :                     res += KH[i][8] * P[8][j];
     676           0 :                     KHP[i][j] = res;
     677             :                 }
     678             :             }
     679             : 
     680             :             // Check that we are not going to drive any variances negative and skip the update if so
     681             :             bool healthyFusion = true;
     682           0 :             for (uint8_t i= 0; i<=stateIndexLim; i++) {
     683           0 :                 if (KHP[i][i] > P[i][i]) {
     684             :                     healthyFusion = false;
     685             :                 }
     686             :             }
     687             : 
     688           0 :             if (healthyFusion) {
     689             :                 // update the covariance matrix
     690           0 :                 for (uint8_t i= 0; i<=stateIndexLim; i++) {
     691           0 :                     for (uint8_t j= 0; j<=stateIndexLim; j++) {
     692           0 :                         P[i][j] = P[i][j] - KHP[i][j];
     693             :                     }
     694             :                 }
     695             : 
     696             :                 // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
     697           0 :                 ForceSymmetry();
     698           0 :                 ConstrainVariances();
     699             : 
     700             :                 // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
     701           0 :                 stateStruct.angErr.zero();
     702             : 
     703             :                 // correct the state vector
     704           0 :                 for (uint8_t j= 0; j<=stateIndexLim; j++) {
     705           0 :                     statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
     706             :                 }
     707             : 
     708             :                 // the first 3 states represent the angular misalignment vector.
     709             :                 // This is used to correct the estimated quaternion on the current time step
     710           0 :                 stateStruct.quat.rotate(stateStruct.angErr);
     711             : 
     712             :             } else {
     713             :                 // record bad axis
     714           0 :                 if (obsIndex == 0) {
     715           0 :                     faultStatus.bad_xflow = true;
     716             :                 } else if (obsIndex == 1) {
     717           0 :                     faultStatus.bad_yflow = true;
     718             :                 }
     719             : 
     720             :             }
     721             :         }
     722             :     }
     723             : }
     724             : 
     725             : /********************************************************
     726             : *                   MISC FUNCTIONS                      *
     727             : ********************************************************/
     728             :