diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp
index 34154eada1..0ceb2a73d2 100644
--- a/ArduCopter/commands.cpp
+++ b/ArduCopter/commands.cpp
@@ -107,7 +107,7 @@ bool Copter::far_from_EKF_origin(const Location& loc)
 {
     // check distance to EKF origin
     Location ekf_origin;
-    if (ahrs.get_origin(ekf_origin) && (ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M)) {
+    if (ahrs.get_origin(ekf_origin) && ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) || (labs(ekf_origin.alt - loc.alt) > EKF_ORIGIN_MAX_DIST_M))) {
         return true;
     }