AP_NavEKF2: moved some of the intermediate vars to the stack

this keeps stack frames below 1k, while giving faster access to the
variables and saving more memory

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -39,6 +39,7 @@ void NavEKF2_core::FuseAirspeed()
     float SK_TAS;
     Vector24 H_TAS;
     float VtasPred;
+    Vector28 Kfusion;
 
     // health is set bad until test passed
     tasHealth = false;
@@ -269,6 +270,7 @@ void NavEKF2_core::FuseSideslip()
     Vector3f vel_rel_wind;
     Vector24 H_BETA;
     float innovBeta;
+    Vector28 Kfusion;
 
     // copy required states to local variable names
     q0 = stateStruct.quat[0];

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -346,6 +346,7 @@ void NavEKF2_core::FuseMagnetometer()
     Vector6 SK_MX;
     Vector6 SK_MY;
     Vector6 SK_MZ;
+    Vector28 Kfusion;
 
     hal.util->perf_end(_perf_test[1]);
     
@@ -780,6 +781,8 @@ void NavEKF2_core::fuseEulerYaw()
     float measured_yaw;
     float H_YAW[3];
     Matrix3f Tbn_zeroYaw;
+    Vector28 Kfusion;
+
     if (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) {
         // calculate observation jacobian when we are observing the first rotation in a 321 sequence
         float t2 = q0*q0;
@@ -1037,6 +1040,8 @@ void NavEKF2_core::FuseDeclination(float declErr)
     float t12 = 1.0f/t11;
 
     float H_MAG[24];
+    Vector28 Kfusion;
+
     H_MAG[16] = -magE*t5;
     H_MAG[17] = magN*t5;
 

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -285,6 +285,7 @@ void NavEKF2_core::FuseOptFlow()
     Vector3f relVelSensor;
     Vector14 SH_LOS;
     Vector2 losPred;
+    Vector28 Kfusion;
 
     // Copy required states to local variable names
     float q0  = stateStruct.quat[0];

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -438,6 +438,7 @@ void NavEKF2_core::FuseVelPosNED()
     Vector6 R_OBS; // Measurement variances used for fusion
     Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
     float SK;
+    Vector28 Kfusion;
 
     // perform sequential fusion of GPS measurements. This assumes that the
     // errors in the different velocity and position components are

libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp

@@ -43,6 +43,7 @@ void NavEKF2_core::FuseRngBcn()
     float bcn_pd;
     const float R_BCN = sq(MAX(rngBcnDataDelayed.rngErr , 0.1f));
     float rngPred;
+    Vector28 Kfusion;
 
     // health is set bad until test passed
     rngBcnHealth = false;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -32,15 +32,9 @@ NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
     frontend(_frontend),
     // setup the intermediate variables shared by all cores (to save memory)
     common((struct core_common *)_frontend->core_common),
-    Kfusion(common->Kfusion),
     KH(common->KH),
     KHP(common->KHP),
-    nextP(common->nextP),
-    processNoise(common->processNoise),
-    SF(common->SF),
-    SG(common->SG),
-    SQ(common->SQ),
-    SPP(common->SPP)
+    nextP(common->nextP)
 {
     _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test0");
     _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test1");
@@ -882,6 +876,10 @@ void NavEKF2_core::CovariancePrediction()
     float day_s;        // Y axis delta angle measurement scale factor
     float daz_s;        // Z axis delta angle measurement scale factor
     float dvz_b;        // Z axis delta velocity measurement bias (rad)
+    Vector25 SF;
+    Vector5 SG;
+    Vector8 SQ;
+    Vector24 processNoise;
 
     // calculate covariance prediction process noise
     // use filtered height rate to increase wind process noise when climbing or descending
@@ -979,6 +977,7 @@ void NavEKF2_core::CovariancePrediction()
     SQ[6] = 2*q1*q2;
     SQ[7] = SG[4];
 
+    Vector23 SPP;
     SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
     SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
     SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -494,15 +494,9 @@ private:
       place to calculate maximum stack usage using static analysis.
      */
     struct core_common {
-        Vector28 Kfusion;
         Matrix24 KH;
         Matrix24 KHP;
         Matrix24 nextP;
-        Vector24 processNoise;
-        Vector25 SF;
-        Vector5 SG;
-        Vector8 SQ;
-        Vector23 SPP;
     } *common;
 
     // bias estimates for the IMUs that are enabled but not being used
@@ -822,7 +816,6 @@ private:
     bool badIMUdata;                // boolean true if the bad IMU data is detected
 
     float gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
-    Vector28 &Kfusion;              // Kalman gain vector
     Matrix24 &KH;                   // intermediate result used for covariance updates
     Matrix24 &KHP;                  // intermediate result used for covariance updates
     Matrix24 P;                     // covariance matrix
@@ -880,11 +873,6 @@ private:
     uint32_t lastYawTime_ms;        // time stamp when yaw observation was last fused (msec)
     uint32_t ekfStartTime_ms;       // time the EKF was started (msec)
     Matrix24 &nextP;                // Predicted covariance matrix before addition of process noise to diagonals
-    Vector24 &processNoise;         // process noise added to diagonals of predicted covariance matrix
-    Vector25 &SF;                   // intermediate variables used to calculate predicted covariance matrix
-    Vector5 &SG;                    // intermediate variables used to calculate predicted covariance matrix
-    Vector8 &SQ;                    // intermediate variables used to calculate predicted covariance matrix
-    Vector23 &SPP;                  // intermediate variables used to calculate predicted covariance matrix
     Vector2f lastKnownPositionNE;   // last known position
     uint32_t lastDecayTime_ms;      // time of last decay of GPS position offset
     float velTestRatio;             // sum of squares of GPS velocity innovation divided by fail threshold