Line data    Source code

       1             : #include "AP_NavEKF3.h"
       2             : #include "AP_NavEKF3_core.h"
       3             : 
       4             : #if EK3_FEATURE_BEACON_FUSION
       5             : 
       6             : #include <AP_DAL/AP_DAL.h>
       7             : 
       8             : // initialise state:
       9        3144 : void NavEKF3_core::BeaconFusion::InitialiseVariables()
      10             : {
      11        3144 :     memset((void *)&dataDelayed, 0, sizeof(dataDelayed));
      12        3144 :     lastPassTime_ms = 0;
      13        3144 :     testRatio = 0.0f;
      14        3144 :     health = false;
      15        3144 :     varInnov = 0.0f;
      16        3144 :     innov = 0.0f;
      17        3144 :     memset(&lastTime_ms, 0, sizeof(lastTime_ms));
      18        3144 :     dataToFuse = false;
      19        3144 :     vehiclePosNED.zero();
      20        3144 :     vehiclePosErr = 1.0f;
      21        3144 :     last3DmeasTime_ms = 0;
      22        3144 :     goodToAlign = false;
      23        3144 :     lastChecked = 0;
      24        3144 :     receiverPos.zero();
      25        3144 :     memset(&receiverPosCov, 0, sizeof(receiverPosCov));
      26        3144 :     alignmentStarted =  false;
      27        3144 :     alignmentCompleted = false;
      28        3144 :     lastIndex = 0;
      29        3144 :     posSum.zero();
      30        3144 :     numMeas = 0;
      31        3144 :     sum = 0.0f;
      32        3144 :     N = 0;
      33        3144 :     maxPosD = 0.0f;
      34        3144 :     minPosD = 0.0f;
      35        3144 :     posDownOffsetMax = 0.0f;
      36        3144 :     posOffsetMaxVar = 0.0f;
      37        3144 :     maxOffsetStateChangeFilt = 0.0f;
      38        3144 :     posDownOffsetMin = 0.0f;
      39        3144 :     posOffsetMinVar = 0.0f;
      40        3144 :     minOffsetStateChangeFilt = 0.0f;
      41        3144 :     fuseDataReportIndex = 0;
      42        3144 :     delete[] fusionReport;
      43        3144 :     fusionReport = nullptr;
      44        3144 :     numFusionReports = 0;
      45        3144 :     auto *beacon = dal.beacon();
      46        3144 :     if (beacon != nullptr) {
      47           8 :         const uint8_t count = beacon->count();
      48          40 :         fusionReport = NEW_NOTHROW BeaconFusion::FusionReport[count];
      49           8 :         if (fusionReport != nullptr) {
      50           8 :             numFusionReports = count;
      51             :         }
      52             :     }
      53        3144 :     posOffsetNED.zero();
      54        3144 :     originEstInit = false;
      55        3144 : }
      56             : 
      57             : /********************************************************
      58             : *                   FUSE MEASURED_DATA                  *
      59             : ********************************************************/
      60             : 
      61             : // select fusion of range beacon measurements
      62    19965274 : void NavEKF3_core::SelectRngBcnFusion()
      63             : {
      64             :     // read range data from the sensor and check for new data in the buffer
      65    19965274 :     readRngBcnData();
      66             : 
      67             :     // Determine if we need to fuse range beacon data on this time step
      68    19965274 :     if (rngBcn.dataToFuse) {
      69       48202 :         if (PV_AidingMode == AID_ABSOLUTE) {
      70       32663 :             if ((frontend->sources.getPosXYSource(core_index) == AP_NavEKF_Source::SourceXY::BEACON) && rngBcn.alignmentCompleted) {
      71       17621 :                 if (!rngBcn.originEstInit) {
      72           6 :                     rngBcn.originEstInit = true;
      73           6 :                     rngBcn.posOffsetNED.x = rngBcn.receiverPos.x - stateStruct.position.x;
      74           6 :                     rngBcn.posOffsetNED.y = rngBcn.receiverPos.y - stateStruct.position.y;
      75             :                 }
      76             :                 // beacons are used as the primary means of position reference
      77       17621 :                 FuseRngBcn();
      78             :             } else {
      79             :                 // If another source (i.e. GPS, ExtNav) is the primary reference, we continue to use the beacon data
      80             :                 // to calculate an independent position that is used to update the beacon position offset if we need to
      81             :                 // start using beacon data as the primary reference.
      82       15042 :                 FuseRngBcnStatic();
      83             :                 // record that the beacon origin needs to be initialised
      84       15042 :                 rngBcn.originEstInit = false;
      85             :             }
      86             :         } else {
      87             :             // If we aren't able to use the data in the main filter, use a simple 3-state filter to estimate position only
      88       15539 :             FuseRngBcnStatic();
      89             :             // record that the beacon origin needs to be initialised
      90       15539 :             rngBcn.originEstInit = false;
      91             :         }
      92             :     }
      93    19965274 : }
      94             : 
      95       17621 : void NavEKF3_core::FuseRngBcn()
      96             : {
      97             :     // declarations
      98             :     ftype pn;
      99             :     ftype pe;
     100             :     ftype pd;
     101             :     ftype bcn_pn;
     102             :     ftype bcn_pe;
     103             :     ftype bcn_pd;
     104       17621 :     const ftype R_BCN = sq(MAX(rngBcn.dataDelayed.rngErr , 0.1f));
     105             :     ftype rngPred;
     106             : 
     107             :     // health is set bad until test passed
     108       17621 :     rngBcn.health = false;
     109             : 
     110       17621 :     if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
     111             :         // calculate the vertical offset from EKF datum to beacon datum
     112       17621 :         CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
     113             :     } else {
     114           0 :         rngBcn.posOffsetNED.z = 0.0f;
     115             :     }
     116             : 
     117             :     // copy required states to local variable names
     118       17621 :     pn = stateStruct.position.x;
     119       17621 :     pe = stateStruct.position.y;
     120       17621 :     pd = stateStruct.position.z;
     121       17621 :     bcn_pn = rngBcn.dataDelayed.beacon_posNED.x;
     122       17621 :     bcn_pe = rngBcn.dataDelayed.beacon_posNED.y;
     123       17621 :     bcn_pd = rngBcn.dataDelayed.beacon_posNED.z + rngBcn.posOffsetNED.z;
     124             : 
     125             :     // predicted range
     126       17621 :     Vector3F deltaPosNED = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
     127       17621 :     rngPred = deltaPosNED.length();
     128             : 
     129             :     // calculate measurement innovation
     130       17621 :     rngBcn.innov = rngPred - rngBcn.dataDelayed.rng;
     131             : 
     132             :     // perform fusion of range measurement
     133       17621 :     if (rngPred > 0.1f)
     134             :     {
     135             :         // calculate observation jacobians
     136             :         ftype H_BCN[24];
     137       17621 :         memset(H_BCN, 0, sizeof(H_BCN));
     138       17621 :         ftype t2 = bcn_pd-pd;
     139       17621 :         ftype t3 = bcn_pe-pe;
     140       17621 :         ftype t4 = bcn_pn-pn;
     141       17621 :         ftype t5 = t2*t2;
     142       17621 :         ftype t6 = t3*t3;
     143       17621 :         ftype t7 = t4*t4;
     144       17621 :         ftype t8 = t5+t6+t7;
     145       17621 :         ftype t9 = 1.0f/sqrtF(t8);
     146       17621 :         H_BCN[7] = -t4*t9;
     147       17621 :         H_BCN[8] = -t3*t9;
     148             :         // If we are not using the beacons as a height reference, we pretend that the beacons
     149             :         // are at the same height as the flight vehicle when calculating the observation derivatives
     150             :         // and Kalman gains
     151             :         // TODO  - less hacky way of achieving this, preferably using an alternative derivation
     152       17621 :         if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
     153             :             t2 = 0.0f;
     154             :         }
     155       17621 :         H_BCN[9] = -t2*t9;
     156             : 
     157             :         // calculate Kalman gains
     158       17621 :         ftype t10 = P[9][9]*t2*t9;
     159       17621 :         ftype t11 = P[8][9]*t3*t9;
     160       17621 :         ftype t12 = P[7][9]*t4*t9;
     161       17621 :         ftype t13 = t10+t11+t12;
     162       17621 :         ftype t14 = t2*t9*t13;
     163       17621 :         ftype t15 = P[9][8]*t2*t9;
     164       17621 :         ftype t16 = P[8][8]*t3*t9;
     165       17621 :         ftype t17 = P[7][8]*t4*t9;
     166       17621 :         ftype t18 = t15+t16+t17;
     167       17621 :         ftype t19 = t3*t9*t18;
     168       17621 :         ftype t20 = P[9][7]*t2*t9;
     169       17621 :         ftype t21 = P[8][7]*t3*t9;
     170       17621 :         ftype t22 = P[7][7]*t4*t9;
     171       17621 :         ftype t23 = t20+t21+t22;
     172       17621 :         ftype t24 = t4*t9*t23;
     173       17621 :         rngBcn.varInnov = R_BCN+t14+t19+t24;
     174             :         ftype t26;
     175       17621 :         if (rngBcn.varInnov >= R_BCN) {
     176       17621 :             t26 = 1.0f/rngBcn.varInnov;
     177       17621 :             faultStatus.bad_rngbcn = false;
     178             :         } else {
     179             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
     180             :             // we reset the covariance matrix and try again next measurement
     181           0 :             CovarianceInit();
     182           0 :             faultStatus.bad_rngbcn = true;
     183           0 :             return;
     184             :         }
     185             : 
     186       17621 :         Kfusion[0] = -t26*(P[0][7]*t4*t9+P[0][8]*t3*t9+P[0][9]*t2*t9);
     187       17621 :         Kfusion[1] = -t26*(P[1][7]*t4*t9+P[1][8]*t3*t9+P[1][9]*t2*t9);
     188       17621 :         Kfusion[2] = -t26*(P[2][7]*t4*t9+P[2][8]*t3*t9+P[2][9]*t2*t9);
     189       17621 :         Kfusion[3] = -t26*(P[3][7]*t4*t9+P[3][8]*t3*t9+P[3][9]*t2*t9);
     190       17621 :         Kfusion[4] = -t26*(P[4][7]*t4*t9+P[4][8]*t3*t9+P[4][9]*t2*t9);
     191       17621 :         Kfusion[5] = -t26*(P[5][7]*t4*t9+P[5][8]*t3*t9+P[5][9]*t2*t9);
     192       17621 :         Kfusion[7] = -t26*(t22+P[7][8]*t3*t9+P[7][9]*t2*t9);
     193       17621 :         Kfusion[8] = -t26*(t16+P[8][7]*t4*t9+P[8][9]*t2*t9);
     194             : 
     195             :         // values to calculate in Kfusion (others are set to zero, indices 0-9 ignored)
     196             :         uint32_t kalman_mask = 0;
     197             : 
     198       17621 :         if (!inhibitDelAngBiasStates) {
     199             :             kalman_mask |= (1<<10) | (1<<11) | (1<<12);
     200             :         }
     201             : 
     202       17621 :         if (!inhibitDelVelBiasStates && !badIMUdata) {
     203       70484 :             for (uint8_t index = 0; index < 3; index++) {
     204       52863 :                 const uint8_t stateIndex = index + 13;
     205       52863 :                 if (!dvelBiasAxisInhibit[index]) {
     206       45797 :                     kalman_mask |= (1<<stateIndex);
     207             :                 }
     208             :             }
     209             :         }
     210             : 
     211             :         // only allow the range observations to modify the vertical states if we are using it as a height reference
     212       17621 :         if (activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) {
     213           0 :             Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9);
     214           0 :             Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9);
     215             :         } else {
     216       17621 :             Kfusion[6] = 0.0f;
     217       17621 :             Kfusion[9] = 0.0f;
     218             :         }
     219             : 
     220       17621 :         if (!inhibitMagStates) {
     221       10089 :             kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
     222             :         }
     223             : 
     224       17621 :         if (!inhibitWindStates && !treatWindStatesAsTruth) {
     225           0 :             kalman_mask |= (1<<22) | (1<<23);
     226             :         }
     227             : 
     228      264315 :         for (auto i=10; i<24; i++) { // 0-9 are already computed
     229             :             ftype res = 0;
     230      246694 :             if (kalman_mask & (1<<i)) {
     231      159194 :                 res = -t26*(P[i][7]*t4*t9+P[i][8]*t3*t9+P[i][9]*t2*t9);
     232             :             }
     233      246694 :             Kfusion[i] = res;
     234             :         }
     235             : 
     236             :         // Calculate innovation using the selected offset value
     237       17621 :         Vector3F delta = stateStruct.position - rngBcn.dataDelayed.beacon_posNED;
     238       17621 :         rngBcn.innov = delta.length() - rngBcn.dataDelayed.rng;
     239             : 
     240             :         // calculate the innovation consistency test ratio
     241       17621 :         rngBcn.testRatio = sq(rngBcn.innov) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * rngBcn.varInnov);
     242             : 
     243             :         // fail if the ratio is > 1, but don't fail if bad IMU data
     244       17621 :         rngBcn.health = ((rngBcn.testRatio < 1.0f) || badIMUdata);
     245             : 
     246             :         // test the ratio before fusing data
     247       17621 :         if (rngBcn.health) {
     248             :             // restart the counter
     249       17621 :             rngBcn.lastPassTime_ms = imuSampleTime_ms;
     250             : 
     251             :             // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
     252             :             // the zero elements of H to reduce the number of operations.
     253      360091 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     254             :                 // j as the inner loop allows the compiler to hoist the KH product
     255             :                 // to save computation, and do the inner indexing more efficiently.
     256     7153738 :                 for (unsigned j = 0; j<=stateIndexLim; j++) {
     257             :                     ftype res = 0;
     258     6811268 :                     res += (Kfusion[i] * H_BCN[7]) * P[7][j];
     259     6811268 :                     res += (Kfusion[i] * H_BCN[8]) * P[8][j];
     260     6811268 :                     res += (Kfusion[i] * H_BCN[9]) * P[9][j];
     261     6811268 :                     KHP[i][j] = res;
     262             :                 }
     263             :             }
     264             : 
     265             :             // finish fusion from KHP and Kfusion then record health status
     266       17621 :             faultStatus.bad_rngbcn = FinishFusion(rngBcn.innov);
     267             :         }
     268             : 
     269             :         // Update the fusion report
     270       17621 :         if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
     271       17621 :             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
     272       17621 :             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
     273       17621 :             report.innov = rngBcn.innov;
     274       17621 :             report.innovVar = rngBcn.varInnov;
     275       17621 :             report.rng = rngBcn.dataDelayed.rng;
     276       17621 :             report.testRatio = rngBcn.testRatio;
     277             :         }
     278             :     }
     279             : }
     280             : 
     281             : /*
     282             : Use range beacon measurements to calculate a static position using a 3-state EKF algorithm.
     283             : Algorithm based on the following:
     284             : https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon.m
     285             : */
     286       30581 : void NavEKF3_core::FuseRngBcnStatic()
     287             : {
     288             :     // get the estimated range measurement variance
     289       30581 :     const ftype R_RNG = sq(MAX(rngBcn.dataDelayed.rngErr , 0.1f));
     290             : 
     291             :     /*
     292             :     The first thing to do is to check if we have started the alignment and if not, initialise the
     293             :     states and covariance to a first guess. To do this iterate through the available beacons and then
     294             :     initialise the initial position to the mean beacon position. The initial position uncertainty
     295             :     is set to the mean range measurement.
     296             :     */
     297       30581 :     if (!rngBcn.alignmentStarted) {
     298         802 :         if (rngBcn.dataDelayed.beacon_ID != rngBcn.lastIndex) {
     299         800 :             rngBcn.posSum += rngBcn.dataDelayed.beacon_posNED;
     300         800 :             rngBcn.lastIndex = rngBcn.dataDelayed.beacon_ID;
     301         800 :             rngBcn.sum += rngBcn.dataDelayed.rng;
     302         800 :             rngBcn.numMeas++;
     303             : 
     304             :             // capture the beacon vertical spread
     305         800 :             if (rngBcn.dataDelayed.beacon_posNED.z > rngBcn.maxPosD) {
     306           0 :                 rngBcn.maxPosD = rngBcn.dataDelayed.beacon_posNED.z;
     307         800 :             } else if(rngBcn.dataDelayed.beacon_posNED.z < rngBcn.minPosD) {
     308           0 :                 rngBcn.minPosD = rngBcn.dataDelayed.beacon_posNED.z;
     309             :             }
     310             :         }
     311         802 :         if (rngBcn.numMeas >= 100) {
     312           8 :             rngBcn.alignmentStarted = true;
     313           8 :             ftype tempVar = 1.0f / (ftype)rngBcn.numMeas;
     314             :             // initialise the receiver position to the centre of the beacons and at zero height
     315           8 :             rngBcn.receiverPos.x = rngBcn.posSum.x * tempVar;
     316           8 :             rngBcn.receiverPos.y = rngBcn.posSum.y * tempVar;
     317           8 :             rngBcn.receiverPos.z = 0.0f;
     318           8 :             rngBcn.receiverPosCov[2][2] = rngBcn.receiverPosCov[1][1] = rngBcn.receiverPosCov[0][0] = rngBcn.sum * tempVar;
     319           8 :             rngBcn.lastIndex  = 0;
     320           8 :             rngBcn.numMeas = 0;
     321           8 :             rngBcn.posSum.zero();
     322           8 :             rngBcn.sum = 0.0f;
     323             :         }
     324             :     }
     325             : 
     326       30581 :     if (rngBcn.alignmentStarted) {
     327       29787 :         rngBcn.numMeas++;
     328             : 
     329       29787 :         if (rngBcn.numMeas >= 100) {
     330             :             // 100 observations is enough for a stable estimate under most conditions
     331             :             // TODO monitor stability of the position estimate
     332       17938 :             rngBcn.alignmentCompleted = true;
     333             : 
     334             :         }
     335             : 
     336       29787 :         if (rngBcn.alignmentCompleted) {
     337       28995 :             if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
     338             :                 // We are using a different height reference for the main EKF so need to estimate a vertical
     339             :                 // position offset to be applied to the beacon system that minimises the range innovations
     340             :                 // The position estimate should be stable after 100 iterations so we use a simple dual
     341             :                 // hypothesis 1-state EKF to estimate the offset
     342             :                 Vector3F refPosNED;
     343       28995 :                 refPosNED.x = rngBcn.receiverPos.x;
     344       28995 :                 refPosNED.y = rngBcn.receiverPos.y;
     345       28995 :                 refPosNED.z = stateStruct.position.z;
     346       28995 :                 CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true);
     347             : 
     348             :             } else {
     349             :                 // we are using the beacons as the primary height source, so don't modify their vertical position
     350           0 :                 rngBcn.posOffsetNED.z = 0.0f;
     351             : 
     352             :             }
     353             :         } else {
     354         792 :             if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) {
     355             :                 // The position estimate is not yet stable so we cannot run the 1-state EKF to estimate
     356             :                 // beacon system vertical position offset. Instead we initialise the dual hypothesis offset states
     357             :                 // using the beacon vertical position, vertical position estimate relative to the beacon origin
     358             :                 // and the main EKF vertical position
     359             : 
     360             :                 // Calculate the mid vertical position of all beacons
     361         792 :                 ftype bcnMidPosD = 0.5f * (rngBcn.minPosD + rngBcn.maxPosD);
     362             : 
     363             :                 // calculate the delta to the estimated receiver position
     364         792 :                 ftype delta = rngBcn.receiverPos.z - bcnMidPosD;
     365             : 
     366             :                 // calculate the two hypothesis for our vertical position
     367             :                 ftype receiverPosDownMax;
     368             :                 ftype receiverPosDownMin;
     369         792 :                 if (delta >= 0.0f) {
     370             :                     receiverPosDownMax = rngBcn.receiverPos.z;
     371         421 :                     receiverPosDownMin = rngBcn.receiverPos.z - 2.0f * delta;
     372             :                 } else {
     373         371 :                     receiverPosDownMax = rngBcn.receiverPos.z - 2.0f * delta;
     374             :                     receiverPosDownMin = rngBcn.receiverPos.z;
     375             :                 }
     376             : 
     377         792 :                 rngBcn.posDownOffsetMax = stateStruct.position.z - receiverPosDownMin;
     378         792 :                 rngBcn.posDownOffsetMin = stateStruct.position.z - receiverPosDownMax;
     379             :             } else {
     380             :                 // We are using the beacons as the primary height reference, so don't modify their vertical position
     381           0 :                 rngBcn.posOffsetNED.z = 0.0f;
     382             :             }
     383             :         }
     384             : 
     385             :         // Add some process noise to the states at each time step
     386      119148 :         for (uint8_t i= 0; i<=2; i++) {
     387       89361 :             rngBcn.receiverPosCov[i][i] += 0.1f;
     388             :         }
     389             : 
     390             :         // calculate the observation jacobian
     391       29787 :         ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - rngBcn.receiverPos.z + rngBcn.posOffsetNED.z;
     392       29787 :         ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - rngBcn.receiverPos.y;
     393       29787 :         ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - rngBcn.receiverPos.x;
     394       29787 :         ftype t5 = t2*t2;
     395       29787 :         ftype t6 = t3*t3;
     396       29787 :         ftype t7 = t4*t4;
     397       29787 :         ftype t8 = t5+t6+t7;
     398       29787 :         if (t8 < 0.1f) {
     399             :             // calculation will be badly conditioned
     400           0 :             return;
     401             :         }
     402       29787 :         ftype t9 = 1.0f/sqrtF(t8);
     403       29787 :         ftype t10 = rngBcn.dataDelayed.beacon_posNED.x*2.0f;
     404       29787 :         ftype t15 = rngBcn.receiverPos.x*2.0f;
     405       29787 :         ftype t11 = t10-t15;
     406       29787 :         ftype t12 = rngBcn.dataDelayed.beacon_posNED.y*2.0f;
     407       29787 :         ftype t14 = rngBcn.receiverPos.y*2.0f;
     408       29787 :         ftype t13 = t12-t14;
     409       29787 :         ftype t16 = rngBcn.dataDelayed.beacon_posNED.z*2.0f;
     410       29787 :         ftype t18 = rngBcn.receiverPos.z*2.0f;
     411       29787 :         ftype t17 = t16-t18;
     412             :         ftype H_RNG[3];
     413       29787 :         H_RNG[0] = -t9*t11*0.5f;
     414       29787 :         H_RNG[1] = -t9*t13*0.5f;
     415       29787 :         H_RNG[2] = -t9*t17*0.5f;
     416             : 
     417             :         // calculate the Kalman gains
     418       29787 :         ftype t19 = rngBcn.receiverPosCov[0][0]*t9*t11*0.5f;
     419       29787 :         ftype t20 = rngBcn.receiverPosCov[1][1]*t9*t13*0.5f;
     420       29787 :         ftype t21 = rngBcn.receiverPosCov[0][1]*t9*t11*0.5f;
     421       29787 :         ftype t22 = rngBcn.receiverPosCov[2][1]*t9*t17*0.5f;
     422       29787 :         ftype t23 = t20+t21+t22;
     423       29787 :         ftype t24 = t9*t13*t23*0.5f;
     424       29787 :         ftype t25 = rngBcn.receiverPosCov[1][2]*t9*t13*0.5f;
     425       29787 :         ftype t26 = rngBcn.receiverPosCov[0][2]*t9*t11*0.5f;
     426       29787 :         ftype t27 = rngBcn.receiverPosCov[2][2]*t9*t17*0.5f;
     427       29787 :         ftype t28 = t25+t26+t27;
     428       29787 :         ftype t29 = t9*t17*t28*0.5f;
     429       29787 :         ftype t30 = rngBcn.receiverPosCov[1][0]*t9*t13*0.5f;
     430       29787 :         ftype t31 = rngBcn.receiverPosCov[2][0]*t9*t17*0.5f;
     431       29787 :         ftype t32 = t19+t30+t31;
     432       29787 :         ftype t33 = t9*t11*t32*0.5f;
     433       29787 :         rngBcn.varInnov = R_RNG+t24+t29+t33;
     434       29787 :         ftype t35 = 1.0f/rngBcn.varInnov;
     435             :         ftype K_RNG[3];
     436       29787 :         K_RNG[0] = -t35*(t19+rngBcn.receiverPosCov[0][1]*t9*t13*0.5f+rngBcn.receiverPosCov[0][2]*t9*t17*0.5f);
     437       29787 :         K_RNG[1] = -t35*(t20+rngBcn.receiverPosCov[1][0]*t9*t11*0.5f+rngBcn.receiverPosCov[1][2]*t9*t17*0.5f);
     438       29787 :         K_RNG[2] = -t35*(t27+rngBcn.receiverPosCov[2][0]*t9*t11*0.5f+rngBcn.receiverPosCov[2][1]*t9*t13*0.5f);
     439             : 
     440             :         // calculate range measurement innovation
     441       29787 :         Vector3F deltaPosNED = rngBcn.receiverPos - rngBcn.dataDelayed.beacon_posNED;
     442       29787 :         deltaPosNED.z -= rngBcn.posOffsetNED.z;
     443       29787 :         rngBcn.innov = deltaPosNED.length() - rngBcn.dataDelayed.rng;
     444             : 
     445             :         // calculate the innovation consistency test ratio
     446       29787 :         rngBcn.testRatio = sq(rngBcn.innov) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * rngBcn.varInnov);
     447             : 
     448             :         // fail if the ratio is > 1, but don't fail if bad IMU data
     449       29787 :         rngBcn.health = ((rngBcn.testRatio < 1.0f) || badIMUdata || !rngBcn.alignmentCompleted);
     450             : 
     451             :         // test the ratio before fusing data
     452       29787 :         if (rngBcn.health) {
     453             : 
     454             :             // update the state
     455       15293 :             rngBcn.receiverPos.x -= K_RNG[0] * rngBcn.innov;
     456       15293 :             rngBcn.receiverPos.y -= K_RNG[1] * rngBcn.innov;
     457       15293 :             rngBcn.receiverPos.z -= K_RNG[2] * rngBcn.innov;
     458             : 
     459             :             // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
     460             :             // the zero elements of H to reduce the number of operations.
     461       61172 :             for (unsigned i = 0; i<=2; i++) {
     462             :                 // j as the inner loop allows the compiler to hoist the KH product
     463             :                 // to save computation, and do the inner indexing more efficiently.
     464      183516 :                 for (unsigned j = 0; j<=2; j++) {
     465             :                     ftype res = 0;
     466      137637 :                     res += (K_RNG[i] * H_RNG[0]) * rngBcn.receiverPosCov[0][j];
     467      137637 :                     res += (K_RNG[i] * H_RNG[1]) * rngBcn.receiverPosCov[1][j];
     468      137637 :                     res += (K_RNG[i] * H_RNG[2]) * rngBcn.receiverPosCov[2][j];
     469      137637 :                     KHP[i][j] = res;
     470             :                 }
     471             :             }
     472             : 
     473             :             // prevent negative variances
     474       61172 :             for (uint8_t i= 0; i<=2; i++) {
     475       45879 :                 if (rngBcn.receiverPosCov[i][i] < 0.0f) {
     476           0 :                     rngBcn.receiverPosCov[i][i] = 0.0f;
     477           0 :                     KHP[i][i] = 0.0f;
     478       45879 :                 } else if (KHP[i][i] > rngBcn.receiverPosCov[i][i]) {
     479           0 :                     KHP[i][i] = rngBcn.receiverPosCov[i][i];
     480             :                 }
     481             :             }
     482             : 
     483             :             // apply the covariance correction
     484       61172 :             for (uint8_t i= 0; i<=2; i++) {
     485      183516 :                 for (uint8_t j= 0; j<=2; j++) {
     486      137637 :                     rngBcn.receiverPosCov[i][j] -= KHP[i][j];
     487             :                 }
     488             :             }
     489             : 
     490             :             // ensure the covariance matrix is symmetric
     491       45879 :             for (uint8_t i=1; i<=2; i++) {
     492       76465 :                 for (uint8_t j=0; j<=i-1; j++) {
     493       45879 :                     ftype temp = 0.5f*(rngBcn.receiverPosCov[i][j] + rngBcn.receiverPosCov[j][i]);
     494       45879 :                     rngBcn.receiverPosCov[i][j] = temp;
     495       45879 :                     rngBcn.receiverPosCov[j][i] = temp;
     496             :                 }
     497             :             }
     498             : 
     499             :         }
     500             : 
     501       29787 :         if (rngBcn.numMeas >= 100) {
     502             :             // 100 observations is enough for a stable estimate under most conditions
     503             :             // TODO monitor stability of the position estimate
     504       17938 :             rngBcn.alignmentCompleted = true;
     505             :         }
     506             :         // Update the fusion report
     507       29787 :         if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
     508       29787 :             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
     509       29787 :             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
     510       29787 :             report.innov = rngBcn.innov;
     511       29787 :             report.innovVar = rngBcn.varInnov;
     512       29787 :             report.rng = rngBcn.dataDelayed.rng;
     513       29787 :             report.testRatio = rngBcn.testRatio;
     514             :         }
     515             :     }
     516             : }
     517             : 
     518             : /*
     519             : Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
     520             : Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
     521             : */
     522       46616 : void NavEKF3_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning)
     523             : {
     524             :     // Handle height offsets between the primary height source and the range beacons by estimating
     525             :     // the beacon systems global vertical position offset using a single state Kalman filter
     526             :     // The estimated offset is used to correct the beacon height when calculating innovations
     527             :     // A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
     528             :     // The main filter then uses the offset with the smaller innovations
     529             : 
     530             :     ftype innov;    // range measurement innovation (m)
     531             :     ftype innovVar; // range measurement innovation variance (m^2)
     532             :     ftype gain;     // Kalman gain
     533             :     ftype obsDeriv; // derivative of observation relative to state
     534             : 
     535             :     const ftype stateNoiseVar = 0.1f; // State process noise variance
     536             :     const ftype filtAlpha = 0.1f; // LPF constant
     537             :     const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
     538             : 
     539             :     // estimate upper value for offset
     540             : 
     541             :     // calculate observation derivative
     542       46616 :     ftype t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMax;
     543       46616 :     ftype t3 = rngBcn.dataDelayed.beacon_posNED.y - vehiclePosNED.y;
     544       46616 :     ftype t4 = rngBcn.dataDelayed.beacon_posNED.x - vehiclePosNED.x;
     545       46616 :     ftype t5 = t2*t2;
     546       46616 :     ftype t6 = t3*t3;
     547       46616 :     ftype t7 = t4*t4;
     548       46616 :     ftype t8 = t5+t6+t7;
     549             :     ftype t9;
     550       46616 :     if (t8 > 0.1f) {
     551       46616 :         t9 = 1.0f/sqrtF(t8);
     552       46616 :         obsDeriv = t2*t9;
     553             : 
     554             :         // Calculate innovation
     555       46616 :         innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
     556             : 
     557             :         // covariance prediction
     558       46616 :         rngBcn.posOffsetMaxVar += stateNoiseVar;
     559             : 
     560             :         // calculate the innovation variance
     561       46616 :         innovVar = obsDeriv * rngBcn.posOffsetMaxVar * obsDeriv + obsVar;
     562       46616 :         innovVar = MAX(innovVar, obsVar);
     563             : 
     564             :         // calculate the Kalman gain
     565       46616 :         gain = (rngBcn.posOffsetMaxVar * obsDeriv) / innovVar;
     566             : 
     567             :         // calculate a filtered state change magnitude to be used to select between the high or low offset
     568       46616 :         ftype stateChange = innov * gain;
     569       46616 :         rngBcn.maxOffsetStateChangeFilt = (1.0f - filtAlpha) * rngBcn.maxOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
     570             : 
     571             :         // Reject range innovation spikes using a 5-sigma threshold unless aligning
     572       46616 :         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
     573             : 
     574             :             // state update
     575       46616 :             rngBcn.posDownOffsetMax -= stateChange;
     576             : 
     577             :             // covariance update
     578       46616 :             rngBcn.posOffsetMaxVar -= gain * obsDeriv * rngBcn.posOffsetMaxVar;
     579       46616 :             rngBcn.posOffsetMaxVar = MAX(rngBcn.posOffsetMaxVar, 0.0f);
     580             :         }
     581             :     }
     582             : 
     583             :     // estimate lower value for offset
     584             : 
     585             :     // calculate observation derivative
     586       46616 :     t2 = rngBcn.dataDelayed.beacon_posNED.z - vehiclePosNED.z + rngBcn.posDownOffsetMin;
     587       46616 :     t5 = t2*t2;
     588       46616 :     t8 = t5+t6+t7;
     589       46616 :     if (t8 > 0.1f) {
     590       46616 :         t9 = 1.0f/sqrtF(t8);
     591       46616 :         obsDeriv = t2*t9;
     592             : 
     593             :         // Calculate innovation
     594       46616 :         innov = sqrtF(t8) - rngBcn.dataDelayed.rng;
     595             : 
     596             :         // covariance prediction
     597       46616 :         rngBcn.posOffsetMinVar += stateNoiseVar;
     598             : 
     599             :         // calculate the innovation variance
     600       46616 :         innovVar = obsDeriv * rngBcn.posOffsetMinVar * obsDeriv + obsVar;
     601       46616 :         innovVar = MAX(innovVar, obsVar);
     602             : 
     603             :         // calculate the Kalman gain
     604       46616 :         gain = (rngBcn.posOffsetMinVar * obsDeriv) / innovVar;
     605             : 
     606             :         // calculate a filtered state change magnitude to be used to select between the high or low offset
     607       46616 :         ftype stateChange = innov * gain;
     608       46616 :         rngBcn.minOffsetStateChangeFilt = (1.0f - filtAlpha) * rngBcn.minOffsetStateChangeFilt + fminF(fabsF(filtAlpha * stateChange) , 1.0f);
     609             : 
     610             :         // Reject range innovation spikes using a 5-sigma threshold unless aligning
     611       46616 :         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
     612             : 
     613             :             // state update
     614       46616 :             rngBcn.posDownOffsetMin -= stateChange;
     615             : 
     616             :             // covariance update
     617       46616 :             rngBcn.posOffsetMinVar -= gain * obsDeriv * rngBcn.posOffsetMinVar;
     618       46616 :             rngBcn.posOffsetMinVar = MAX(rngBcn.posOffsetMinVar, 0.0f);
     619             :         }
     620             :     }
     621             : 
     622             :     // calculate the mid vertical position of all beacons
     623       46616 :     ftype bcnMidPosD = 0.5f * (rngBcn.minPosD + rngBcn.maxPosD);
     624             : 
     625             :     // ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
     626       46616 :     rngBcn.posDownOffsetMax = MAX(rngBcn.posDownOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
     627       46616 :     rngBcn.posDownOffsetMin  = MIN(rngBcn.posDownOffsetMin,  vehiclePosNED.z - bcnMidPosD - 0.5f);
     628             : 
     629             :     // calculate the innovation for the main filter using the offset that is most stable
     630             :     // apply hysteresis to prevent rapid switching
     631       46616 :     if (!rngBcn.usingMinHypothesis && (rngBcn.minOffsetStateChangeFilt < (0.8f * rngBcn.maxOffsetStateChangeFilt))) {
     632          15 :         rngBcn.usingMinHypothesis = true;
     633       46601 :     } else if (rngBcn.usingMinHypothesis && (rngBcn.maxOffsetStateChangeFilt < (0.8f * rngBcn.minOffsetStateChangeFilt))) {
     634          13 :         rngBcn.usingMinHypothesis = false;
     635             :     }
     636       46616 :     if (rngBcn.usingMinHypothesis) {
     637        4894 :         rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMin;
     638             :     } else {
     639       41722 :         rngBcn.posOffsetNED.z = rngBcn.posDownOffsetMax;
     640             :     }
     641             : 
     642             :     // apply the vertical offset to the beacon positions
     643       46616 :     rngBcn.dataDelayed.beacon_posNED.z += rngBcn.posOffsetNED.z;
     644       46616 : }
     645             : 
     646             : #endif  // EK3_FEATURE_BEACON_FUSION