diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp
index 82f7f0824a..e4f914ba52 100644
--- a/libraries/AC_Fence/AC_Fence.cpp
+++ b/libraries/AC_Fence/AC_Fence.cpp
@@ -65,7 +65,8 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
 };
 
 /// Default constructor.
-AC_Fence::AC_Fence(const AP_InertialNav& inav) :
+AC_Fence::AC_Fence(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
+    _ahrs(ahrs),
     _inav(inav),
     _alt_max_backup(0),
     _circle_radius_backup(0),
@@ -233,16 +234,36 @@ uint8_t AC_Fence::check_fence(float curr_alt)
 }
 
 // returns true if the destination is within fence (used to reject waypoints outside the fence)
-bool AC_Fence::check_destination_within_fence(float dest_alt, float dest_distance_to_home)
+bool AC_Fence::check_destination_within_fence(const Location_Class& loc)
 {
     // Altitude fence check
-    if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) && (dest_alt >= _alt_max)) {
-        return false;
+    if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
+        int32_t alt_above_home_cm;
+        if (loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_home_cm)) {
+            if ((alt_above_home_cm * 0.01f) > _alt_max) {
+                return false;
+            }
+        }
     }
 
     // Circular fence check
-    if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) && (dest_distance_to_home >= _circle_radius)) {
-        return false;
+    if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
+        if ((get_distance_cm(_ahrs.get_home(), loc) * 0.01f) > _circle_radius) {
+            return false;
+        }
+    }
+
+    // polygon fence check
+    if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
+        // check ekf has a good location
+        Location temp_loc;
+        if (_inav.get_location(temp_loc)) {
+            const struct Location &ekf_origin = _inav.get_origin();
+            Vector2f position = location_diff(ekf_origin, loc) * 100.0f;
+            if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
+                return false;
+            }
+        }
     }
 
     return true;
diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h
index da7dfd7937..7f74774cc0 100644
--- a/libraries/AC_Fence/AC_Fence.h
+++ b/libraries/AC_Fence/AC_Fence.h
@@ -6,8 +6,10 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_AHRS/AP_AHRS.h>
 #include <AP_InertialNav/AP_InertialNav.h>     // Inertial Navigation library
 #include <AC_Fence/AC_PolyFence_loader.h>
+#include <AP_Common/Location.h>
 
 // bit masks for enabled fence types.  Used for TYPE parameter
 #define AC_FENCE_TYPE_NONE                          0       // fence disabled
@@ -35,7 +37,7 @@ class AC_Fence
 public:
 
     /// Constructor
-    AC_Fence(const AP_InertialNav& inav);
+    AC_Fence(const AP_AHRS& ahrs, const AP_InertialNav& inav);
 
     /// enable - allows fence to be enabled/disabled.  Note: this does not update the eeprom saved value
     void enable(bool true_false) { _enabled = true_false; }
@@ -58,7 +60,7 @@ public:
     uint8_t check_fence(float curr_alt);
 
     // returns true if the destination is within fence (used to reject waypoints outside the fence)
-    bool check_destination_within_fence(float dest_alt, float dest_distance_to_home);
+    bool check_destination_within_fence(const Location_Class& loc);
 
     /// get_breaches - returns bit mask of the fence types that have been breached
     uint8_t get_breaches() const { return _breached_fences; }
@@ -123,6 +125,7 @@ private:
     bool load_polygon_from_eeprom(bool force_reload = false);
 
     // pointers to other objects we depend upon
+    const AP_AHRS& _ahrs;
     const AP_InertialNav& _inav;
 
     // parameters