AP_NavEKF: added a common parent class for EKF2 and EKF3 core classes

this provides a common location for static intermediate variables in
EK2 and EK3. This has a few benefits:

 - the compiler can determine the address of the frequently accessed
   variables at compile time, making them faster

 - by sharing between EK2 and EK3 we save a lot of memory if both EK2
   and EK3 are enabled

 - we can fill all these with NaN in SITL on every loop, which allows
   us to catch cases where the variables are ever re-used between
   loops, which guarantees we aren't mixing data between EKF lanes or
   between EK2 and EK3

libraries/AP_NavEKF/AP_NavEKF_core_common.cpp

@@ -0,0 +1,39 @@
+/*
+  NavEKF_core_common holds scratch data shared by EKF2 and EKF3
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "AP_NavEKF_core_common.h"
+
+NavEKF_core_common::Matrix24 NavEKF_core_common::KH;
+NavEKF_core_common::Matrix24 NavEKF_core_common::KHP;
+NavEKF_core_common::Matrix24 NavEKF_core_common::nextP;
+NavEKF_core_common::Vector28 NavEKF_core_common::Kfusion;
+
+/*
+  fill common scratch variables, for detecting re-use of variables between loops in SITL
+ */
+void NavEKF_core_common::fill_scratch_variables(void)
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // fill the common variables with NaN, so we catch any cases in
+    // SITL where they are used without initialisation. These are all
+    // supposed to be scratch variables that are not used between
+    // iterations
+    fill_nanf(&KH[0][0], sizeof(KH)/sizeof(float));
+    fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(float));
+    fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(float));
+    fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(float));
+#endif
+}

libraries/AP_NavEKF/AP_NavEKF_core_common.h

@@ -0,0 +1,51 @@
+/*
+  NavEKF_core_common holds scratch data shared by EKF2 and EKF3
+
+  This program is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <stdint.h>
+#include <AP_Math/AP_Math.h>
+
+/*
+  this declares a common parent class for AP_NavEKF2 and
+  AP_NavEKF3. The purpose of this class is to hold common static
+  scratch space variables. These variables do not hold anything that
+  matters between iterations, they are only intermediate variables. By
+  placing these in a common parent class we save a lot of memory, but
+  we also save a lot of CPU (approx 10% on STM32F427) as the compiler
+  is able to resolve the address of these variables at compile time,
+  which means significantly faster code
+ */
+class NavEKF_core_common {
+public:
+    typedef float ftype;
+#if MATH_CHECK_INDEXES
+    typedef VectorN<ftype,28> Vector28;
+    typedef VectorN<VectorN<ftype,24>,24> Matrix24;
+#else
+    typedef ftype Vector28[28];
+    typedef ftype Matrix24[24][24];
+#endif
+
+protected:
+    static Matrix24 KH;                   // intermediate result used for covariance updates
+    static Matrix24 KHP;                  // intermediate result used for covariance updates
+    static Matrix24 nextP;                // Predicted covariance matrix before addition of process noise to diagonals
+    static Vector28 Kfusion;              // intermediate fusion vector
+
+    // fill all the common scratch variables with NaN on SITL
+    void fill_scratch_variables(void);
+};