diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp
index b43bbdeb07..a1cbb9ab07 100644
--- a/libraries/AC_Fence/AC_Fence.cpp
+++ b/libraries/AC_Fence/AC_Fence.cpp
@@ -667,28 +667,24 @@ bool AC_Fence::check_fence_polygon()
     }
 
     const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
-    if (_poly_loader.breached()) {
+
+    Location loc;
+    if (!AP::ahrs().get_location(loc)) {
+        return false;
+    }
+
+    bool inside_margin = false;
+
+    if (_poly_loader.breached(loc, _margin, inside_margin)) {
         if (!was_breached) {
             record_breach(AC_FENCE_TYPE_POLYGON);
             return true;
         }
         return false;
-    } else if (option_enabled(OPTIONS::MARGIN_BREACH)) {    // check is relatively expensive
-        Location loc;
-        if (AP::ahrs().get_location(loc)) {
-            Location l1 = loc, l2 = loc, l3 = loc, l4 = loc;
-            // quite approximate
-            l1.offset(_margin, _margin);
-            l2.offset(_margin, -_margin);
-            l3.offset(-_margin, _margin);
-            l4.offset(-_margin, -_margin);
-            if (_poly_loader.breached(l1) || _poly_loader.breached(l2)
-                || _poly_loader.breached(l3) || _poly_loader.breached(l4)) {
-                record_margin_breach(AC_FENCE_TYPE_POLYGON);
-            } else {
-                clear_margin_breach(AC_FENCE_TYPE_POLYGON);
-            }
-        }
+    } else if (option_enabled(OPTIONS::MARGIN_BREACH) && inside_margin) {
+        record_margin_breach(AC_FENCE_TYPE_POLYGON);
+    } else {
+        clear_margin_breach(AC_FENCE_TYPE_POLYGON);
     }
 
     if (was_breached) {
diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp
index 1ed5ecce92..db577f336c 100644
--- a/libraries/AC_Fence/AC_PolyFence_loader.cpp
+++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp
@@ -225,7 +225,7 @@ bool AC_PolyFence_loader::breached() const
 
 // check if a position (expressed as lat/lng) is within the boundary
 //   returns true if location is outside the boundary
-bool AC_PolyFence_loader::breached(const Location& loc) const
+bool AC_PolyFence_loader::breached(const Location& loc, float margin, bool& inside_margin) const
 {
     if (!loaded() || total_fence_count() == 0) {
         return false;
@@ -237,12 +237,15 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
 
     const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
     uint16_t num_inclusion_outside = 0;
+    inside_margin = false;
 
     // check we are inside each inclusion zone:
     for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
         const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
         if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
             num_inclusion_outside++;
+        } else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
+            inside_margin = true;
         }
     }
 
@@ -251,6 +254,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
         if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
             return true;
+        } else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
+            inside_margin = true;
         }
     }
 
@@ -262,6 +267,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const float diff_cm = loc.get_distance(circle_center)*100.0f;
         if (diff_cm < circle.radius * 100.0f) {
             return true;
+        } else if (diff_cm < (circle.radius + margin) * 100.0f) {
+            inside_margin = true;
         }
     }
 
@@ -273,6 +280,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const float diff_cm = loc.get_distance(circle_center)*100.0f;
         if (diff_cm > circle.radius * 100.0f) {
             num_inclusion_outside++;
+        } else if (diff_cm > (circle.radius - margin) * 100.0f) {
+            inside_margin = true;
         }
     }
 
diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h
index 1625c87793..5a539d7310 100644
--- a/libraries/AC_Fence/AC_PolyFence_loader.h
+++ b/libraries/AC_Fence/AC_PolyFence_loader.h
@@ -132,8 +132,15 @@ public:
 
     //  breached() - returns true if the vehicle has breached any fence
     bool breached() const WARN_IF_UNUSED;
+    //  breached(Location&, float margin, bool& inside_margin) - returns true if location is outside the boundary
+    //  sets if loc is inside the margin specified
+    bool breached(const Location& loc, float margin, bool& inside_margin) const WARN_IF_UNUSED;
     //  breached(Location&) - returns true if location is outside the boundary
-    bool breached(const Location& loc) const WARN_IF_UNUSED;
+    bool breached(const Location& loc) const WARN_IF_UNUSED
+    {
+        bool inside_margin;
+        return breached(loc, 0.0f, inside_margin);
+    }
 
     // returns true if a polygonal include fence could be returned
     bool inclusion_boundary_available() const WARN_IF_UNUSED {