diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index fb4b331e6e..2d0fe9ae7e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -1588,21 +1588,6 @@ void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms,
     }
 }
 
-// return data for debugging body frame odometry fusion
-uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const
-{
-    uint32_t ret = 0;
-    if (instance < 0 || instance >= num_cores) {
-        instance = primary;
-    }
-
-    if (core) {
-        ret = core[instance].getBodyFrameOdomDebug(velInnov, velInnovVar);
-    }
-
-    return ret;
-}
-
 // parameter conversion of EKF3 parameters
 void NavEKF3::convert_parameters()
 {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h
index 8670cdce4e..3a451f77f8 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.h
@@ -222,16 +222,6 @@ public:
     */
     void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
 
-    /*
-     * Return data for debugging body frame odometry fusion:
-     *
-     * velInnov are the XYZ body frame velocity innovations (m/s)
-     * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
-     *
-     * Return the system time stamp of the last update (msec)
-     */
-    uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const;
-
     /*
      * Writes the measurement from a yaw angle sensor
      *
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index d1a7e83d65..eb3a524741 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -263,6 +263,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
     rngBcnFuseDataReportIndex++;
 }
 
+#if EK3_FEATURE_BODY_ODOM
 void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
 {
     if (core_index != frontend->primary) {
@@ -270,25 +271,25 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
         return;
     }
 
-    Vector3f velBodyInnov,velBodyInnovVar;
     static uint32_t lastUpdateTime_ms = 0;
-    uint32_t updateTime_ms = getBodyFrameOdomDebug( velBodyInnov, velBodyInnovVar);
+    const uint32_t updateTime_ms = MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
     if (updateTime_ms > lastUpdateTime_ms) {
         const struct log_XKFD pkt11{
             LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
             time_us : time_us,
             core    : DAL_CORE(core_index),
-            velInnovX : velBodyInnov.x,
-            velInnovY : velBodyInnov.y,
-            velInnovZ : velBodyInnov.z,
-            velInnovVarX : velBodyInnovVar.x,
-            velInnovVarY : velBodyInnovVar.y,
-            velInnovVarZ : velBodyInnovVar.z
+            velInnovX : innovBodyVel[0],
+            velInnovY : innovBodyVel[1],
+            velInnovZ : innovBodyVel[2],
+            velInnovVarX : varInnovBodyVel[0],
+            velInnovVarY : varInnovBodyVel[1],
+            velInnovVarZ : varInnovBodyVel[2]
          };
         AP::logger().WriteBlock(&pkt11, sizeof(pkt11));
         lastUpdateTime_ms = updateTime_ms;
     }
 }
+#endif
 
 void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const
 {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
index 2118c01632..625a9a7b76 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
@@ -59,20 +59,6 @@ float NavEKF3_core::errorScore() const
     return score;
 }
 
-#if EK3_FEATURE_BODY_ODOM
-// return data for debugging body frame odometry fusion
-uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
-{
-    velInnov.x = innovBodyVel[0];
-    velInnov.y = innovBodyVel[1];
-    velInnov.z = innovBodyVel[2];
-    velInnovVar.x = varInnovBodyVel[0];
-    velInnovVar.y = varInnovBodyVel[1];
-    velInnovVar.z = varInnovBodyVel[2];
-    return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
-}
-#endif // EK3_FEATURE_BODY_ODOM
-
 // provides the height limit to be observed by the control loops
 // returns false if no height limiting is required
 // this is needed to ensure the vehicle does not fly too high when using optical flow navigation