AP_NavEKF3: name covariance matrix as mutable

In preparation for changing the regular `P` name to const, in
preparation for auditing the code so that writes to the matrix keep its
necessary numeric properties.

Sadly there is not a better way than a per-file `#define` to make the
switch. Making `P` a reference to `Pmut` substantially changes the
compiler output. Defining `P` in the header file conflicts with other
includes. Doing the rename at the top of each file allows each file to
be fixed independently.

libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@@ -4,6 +4,8 @@
 #include "AP_NavEKF3_core.h"
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 /********************************************************
 *                   RESET FUNCTIONS                     *
 ********************************************************/

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -6,6 +6,8 @@
 
 #include "AP_DAL/AP_DAL.h"
 
+#define P (Pmut)
+
 // Control filter mode transitions
 void NavEKF3_core::controlFilterModes()
 {

libraries/AP_NavEKF3/AP_NavEKF3_GyroBias.cpp

@@ -1,6 +1,8 @@
 #include "AP_NavEKF3_core.h"
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 // reset the body axis gyro bias states to zero and re-initialise the corresponding covariances
 // Assume that the calibration is performed to an accuracy of 0.5 deg/sec which will require averaging under static conditions
 // WARNING - a non-blocking calibration method must be used

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -12,6 +12,8 @@
 
 #pragma GCC diagnostic ignored "-Wnarrowing"
 
+#define P (Pmut)
+
 void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
 {
     // Write first EKF packet

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -14,6 +14,8 @@
   #define GPS_VEL_YAW_ALIGN_MIN_SPD 1.0F
 #endif
 
+#define P (Pmut)
+
 /********************************************************
 *                   RESET FUNCTIONS                     *
 ********************************************************/

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -8,6 +8,8 @@
 #include <AP_DAL/AP_DAL.h>
 #include <AP_InternalError/AP_InternalError.h>
 
+#define P (Pmut)
+
 #if AP_RANGEFINDER_ENABLED
 /********************************************************
 *              OPT FLOW AND RANGE FINDER                *

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -9,6 +9,8 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 /********************************************************
 *                   RESET FUNCTIONS                     *
 ********************************************************/

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -5,6 +5,8 @@
 #include <AP_DAL/AP_DAL.h>
 #include <GCS_MAVLink/GCS.h>
 
+#define P (Pmut)
+
 // Check basic filter health metrics and return a consolidated health status
 bool NavEKF3_core::healthy(void) const
 {

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -5,6 +5,8 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 /********************************************************
 *                   RESET FUNCTIONS                     *
 ********************************************************/

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

@@ -5,6 +5,8 @@
 
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 // initialise state:
 void NavEKF3_core::BeaconFusion::InitialiseVariables()
 {

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -7,6 +7,8 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_DAL/AP_DAL.h>
 
+#define P (Pmut)
+
 // constructor
 NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend, AP_DAL &_dal) :
     dal(_dal),

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1110,7 +1110,7 @@ private:
     uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
 
     ftype gpsNoiseScaler;           // Used to scale the  GPS measurement noise and consistency gates to compensate for operation with small satellite counts
-    Matrix24 P;                     // covariance matrix
+    Matrix24 Pmut;                  // covariance matrix, must remain symmetric and positive semi-definite
     EKF_IMU_buffer_t<imu_elements> storedIMU;      // IMU data buffer
     EKF_obs_buffer_t<gps_elements> storedGPS;      // GPS data buffer
     EKF_obs_buffer_t<mag_elements> storedMag;      // Magnetometer data buffer