DataFlash: Log EKF2 output predictor tracking data

libraries/DataFlash/LogFile.cpp

@@ -1269,7 +1269,10 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
                 offset : (int16_t)(100*gndOffset),
                 RI : (int16_t)(100*rngInnov),
                 meaRng : (uint16_t)(100*range),
-                  errHAGL : (uint16_t)(100*gndOffsetErr)
+                errHAGL : (uint16_t)(100*gndOffsetErr),
+                angErr : 0.0f,
+                velErr : 0.0f,
+                posErr : 0.0f
             };
             WriteBlock(&pkt5, sizeof(pkt5));
         }
@@ -1411,7 +1414,6 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     WriteBlock(&pkt4, sizeof(pkt4));
 
     // Write fifth EKF packet - take data from the primary instance
-    if (optFlowEnabled) {
     float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
     float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
     float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
@@ -1420,7 +1422,9 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
     float rngInnov=0; // range finder innovations
     float range=0; // measured range
     float gndOffsetErr=0; // filter ground offset state error
+    Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
     ahrs.get_NavEKF2().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
+    ahrs.get_NavEKF2().getOutputTrackingError(0,predictorErrors);
     struct log_EKF5 pkt5 = {
         LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
         time_us : time_us,
@@ -1432,10 +1436,12 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
         offset : (int16_t)(100*gndOffset),
         RI : (int16_t)(100*rngInnov),
         meaRng : (uint16_t)(100*range),
-            errHAGL : (uint16_t)(100*gndOffsetErr)
+        errHAGL : (uint16_t)(100*gndOffsetErr),
+        angErr : (float)predictorErrors.x,
+        velErr : (float)predictorErrors.y,
+        posErr : (float)predictorErrors.z
      };
     WriteBlock(&pkt5, sizeof(pkt5));
-    }
 
     // log innovations for the second IMU if enabled
     if (ahrs.get_NavEKF2().activeCores() >= 2) {

libraries/DataFlash/LogStructure.h

@@ -352,6 +352,9 @@ struct PACKED log_EKF5 {
     int16_t RI;
     uint16_t meaRng;
     uint16_t errHAGL;
+    float angErr;
+    float velErr;
+    float posErr;
 };
 
 struct PACKED log_Cmd {
@@ -795,7 +798,7 @@ Format characters in the format string for binary log messages
     { LOG_EKF4_MSG, sizeof(log_EKF4), \
       "EKF4","QcccccccbbHBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,OFE,FS,TS,SS,GPS" }, \
     { LOG_EKF5_MSG, sizeof(log_EKF5), \
-      "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
+      "EKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,_,_,_" }, \
     { LOG_NKF1_MSG, sizeof(log_EKF1), \
       "NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
     { LOG_NKF2_MSG, sizeof(log_NKF2), \
@@ -805,7 +808,7 @@ Format characters in the format string for binary log messages
     { LOG_NKF4_MSG, sizeof(log_NKF4), \
       "NKF4","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
     { LOG_NKF5_MSG, sizeof(log_EKF5), \
-      "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
+      "NKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos" }, \
     { LOG_NKF6_MSG, sizeof(log_EKF1), \
       "NKF6","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
     { LOG_NKF7_MSG, sizeof(log_NKF2), \