diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 01725bac1e..7ad0642597 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/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]; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index c37ac6cdf2..9e6acfa967 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index aa13ad939e..a02b6fa50c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/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]; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 4cf7ddf07e..070b679e2f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp index c8c9782459..d5c37fc01b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp +++ b/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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 8020823652..7d2e255934 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/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]; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 763b5d9aa9..ec617e9eae 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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