diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
index 36743c846a5..7bbf429b91e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
@@ -4,7 +4,7 @@
 #include "AP_NavEKF3_core.h"
 #include <AP_DAL/AP_DAL.h>
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 /********************************************************
 *                   RESET FUNCTIONS                     *
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 420fce82902..efc7302298f 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -12,7 +12,7 @@
 
 #pragma GCC diagnostic ignored "-Wnarrowing"
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const
 {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 7d252488cbd..3eb33b4b850 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -8,7 +8,7 @@
 #include <AP_DAL/AP_DAL.h>
 #include <AP_InternalError/AP_InternalError.h>
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 #if AP_RANGEFINDER_ENABLED
 /********************************************************
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
index 83adf392d8a..b6fece28f7e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
@@ -9,7 +9,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_DAL/AP_DAL.h>
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 /********************************************************
 *                   RESET FUNCTIONS                     *
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
index 70b214a5119..925499ed943 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
@@ -5,7 +5,7 @@
 #include <AP_DAL/AP_DAL.h>
 #include <GCS_MAVLink/GCS.h>
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 // Check basic filter health metrics and return a consolidated health status
 bool NavEKF3_core::healthy(void) const
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index f79b209c3b6..9e0fef32a0c 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -5,7 +5,7 @@
 
 #include <AP_DAL/AP_DAL.h>
 
-#define P (Pmut)
+#define P (const_cast<const Matrix24 &>(Pmut))
 
 // initialise state:
 void NavEKF3_core::BeaconFusion::InitialiseVariables()