DataFlash: Log EKF2 output predictor tracking data

libraries/DataFlash/LogFile.cpp

@@ -1268,10 +1268,13 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
                 HAGL : (int16_t)(100*HAGL),
                 offset : (int16_t)(100*gndOffset),
                 RI : (int16_t)(100*rngInnov),
-                  meaRng : (uint16_t)(100*range),
-                  errHAGL : (uint16_t)(100*gndOffsetErr)
-               };
-              WriteBlock(&pkt5, sizeof(pkt5));
+                meaRng : (uint16_t)(100*range),
+                errHAGL : (uint16_t)(100*gndOffsetErr),
+                angErr : 0.0f,
+                velErr : 0.0f,
+                posErr : 0.0f
+            };
+            WriteBlock(&pkt5, sizeof(pkt5));
         }
     }
     // only log EKF2 if enabled
@@ -1411,31 +1414,34 @@ 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
-        float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
-        float HAGL=0; // height above ground level
-        float rngInnov=0; // range finder innovations
-        float range=0; // measured range
-        float gndOffsetErr=0; // filter ground offset state error
-        ahrs.get_NavEKF2().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
-        struct log_EKF5 pkt5 = {
-            LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
-            time_us : time_us,
-            normInnov : (uint8_t)(MIN(100*normInnov,255)),
-            FIX : (int16_t)(1000*flowInnovX),
-            FIY : (int16_t)(1000*flowInnovY),
-            AFI : (int16_t)(1000*auxFlowInnov),
-            HAGL : (int16_t)(100*HAGL),
-            offset : (int16_t)(100*gndOffset),
-            RI : (int16_t)(100*rngInnov),
-            meaRng : (uint16_t)(100*range),
-            errHAGL : (uint16_t)(100*gndOffsetErr)
-         };
-        WriteBlock(&pkt5, sizeof(pkt5));
-    }
+    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
+    float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
+    float HAGL=0; // height above ground level
+    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,
+        normInnov : (uint8_t)(MIN(100*normInnov,255)),
+        FIX : (int16_t)(1000*flowInnovX),
+        FIY : (int16_t)(1000*flowInnovY),
+        AFI : (int16_t)(1000*auxFlowInnov),
+        HAGL : (int16_t)(100*HAGL),
+        offset : (int16_t)(100*gndOffset),
+        RI : (int16_t)(100*rngInnov),
+        meaRng : (uint16_t)(100*range),
+        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), \