diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index e7c31636f0..fb5225caaa 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -393,6 +393,20 @@ bool Location::same_latlon_as(const Location &loc2) const
     return (lat == loc2.lat) && (lng == loc2.lng);
 }
 
+bool Location::same_alt_as(const Location &loc2) const
+{
+    // fast path if the altitude frame is the same
+    if (this->get_alt_frame() == loc2.get_alt_frame()) {
+        return this->alt == loc2.alt;
+    }
+
+    ftype alt_diff;
+    bool have_diff = this->get_alt_distance(loc2, alt_diff);
+
+    const ftype tolerance = FLT_EPSILON;
+    return have_diff && (fabsF(alt_diff) < tolerance);
+}
+
 // return true when lat and lng are within range
 bool Location::check_latlng() const
 {
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index e294cef2fb..ad1260fdac 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -107,6 +107,9 @@ public:
     // check if lat and lng match. Ignore altitude and options
     bool same_latlon_as(const Location &loc2) const;
 
+    // check if altitude matches.
+    bool same_alt_as(const Location &loc2) const;
+
     /*
      * convert invalid waypoint with useful data. return true if location changed
      */