diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index f7000b0f30..2ee3b414ad 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -223,6 +223,9 @@ public:
         return false;
     }
 
+    // see if EKF lane switching is possible to avoid EKF failsafe
+    virtual void check_lane_switch(void) {}
+    
     // Euler angles (radians)
     float roll;
     float pitch;
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 4f379632b7..737b82522e 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -1773,6 +1773,28 @@ uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
     return AP::ins().get_primary_gyro();
 }
 
+// see if EKF lane switching is possible to avoid EKF failsafe
+void AP_AHRS_NavEKF::check_lane_switch(void)
+{
+    switch (active_EKF_type()) {
+    case EKF_TYPE_NONE:
+        break;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKF_TYPE_SITL:
+        break;
+#endif
+
+    case EKF_TYPE2:
+        EKF2.checkLaneSwitch();
+        break;
+
+    case EKF_TYPE3:
+        EKF3.checkLaneSwitch();
+        break;
+    }
+}
+
 
 AP_AHRS_NavEKF &AP::ahrs_navekf()
 {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 30b82a6f13..46af3d1096 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -259,6 +259,9 @@ public:
     // get the index of the current primary gyro sensor
     uint8_t get_primary_gyro_index(void) const override;
 
+    // see if EKF lane switching is possible to avoid EKF failsafe
+    void check_lane_switch(void) override;
+    
 private:
     enum EKF_TYPE {EKF_TYPE_NONE=0,
                    EKF_TYPE3=3,