diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 8f73f8ae02c71..33dff0dac2d57 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -2031,7 +2031,10 @@ void NavEKF3_core::SelectBodyOdomFusion() // Check for body odometry data (aka visual position delta) at the fusion time horizon const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms); if (bodyOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { - + // If VelZ source is set to none, don't use the odometry data for Z velocity + if (!frontend->sources.haveVelZSource()) { + bodyOdmDataDelayed.vel.z = stateStruct.velocity.z; + } // Fuse data into the main filter FuseBodyVel(); }