Line data    Source code

       1             : #include "AP_NavEKF2.h"
       2             : #include "AP_NavEKF2_core.h"
       3             : 
       4             : /********************************************************
       5             : *                   FUSE MEASURED_DATA                  *
       6             : ********************************************************/
       7             : 
       8             : #if AP_BEACON_ENABLED
       9             : // select fusion of range beacon measurements
      10     3248540 : void NavEKF2_core::SelectRngBcnFusion()
      11             : {
      12             :     // read range data from the sensor and check for new data in the buffer
      13     3248540 :     readRngBcnData();
      14             : 
      15             :     // Determine if we need to fuse range beacon data on this time step
      16     3248540 :     if (rngBcnDataToFuse) {
      17           0 :         if (PV_AidingMode == AID_ABSOLUTE) {
      18             :             // Normal operating mode is to fuse the range data into the main filter
      19           0 :             FuseRngBcn();
      20             :         } else {
      21             :             // If we aren't able to use the data in the main filter, use a simple 3-state filter to estimte position only
      22           0 :             FuseRngBcnStatic();
      23             :         }
      24             :     }
      25     3248540 : }
      26             : 
      27           0 : void NavEKF2_core::FuseRngBcn()
      28             : {
      29             :     // declarations
      30             :     ftype pn;
      31             :     ftype pe;
      32             :     ftype pd;
      33             :     ftype bcn_pn;
      34             :     ftype bcn_pe;
      35             :     ftype bcn_pd;
      36           0 :     const ftype R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
      37             :     ftype rngPred;
      38             : 
      39             :     // health is set bad until test passed
      40           0 :     rngBcnHealth = false;
      41             : 
      42           0 :     if (activeHgtSource != HGT_SOURCE_BCN) {
      43             :         // calculate the vertical offset from EKF datum to beacon datum
      44           0 :         CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false);
      45             :     } else {
      46           0 :         bcnPosOffset = 0.0f;
      47             :     }
      48             : 
      49             :     // copy required states to local variable names
      50           0 :     pn = stateStruct.position.x;
      51           0 :     pe = stateStruct.position.y;
      52           0 :     pd = stateStruct.position.z;
      53           0 :     bcn_pn = rngBcnDataDelayed.beacon_posNED.x;
      54           0 :     bcn_pe = rngBcnDataDelayed.beacon_posNED.y;
      55           0 :     bcn_pd = rngBcnDataDelayed.beacon_posNED.z + bcnPosOffset;
      56             : 
      57             :     // predicted range
      58           0 :     Vector3F deltaPosNED = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
      59           0 :     rngPred = deltaPosNED.length();
      60             : 
      61             :     // calculate measurement innovation
      62           0 :     innovRngBcn = rngPred - rngBcnDataDelayed.rng;
      63             : 
      64             :     // perform fusion of range measurement
      65           0 :     if (rngPred > 0.1f)
      66             :     {
      67             :         // calculate observation jacobians
      68           0 :         ftype H_BCN[24] = {};
      69           0 :         ftype t2 = bcn_pd-pd;
      70           0 :         ftype t3 = bcn_pe-pe;
      71           0 :         ftype t4 = bcn_pn-pn;
      72           0 :         ftype t5 = t2*t2;
      73           0 :         ftype t6 = t3*t3;
      74           0 :         ftype t7 = t4*t4;
      75           0 :         ftype t8 = t5+t6+t7;
      76           0 :         ftype t9 = 1.0f/sqrtF(t8);
      77           0 :         H_BCN[6] = -t4*t9;
      78           0 :         H_BCN[7] = -t3*t9;
      79           0 :         H_BCN[8] = -t2*t9;
      80             : 
      81             :         // calculate Kalman gains
      82           0 :         ftype t10 = P[8][8]*t2*t9;
      83           0 :         ftype t11 = P[7][8]*t3*t9;
      84           0 :         ftype t12 = P[6][8]*t4*t9;
      85           0 :         ftype t13 = t10+t11+t12;
      86           0 :         ftype t14 = t2*t9*t13;
      87           0 :         ftype t15 = P[8][7]*t2*t9;
      88           0 :         ftype t16 = P[7][7]*t3*t9;
      89           0 :         ftype t17 = P[6][7]*t4*t9;
      90           0 :         ftype t18 = t15+t16+t17;
      91           0 :         ftype t19 = t3*t9*t18;
      92           0 :         ftype t20 = P[8][6]*t2*t9;
      93           0 :         ftype t21 = P[7][6]*t3*t9;
      94           0 :         ftype t22 = P[6][6]*t4*t9;
      95           0 :         ftype t23 = t20+t21+t22;
      96           0 :         ftype t24 = t4*t9*t23;
      97           0 :         varInnovRngBcn = R_BCN+t14+t19+t24;
      98             :         ftype t26;
      99           0 :         if (varInnovRngBcn >= R_BCN) {
     100           0 :             t26 = 1.0/varInnovRngBcn;
     101           0 :             faultStatus.bad_rngbcn = false;
     102             :         } else {
     103             :             // the calculation is badly conditioned, so we cannot perform fusion on this step
     104             :             // we reset the covariance matrix and try again next measurement
     105           0 :             CovarianceInit();
     106           0 :             faultStatus.bad_rngbcn = true;
     107           0 :             return;
     108             :         }
     109             : 
     110           0 :         Kfusion[0] = -t26*(P[0][6]*t4*t9+P[0][7]*t3*t9+P[0][8]*t2*t9);
     111           0 :         Kfusion[1] = -t26*(P[1][6]*t4*t9+P[1][7]*t3*t9+P[1][8]*t2*t9);
     112           0 :         Kfusion[2] = -t26*(P[2][6]*t4*t9+P[2][7]*t3*t9+P[2][8]*t2*t9);
     113           0 :         Kfusion[3] = -t26*(P[3][6]*t4*t9+P[3][7]*t3*t9+P[3][8]*t2*t9);
     114           0 :         Kfusion[4] = -t26*(P[4][6]*t4*t9+P[4][7]*t3*t9+P[4][8]*t2*t9);
     115           0 :         Kfusion[6] = -t26*(t22+P[6][7]*t3*t9+P[6][8]*t2*t9);
     116           0 :         Kfusion[7] = -t26*(t16+P[7][6]*t4*t9+P[7][8]*t2*t9);
     117           0 :         if (activeHgtSource == HGT_SOURCE_BCN) {
     118             :             // We are using the range beacon as the primary height reference, so allow it to modify the EKF's vertical states
     119           0 :             Kfusion[5] = -t26*(P[5][6]*t4*t9+P[5][7]*t3*t9+P[5][8]*t2*t9);
     120           0 :             Kfusion[8] = -t26*(t10+P[8][6]*t4*t9+P[8][7]*t3*t9);
     121           0 :             Kfusion[15] = -t26*(P[15][6]*t4*t9+P[15][7]*t3*t9+P[15][8]*t2*t9);
     122           0 :             bcnPosOffset = 0.0f;
     123             : 
     124             :         } else {
     125             :             // don't allow the range measurement to affect the vertical states in the main filter
     126           0 :             Kfusion[5] = 0.0f;
     127           0 :             Kfusion[8] = 0.0f;
     128           0 :             Kfusion[15] = 0.0f;
     129             : 
     130             :         }
     131           0 :         Kfusion[9] = -t26*(P[9][6]*t4*t9+P[9][7]*t3*t9+P[9][8]*t2*t9);
     132           0 :         Kfusion[10] = -t26*(P[10][6]*t4*t9+P[10][7]*t3*t9+P[10][8]*t2*t9);
     133           0 :         Kfusion[11] = -t26*(P[11][6]*t4*t9+P[11][7]*t3*t9+P[11][8]*t2*t9);
     134           0 :         Kfusion[12] = -t26*(P[12][6]*t4*t9+P[12][7]*t3*t9+P[12][8]*t2*t9);
     135           0 :         Kfusion[13] = -t26*(P[13][6]*t4*t9+P[13][7]*t3*t9+P[13][8]*t2*t9);
     136           0 :         Kfusion[14] = -t26*(P[14][6]*t4*t9+P[14][7]*t3*t9+P[14][8]*t2*t9);
     137           0 :         if (!inhibitMagStates) {
     138           0 :             Kfusion[16] = -t26*(P[16][6]*t4*t9+P[16][7]*t3*t9+P[16][8]*t2*t9);
     139           0 :             Kfusion[17] = -t26*(P[17][6]*t4*t9+P[17][7]*t3*t9+P[17][8]*t2*t9);
     140           0 :             Kfusion[18] = -t26*(P[18][6]*t4*t9+P[18][7]*t3*t9+P[18][8]*t2*t9);
     141           0 :             Kfusion[19] = -t26*(P[19][6]*t4*t9+P[19][7]*t3*t9+P[19][8]*t2*t9);
     142           0 :             Kfusion[20] = -t26*(P[20][6]*t4*t9+P[20][7]*t3*t9+P[20][8]*t2*t9);
     143           0 :             Kfusion[21] = -t26*(P[21][6]*t4*t9+P[21][7]*t3*t9+P[21][8]*t2*t9);
     144             :         } else {
     145             :             // zero indexes 16 to 21 = 6
     146           0 :             zero_range(&Kfusion[0], 16, 21);
     147             :         }
     148           0 :         Kfusion[22] = -t26*(P[22][6]*t4*t9+P[22][7]*t3*t9+P[22][8]*t2*t9);
     149           0 :         Kfusion[23] = -t26*(P[23][6]*t4*t9+P[23][7]*t3*t9+P[23][8]*t2*t9);
     150             : 
     151             :         // Calculate innovation using the selected offset value
     152           0 :         Vector3F delta = stateStruct.position - rngBcnDataDelayed.beacon_posNED;
     153           0 :         innovRngBcn = delta.length() - rngBcnDataDelayed.rng;
     154             : 
     155             :         // calculate the innovation consistency test ratio
     156           0 :         rngBcnTestRatio = sq(innovRngBcn) / (sq(MAX(0.01f * (ftype)frontend->_rngBcnInnovGate, 1.0f)) * varInnovRngBcn);
     157             : 
     158             :         // fail if the ratio is > 1, but don't fail if bad IMU data
     159           0 :         rngBcnHealth = ((rngBcnTestRatio < 1.0f) || badIMUdata);
     160             : 
     161             :         // test the ratio before fusing data
     162           0 :         if (rngBcnHealth) {
     163             : 
     164             :             // restart the counter
     165           0 :             lastRngBcnPassTime_ms = imuSampleTime_ms;
     166             : 
     167             :             // correct the covariance P = (I - K*H)*P
     168             :             // take advantage of the empty columns in KH to reduce the
     169             :             // number of operations
     170           0 :             for (unsigned i = 0; i<=stateIndexLim; i++) {
     171           0 :                 for (unsigned j = 0; j<=5; j++) {
     172           0 :                     KH[i][j] = 0.0f;
     173             :                 }
     174           0 :                 for (unsigned j = 6; j<=8; j++) {
     175           0 :                     KH[i][j] = Kfusion[i] * H_BCN[j];
     176             :                 }
     177           0 :                 for (unsigned j = 9; j<=23; j++) {
     178           0 :                     KH[i][j] = 0.0f;
     179             :                 }
     180             :             }
     181           0 :             for (unsigned j = 0; j<=stateIndexLim; j++) {
     182           0 :                 for (unsigned i = 0; i<=stateIndexLim; i++) {
     183             :                     ftype res = 0;
     184           0 :                     res += KH[i][6] * P[6][j];
     185           0 :                     res += KH[i][7] * P[7][j];
     186           0 :                     res += KH[i][8] * P[8][j];
     187           0 :                     KHP[i][j] = res;
     188             :                 }
     189             :             }
     190             :             // Check that we are not going to drive any variances negative and skip the update if so
     191             :             bool healthyFusion = true;
     192           0 :             for (uint8_t i= 0; i<=stateIndexLim; i++) {
     193           0 :                 if (KHP[i][i] > P[i][i]) {
     194             :                     healthyFusion = false;
     195             :                 }
     196             :             }
     197           0 :             if (healthyFusion) {
     198             :                 // update the covariance matrix
     199           0 :                 for (uint8_t i= 0; i<=stateIndexLim; i++) {
     200           0 :                     for (uint8_t j= 0; j<=stateIndexLim; j++) {
     201           0 :                         P[i][j] = P[i][j] - KHP[i][j];
     202             :                     }
     203             :                 }
     204             : 
     205             :                 // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
     206           0 :                 ForceSymmetry();
     207           0 :                 ConstrainVariances();
     208             : 
     209             :                 // update the states
     210             :                 // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
     211           0 :                 stateStruct.angErr.zero();
     212             : 
     213             :                 // correct the state vector
     214           0 :                 for (uint8_t j= 0; j<=stateIndexLim; j++) {
     215           0 :                     statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn;
     216             :                 }
     217             : 
     218             :                 // the first 3 states represent the angular misalignment vector.
     219             :                 // This is used to correct the estimated quaternion on the current time step
     220           0 :                 stateStruct.quat.rotate(stateStruct.angErr);
     221             : 
     222             :                 // record healthy fusion
     223           0 :                 faultStatus.bad_rngbcn = false;
     224             : 
     225             :             } else {
     226             :                 // record bad fusion
     227           0 :                 faultStatus.bad_rngbcn = true;
     228             : 
     229             :             }
     230             :         }
     231             : 
     232             :         // Update the fusion report
     233           0 :         rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].beaconPosNED = rngBcnDataDelayed.beacon_posNED;
     234           0 :         rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innov = innovRngBcn;
     235           0 :         rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].innovVar = varInnovRngBcn;
     236           0 :         rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].rng = rngBcnDataDelayed.rng;
     237           0 :         rngBcnFusionReport[rngBcnDataDelayed.beacon_ID].testRatio = rngBcnTestRatio;
     238             :     }
     239             : }
     240             : 
     241             : /*
     242             : Use range beacon measurements to calculate a static position using a 3-state EKF algorithm.
     243             : Algorithm based on the following:
     244             : https://github.com/priseborough/InertialNav/blob/master/derivations/range_beacon.m
     245             : */
     246           0 : void NavEKF2_core::FuseRngBcnStatic()
     247             : {
     248             :     // get the estimated range measurement variance
     249           0 :     const ftype R_RNG = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
     250             : 
     251             :     /*
     252             :     The first thing to do is to check if we have started the alignment and if not, initialise the
     253             :     states and covariance to a first guess. To do this iterate through the available beacons and then
     254             :     initialise the initial position to the mean beacon position. The initial position uncertainty
     255             :     is set to the mean range measurement.
     256             :     */
     257           0 :     if (!rngBcnAlignmentStarted) {
     258           0 :         if (rngBcnDataDelayed.beacon_ID != lastBeaconIndex) {
     259           0 :             rngBcnPosSum += rngBcnDataDelayed.beacon_posNED;
     260           0 :             lastBeaconIndex = rngBcnDataDelayed.beacon_ID;
     261           0 :             rngSum += rngBcnDataDelayed.rng;
     262           0 :             numBcnMeas++;
     263             : 
     264             :             // capture the beacon vertical spread
     265           0 :             if (rngBcnDataDelayed.beacon_posNED.z > maxBcnPosD) {
     266           0 :                 maxBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
     267           0 :             } else if(rngBcnDataDelayed.beacon_posNED.z < minBcnPosD) {
     268           0 :                 minBcnPosD = rngBcnDataDelayed.beacon_posNED.z;
     269             :             }
     270             :         }
     271           0 :         if (numBcnMeas >= 100) {
     272           0 :             rngBcnAlignmentStarted = true;
     273           0 :             ftype tempVar = 1.0f / (float)numBcnMeas;
     274             :             // initialise the receiver position to the centre of the beacons and at zero height
     275           0 :             receiverPos.x = rngBcnPosSum.x * tempVar;
     276           0 :             receiverPos.y = rngBcnPosSum.y * tempVar;
     277           0 :             receiverPos.z = 0.0f;
     278           0 :             receiverPosCov[2][2] = receiverPosCov[1][1] = receiverPosCov[0][0] = rngSum * tempVar;
     279           0 :             lastBeaconIndex  = 0;
     280           0 :             numBcnMeas = 0;
     281           0 :             rngBcnPosSum.zero();
     282           0 :             rngSum = 0.0f;
     283             :         }
     284             :     }
     285             : 
     286           0 :     if (rngBcnAlignmentStarted) {
     287           0 :         numBcnMeas++;
     288             : 
     289           0 :         if (numBcnMeas >= 100) {
     290             :             // 100 observations is enough for a stable estimate under most conditions
     291             :             // TODO monitor stability of the position estimate
     292           0 :             rngBcnAlignmentCompleted = true;
     293             : 
     294             :         }
     295             : 
     296           0 :         if (rngBcnAlignmentCompleted) {
     297           0 :             if (activeHgtSource != HGT_SOURCE_BCN) {
     298             :                 // We are using a different height reference for the main EKF so need to estimate a vertical
     299             :                 // position offset to be applied to the beacon system that minimises the range innovations
     300             :                 // The position estimate should be stable after 100 iterations so we use a simple dual
     301             :                 // hypothesis 1-state EKF to estimate the offset
     302             :                 Vector3F refPosNED;
     303           0 :                 refPosNED.x = receiverPos.x;
     304           0 :                 refPosNED.y = receiverPos.y;
     305           0 :                 refPosNED.z = stateStruct.position.z;
     306           0 :                 CalcRangeBeaconPosDownOffset(R_RNG, refPosNED, true);
     307             : 
     308             :             } else {
     309             :                 // we are using the beacons as the primary height source, so don't modify their vertical position
     310           0 :                 bcnPosOffset = 0.0f;
     311             : 
     312             :             }
     313             :         } else {
     314           0 :             if (activeHgtSource != HGT_SOURCE_BCN) {
     315             :                 // The position estimate is not yet stable so we cannot run the 1-state EKF to estimate
     316             :                 // beacon system vertical position offset. Instead we initialise the dual hypothesis offset states
     317             :                 // using the beacon vertical position, vertical position estimate relative to the beacon origin
     318             :                 // and the main EKF vertical position
     319             : 
     320             :                 // Calculate the mid vertical position of all beacons
     321           0 :                 ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
     322             : 
     323             :                 // calculate the delta to the estimated receiver position
     324           0 :                 ftype delta = receiverPos.z - bcnMidPosD;
     325             : 
     326             :                 // calculate the two hypothesis for our vertical position
     327             :                 ftype receiverPosDownMax;
     328             :                 ftype receiverPosDownMin;
     329           0 :                 if (delta >= 0.0f) {
     330             :                     receiverPosDownMax = receiverPos.z;
     331           0 :                     receiverPosDownMin = receiverPos.z - 2.0f * delta;
     332             :                 } else {
     333           0 :                     receiverPosDownMax = receiverPos.z - 2.0f * delta;
     334             :                     receiverPosDownMin = receiverPos.z;
     335             :                 }
     336             : 
     337           0 :                 bcnPosOffsetMax = stateStruct.position.z - receiverPosDownMin;
     338           0 :                 bcnPosOffsetMin = stateStruct.position.z - receiverPosDownMax;
     339             :             } else {
     340             :                 // We are using the beacons as the primary height reference, so don't modify their vertical position
     341           0 :                 bcnPosOffset = 0.0f;
     342             :             }
     343             :         }
     344             : 
     345             :         // Add some process noise to the states at each time step
     346           0 :         for (uint8_t i= 0; i<=2; i++) {
     347           0 :             receiverPosCov[i][i] += 0.1f;
     348             :         }
     349             : 
     350             :         // calculate the observation jacobian
     351           0 :         ftype t2 = rngBcnDataDelayed.beacon_posNED.z - receiverPos.z + bcnPosOffset;
     352           0 :         ftype t3 = rngBcnDataDelayed.beacon_posNED.y - receiverPos.y;
     353           0 :         ftype t4 = rngBcnDataDelayed.beacon_posNED.x - receiverPos.x;
     354           0 :         ftype t5 = t2*t2;
     355           0 :         ftype t6 = t3*t3;
     356           0 :         ftype t7 = t4*t4;
     357           0 :         ftype t8 = t5+t6+t7;
     358           0 :         if (t8 < 0.1f) {
     359             :             // calculation will be badly conditioned
     360           0 :             return;
     361             :         }
     362           0 :         ftype t9 = 1.0f/sqrtF(t8);
     363           0 :         ftype t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f;
     364           0 :         ftype t15 = receiverPos.x*2.0f;
     365           0 :         ftype t11 = t10-t15;
     366           0 :         ftype t12 = rngBcnDataDelayed.beacon_posNED.y*2.0f;
     367           0 :         ftype t14 = receiverPos.y*2.0f;
     368           0 :         ftype t13 = t12-t14;
     369           0 :         ftype t16 = rngBcnDataDelayed.beacon_posNED.z*2.0f;
     370           0 :         ftype t18 = receiverPos.z*2.0f;
     371           0 :         ftype t17 = t16-t18;
     372             :         ftype H_RNG[3];
     373           0 :         H_RNG[0] = -t9*t11*0.5f;
     374           0 :         H_RNG[1] = -t9*t13*0.5f;
     375           0 :         H_RNG[2] = -t9*t17*0.5f;
     376             : 
     377             :         // calculate the Kalman gains
     378           0 :         ftype t19 = receiverPosCov[0][0]*t9*t11*0.5f;
     379           0 :         ftype t20 = receiverPosCov[1][1]*t9*t13*0.5f;
     380           0 :         ftype t21 = receiverPosCov[0][1]*t9*t11*0.5f;
     381           0 :         ftype t22 = receiverPosCov[2][1]*t9*t17*0.5f;
     382           0 :         ftype t23 = t20+t21+t22;
     383           0 :         ftype t24 = t9*t13*t23*0.5f;
     384           0 :         ftype t25 = receiverPosCov[1][2]*t9*t13*0.5f;
     385           0 :         ftype t26 = receiverPosCov[0][2]*t9*t11*0.5f;
     386           0 :         ftype t27 = receiverPosCov[2][2]*t9*t17*0.5f;
     387           0 :         ftype t28 = t25+t26+t27;
     388           0 :         ftype t29 = t9*t17*t28*0.5f;
     389           0 :         ftype t30 = receiverPosCov[1][0]*t9*t13*0.5f;
     390           0 :         ftype t31 = receiverPosCov[2][0]*t9*t17*0.5f;
     391           0 :         ftype t32 = t19+t30+t31;
     392           0 :         ftype t33 = t9*t11*t32*0.5f;
     393           0 :         varInnovRngBcn = R_RNG+t24+t29+t33;
     394           0 :         ftype t35 = 1.0f/varInnovRngBcn;
     395             :         ftype K_RNG[3];
     396           0 :         K_RNG[0] = -t35*(t19+receiverPosCov[0][1]*t9*t13*0.5f+receiverPosCov[0][2]*t9*t17*0.5f);
     397           0 :         K_RNG[1] = -t35*(t20+receiverPosCov[1][0]*t9*t11*0.5f+receiverPosCov[1][2]*t9*t17*0.5f);
     398           0 :         K_RNG[2] = -t35*(t27+receiverPosCov[2][0]*t9*t11*0.5f+receiverPosCov[2][1]*t9*t13*0.5f);
     399             : 
     400             :         // calculate range measurement innovation
     401           0 :         Vector3F deltaPosNED = receiverPos - rngBcnDataDelayed.beacon_posNED;
     402           0 :         deltaPosNED.z -= bcnPosOffset;
     403           0 :         innovRngBcn = deltaPosNED.length() - rngBcnDataDelayed.rng;
     404             : 
     405             :         // update the state
     406           0 :         receiverPos.x -= K_RNG[0] * innovRngBcn;
     407           0 :         receiverPos.y -= K_RNG[1] * innovRngBcn;
     408           0 :         receiverPos.z -= K_RNG[2] * innovRngBcn;
     409           0 :         receiverPos.z = MAX(receiverPos.z, minBcnPosD + 1.2f);
     410             : 
     411             :         // calculate the covariance correction
     412           0 :         for (unsigned i = 0; i<=2; i++) {
     413           0 :             for (unsigned j = 0; j<=2; j++) {
     414           0 :                 KH[i][j] = K_RNG[i] * H_RNG[j];
     415             :             }
     416             :         }
     417           0 :         for (unsigned j = 0; j<=2; j++) {
     418           0 :             for (unsigned i = 0; i<=2; i++) {
     419             :                 ftype res = 0;
     420           0 :                 res += KH[i][0] * receiverPosCov[0][j];
     421           0 :                 res += KH[i][1] * receiverPosCov[1][j];
     422           0 :                 res += KH[i][2] * receiverPosCov[2][j];
     423           0 :                 KHP[i][j] = res;
     424             :             }
     425             :         }
     426             :         // prevent negative variances
     427           0 :         for (uint8_t i= 0; i<=2; i++) {
     428           0 :             if (receiverPosCov[i][i] < 0.0f) {
     429           0 :                 receiverPosCov[i][i] = 0.0f;
     430           0 :                 KHP[i][i] = 0.0f;
     431           0 :             } else if (KHP[i][i] > receiverPosCov[i][i]) {
     432           0 :                 KHP[i][i] = receiverPosCov[i][i];
     433             :             }
     434             :         }
     435             :         // apply the covariance correction
     436           0 :         for (uint8_t i= 0; i<=2; i++) {
     437           0 :             for (uint8_t j= 0; j<=2; j++) {
     438           0 :                 receiverPosCov[i][j] -= KHP[i][j];
     439             :             }
     440             :         }
     441             :         // ensure the covariance matrix is symmetric
     442           0 :         for (uint8_t i=1; i<=2; i++) {
     443           0 :             for (uint8_t j=0; j<=i-1; j++) {
     444           0 :                 ftype temp = 0.5f*(receiverPosCov[i][j] + receiverPosCov[j][i]);
     445           0 :                 receiverPosCov[i][j] = temp;
     446           0 :                 receiverPosCov[j][i] = temp;
     447             :             }
     448             :         }
     449             : 
     450           0 :         if (numBcnMeas >= 100) {
     451             :             // 100 observations is enough for a stable estimate under most conditions
     452             :             // TODO monitor stability of the position estimate
     453           0 :             rngBcnAlignmentCompleted = true;
     454             :         }
     455             :     }
     456             : }
     457             : 
     458             : /*
     459             : Run a single state Kalman filter to estimate the vertical position offset of the range beacon constellation
     460             : Calculate using a high and low hypothesis and select the hypothesis with the lowest innovation sequence
     461             : */
     462           0 : void NavEKF2_core::CalcRangeBeaconPosDownOffset(ftype obsVar, Vector3F &vehiclePosNED, bool aligning)
     463             : {
     464             :     // Handle height offsets between the primary height source and the range beacons by estimating
     465             :     // the beacon systems global vertical position offset using a single state Kalman filter
     466             :     // The estimated offset is used to correct the beacon height when calculating innovations
     467             :     // A high and low estimate is calculated to handle the ambiguity in height associated with beacon positions that are co-planar
     468             :     // The main filter then uses the offset with the smaller innovations
     469             : 
     470             :     ftype innov;    // range measurement innovation (m)
     471             :     ftype innovVar; // range measurement innovation variance (m^2)
     472             :     ftype gain;     // Kalman gain
     473             :     ftype obsDeriv; // derivative of observation relative to state
     474             : 
     475             :     const ftype stateNoiseVar = 0.1f; // State process noise variance
     476             :     const ftype filtAlpha = 0.01f; // LPF constant
     477             :     const ftype innovGateWidth = 5.0f; // width of innovation consistency check gate in std
     478             : 
     479             :     // estimate upper value for offset
     480             : 
     481             :     // calculate observation derivative
     482           0 :     ftype t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMax;
     483           0 :     ftype t3 = rngBcnDataDelayed.beacon_posNED.y - vehiclePosNED.y;
     484           0 :     ftype t4 = rngBcnDataDelayed.beacon_posNED.x - vehiclePosNED.x;
     485           0 :     ftype t5 = t2*t2;
     486           0 :     ftype t6 = t3*t3;
     487           0 :     ftype t7 = t4*t4;
     488           0 :     ftype t8 = t5+t6+t7;
     489             :     ftype t9;
     490           0 :     if (t8 > 0.1f) {
     491           0 :         t9 = 1.0f/sqrtF(t8);
     492           0 :         obsDeriv = t2*t9;
     493             : 
     494             :         // Calculate innovation
     495           0 :         innov = sqrtF(t8) - rngBcnDataDelayed.rng;
     496             : 
     497             :         // calculate a filtered innovation magnitude to be used to select between the high or low offset
     498           0 :         OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsF(innov);
     499             : 
     500             :         // covariance prediction
     501           0 :         bcnPosOffsetMaxVar += stateNoiseVar;
     502             : 
     503             :         // calculate the innovation variance
     504           0 :         innovVar = obsDeriv * bcnPosOffsetMaxVar * obsDeriv + obsVar;
     505           0 :         innovVar = MAX(innovVar, obsVar);
     506             : 
     507             :         // Reject range innovation spikes using a 5-sigma threshold unless aligning
     508           0 :         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
     509             :             // calculate the Kalman gain
     510           0 :             gain = (bcnPosOffsetMaxVar * obsDeriv) / innovVar;
     511             : 
     512             :             // state update
     513           0 :             bcnPosOffsetMax -= innov * gain;
     514             : 
     515             :             // covariance update
     516           0 :             bcnPosOffsetMaxVar -= gain * obsDeriv * bcnPosOffsetMaxVar;
     517           0 :             bcnPosOffsetMaxVar = MAX(bcnPosOffsetMaxVar, 0.0f);
     518             :         }
     519             :     }
     520             : 
     521             :     // estimate lower value for offset
     522             : 
     523             :     // calculate observation derivative
     524           0 :     t2 = rngBcnDataDelayed.beacon_posNED.z - vehiclePosNED.z + bcnPosOffsetMin;
     525           0 :     t5 = t2*t2;
     526           0 :     t8 = t5+t6+t7;
     527           0 :     if (t8 > 0.1f) {
     528           0 :         t9 = 1.0f/sqrtF(t8);
     529           0 :         obsDeriv = t2*t9;
     530             : 
     531             :         // Calculate innovation
     532           0 :         innov = sqrtF(t8) - rngBcnDataDelayed.rng;
     533             : 
     534             :         // calculate a filtered innovation magnitude to be used to select between the high or low offset
     535           0 :         OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsF(innov);
     536             : 
     537             :         // covariance prediction
     538           0 :         bcnPosOffsetMinVar += stateNoiseVar;
     539             : 
     540             :         // calculate the innovation variance
     541           0 :         innovVar = obsDeriv * bcnPosOffsetMinVar * obsDeriv + obsVar;
     542           0 :         innovVar = MAX(innovVar, obsVar);
     543             : 
     544             :         // Reject range innovation spikes using a 5-sigma threshold unless aligning
     545           0 :         if ((sq(innov) < sq(innovGateWidth) * innovVar) || aligning) {
     546             :             // calculate the Kalman gain
     547           0 :             gain = (bcnPosOffsetMinVar * obsDeriv) / innovVar;
     548             : 
     549             :             // state update
     550           0 :             bcnPosOffsetMin -= innov * gain;
     551             : 
     552             :             // covariance update
     553           0 :             bcnPosOffsetMinVar -= gain * obsDeriv * bcnPosOffsetMinVar;
     554           0 :             bcnPosOffsetMinVar = MAX(bcnPosOffsetMinVar, 0.0f);
     555             :         }
     556             :     }
     557             : 
     558             :     // calculate the mid vertical position of all beacons
     559           0 :     ftype bcnMidPosD = 0.5f * (minBcnPosD + maxBcnPosD);
     560             : 
     561             :     // ensure the two beacon vertical offset hypothesis place the mid point of the beacons below and above the flight vehicle
     562           0 :     bcnPosOffsetMax = MAX(bcnPosOffsetMax, vehiclePosNED.z - bcnMidPosD + 0.5f);
     563           0 :     bcnPosOffsetMin  = MIN(bcnPosOffsetMin,  vehiclePosNED.z - bcnMidPosD - 0.5f);
     564             : 
     565             :     // calculate the innovation for the main filter using the offset with the smallest innovation history
     566           0 :     if (OffsetMaxInnovFilt > OffsetMinInnovFilt) {
     567           0 :         bcnPosOffset = bcnPosOffsetMin;
     568             :     } else {
     569           0 :         bcnPosOffset = bcnPosOffsetMax;
     570             :     }
     571             : 
     572           0 : }
     573             : 
     574             : #endif  // AP_BEACON_ENABLED