diff --git a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
index f6f1a93394..a6434fc934 100644
--- a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml
@@ -277,7 +277,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="5.0" />
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
     <!-- Portland 
     Normalize[{19443.3, 5354.0,48519.5}] = (0.37004, 0.101896, 0.923411)-->
     <define name="H_X" value="0.37004"/>
diff --git a/conf/airframes/BR/bebop_default.xml b/conf/airframes/BR/bebop_default.xml
index 700bf7d766..fdb5376c26 100644
--- a/conf/airframes/BR/bebop_default.xml
+++ b/conf/airframes/BR/bebop_default.xml
@@ -110,7 +110,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml
index e82f0e8c79..bed9450cbe 100644
--- a/conf/airframes/BR/bebop_indi.xml
+++ b/conf/airframes/BR/bebop_indi.xml
@@ -123,7 +123,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/BR/bebop_indi_frog.xml b/conf/airframes/BR/bebop_indi_frog.xml
index 4116b1598c..836d38b714 100644
--- a/conf/airframes/BR/bebop_indi_frog.xml
+++ b/conf/airframes/BR/bebop_indi_frog.xml
@@ -125,7 +125,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/CDW/cdw_bebop.xml b/conf/airframes/CDW/cdw_bebop.xml
index eec47793f4..781ba7ec6c 100644
--- a/conf/airframes/CDW/cdw_bebop.xml
+++ b/conf/airframes/CDW/cdw_bebop.xml
@@ -123,7 +123,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_control.xml b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
index 3d09e7ea46..e06561d103 100644
--- a/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
@@ -39,7 +39,6 @@
   </command_laws>
 
   <section name="INS_BASE" prefix="INS_">
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
     <define name="SONAR_MAX_RANGE" value="4.0"/>
     <define name="SONAR_MIN_RANGE" value="0.01"/>
   </section>
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
index 295e453128..f5b7c09956 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
@@ -114,7 +114,6 @@
 
   <section name="INS" prefix="INS_">
     <!--<define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
index b87bb9dd08..86cfff9612 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
@@ -116,7 +116,6 @@
 
   <section name="INS" prefix="INS_">
     <!--<define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/MM/bebop.xml b/conf/airframes/MM/bebop.xml
index 34ac9a8910..2024557974 100644
--- a/conf/airframes/MM/bebop.xml
+++ b/conf/airframes/MM/bebop.xml
@@ -122,7 +122,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/MM/bebop2_lum1_xbee.xml b/conf/airframes/MM/bebop2_lum1_xbee.xml
index fd135389b3..53fc9366ed 100644
--- a/conf/airframes/MM/bebop2_lum1_xbee.xml
+++ b/conf/airframes/MM/bebop2_lum1_xbee.xml
@@ -138,7 +138,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
index 1843588bb7..26b7ac5197 100644
--- a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
+++ b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
@@ -447,7 +447,6 @@ The most crucial part for the magnetometer calibration:
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="1"/>
     <define name="USE_GPS_ALT_SPEED" value="1"/>
diff --git a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
index ce50ae1595..b0ec9c48bd 100644
--- a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
+++ b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
@@ -183,7 +183,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="1.5"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
diff --git a/conf/airframes/OPENUAS/openuas_durafly_g2.xml b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
index 8b2d49fa72..4a1a43de58 100644
--- a/conf/airframes/OPENUAS/openuas_durafly_g2.xml
+++ b/conf/airframes/OPENUAS/openuas_durafly_g2.xml
@@ -430,7 +430,6 @@
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="1"/>
     <define name="USE_GPS_ALT_SPEED" value="1"/>
diff --git a/conf/airframes/OPENUAS/openuas_eflite_t28.xml b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
index fe4fa22879..17871aaf50 100644
--- a/conf/airframes/OPENUAS/openuas_eflite_t28.xml
+++ b/conf/airframes/OPENUAS/openuas_eflite_t28.xml
@@ -309,7 +309,6 @@ NOTES:
 
     <module name="ins" type="alt_float">  <!--Does not work in SIM: extended thus use alt_float"/> -->
       <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-      <!--<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>-->
     </module>
 
     <!-- See http://wiki.paparazziuav.org/wiki/EnergyControl -->
diff --git a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
index ea21e5f372..8dc4d7dd3b 100644
--- a/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
+++ b/conf/airframes/OPENUAS/openuas_multiplex_minimag.xml
@@ -317,7 +317,6 @@ NOTES:
 
     <module name="ins" type="alt_float">  <!--Does not work in SIM: extended thus use alt_float"/> -->
       <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-      <!--<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>-->
     </module>
 
     <!-- See http://wiki.paparazziuav.org/wiki/EnergyControl -->
diff --git a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
index 057708bdb9..10a34248d4 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
@@ -380,7 +380,6 @@ The most crucial part for the magnetometer calibration:
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="TRUE"/>
     <define name="USE_GPS_ALT_SPEED" value="FALSE"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
index 0d970f6e04..4673f83480 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
@@ -407,7 +407,6 @@ The most crucial part for the magnetometer calibration:
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="TRUE"/>
     <define name="USE_GPS_ALT_SPEED" value="FALSE"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
index 0512464bdc..b5925b171c 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
@@ -420,7 +420,6 @@ The most crucial part for the magnetometer calibration:
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="TRUE"/>
     <define name="USE_GPS_ALT_SPEED" value="FALSE"/>
diff --git a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
index 909644eaa5..2e4afaacf9 100644
--- a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
+++ b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
@@ -455,7 +455,6 @@ The most crucial part for the magnetometer calibration:
     <define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
 
     <define name="SONAR_COMPENSATE_ROTATION" value="TRUE"/><!-- compensate AGL for body rotation -->
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <define name="USE_GPS_ALT" value="1"/>
     <define name="USE_GPS_ALT_SPEED" value="1"/>
diff --git a/conf/airframes/examples/ardrone2.xml b/conf/airframes/examples/ardrone2.xml
index 1653350beb..37d1cc35cf 100644
--- a/conf/airframes/examples/ardrone2.xml
+++ b/conf/airframes/examples/ardrone2.xml
@@ -111,7 +111,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml
index 74745dea83..3124edf5b1 100644
--- a/conf/airframes/examples/ardrone2_gazebo.xml
+++ b/conf/airframes/examples/ardrone2_gazebo.xml
@@ -123,7 +123,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/examples/ardrone2_opticflow_hover.xml b/conf/airframes/examples/ardrone2_opticflow_hover.xml
index bff848900c..5bb143a961 100644
--- a/conf/airframes/examples/ardrone2_opticflow_hover.xml
+++ b/conf/airframes/examples/ardrone2_opticflow_hover.xml
@@ -122,7 +122,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/examples/bebop.xml b/conf/airframes/examples/bebop.xml
index eea576e0f7..98b399403d 100644
--- a/conf/airframes/examples/bebop.xml
+++ b/conf/airframes/examples/bebop.xml
@@ -114,7 +114,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml
index 21623fbf87..a193782655 100644
--- a/conf/airframes/examples/bebop2_indi.xml
+++ b/conf/airframes/examples/bebop2_indi.xml
@@ -92,7 +92,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <!--define name="SONAR_UPDATE_ON_AGL" value="TRUE"/-->
   </section>
 
 
diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
index c237bb671d..1a580871e9 100644
--- a/conf/airframes/examples/bebop2_opticflow.xml
+++ b/conf/airframes/examples/bebop2_opticflow.xml
@@ -105,7 +105,6 @@
     <define name="VFF_VZ_R_GPS" value="0.01"/>     
     <define name="SONAR_MIN_RANGE" value="0.0"/>
     <define name="SONAR_MAX_RANGE" value="3.5"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
   </section>
 
diff --git a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
index c431a1365b..4694248600 100644
--- a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
+++ b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
@@ -111,7 +111,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/examples/swing.xml b/conf/airframes/examples/swing.xml
index f60000d072..7a1579a6f1 100644
--- a/conf/airframes/examples/swing.xml
+++ b/conf/airframes/examples/swing.xml
@@ -102,7 +102,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
diff --git a/conf/airframes/tudelft/ardrone2_indi.xml b/conf/airframes/tudelft/ardrone2_indi.xml
index 8f8806f052..ac18dc913a 100644
--- a/conf/airframes/tudelft/ardrone2_indi.xml
+++ b/conf/airframes/tudelft/ardrone2_indi.xml
@@ -103,7 +103,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/ardrone2_opticflow.xml b/conf/airframes/tudelft/ardrone2_opticflow.xml
index 11226a855c..da6700a4f4 100644
--- a/conf/airframes/tudelft/ardrone2_opticflow.xml
+++ b/conf/airframes/tudelft/ardrone2_opticflow.xml
@@ -124,7 +124,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/bebop2_detect_gate_front.xml b/conf/airframes/tudelft/bebop2_detect_gate_front.xml
index 0fb880c2cd..f17cefe859 100644
--- a/conf/airframes/tudelft/bebop2_detect_gate_front.xml
+++ b/conf/airframes/tudelft/bebop2_detect_gate_front.xml
@@ -145,7 +145,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="0"/>
     <!-- <define name="VFF_R_GPS" value="0.01"/> -->
diff --git a/conf/airframes/tudelft/bebop2_indi.xml b/conf/airframes/tudelft/bebop2_indi.xml
index c846fa6730..7c445ec59f 100644
--- a/conf/airframes/tudelft/bebop2_indi.xml
+++ b/conf/airframes/tudelft/bebop2_indi.xml
@@ -81,7 +81,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/bebop2_no_damping.xml b/conf/airframes/tudelft/bebop2_no_damping.xml
index f29e17b214..350fd9466b 100644
--- a/conf/airframes/tudelft/bebop2_no_damping.xml
+++ b/conf/airframes/tudelft/bebop2_no_damping.xml
@@ -92,7 +92,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/bebop2_opticflow.xml b/conf/airframes/tudelft/bebop2_opticflow.xml
index 34981efd71..661891f180 100644
--- a/conf/airframes/tudelft/bebop2_opticflow.xml
+++ b/conf/airframes/tudelft/bebop2_opticflow.xml
@@ -123,7 +123,6 @@
     <define name="VFF_VZ_R_GPS" value="0.01"/>     
     <define name="SONAR_MIN_RANGE" value="0.0"/>
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="FALSE"/>
 
   </section>
 
diff --git a/conf/airframes/tudelft/bebop2_optitrack.xml b/conf/airframes/tudelft/bebop2_optitrack.xml
index ba13e91243..142addf0ed 100644
--- a/conf/airframes/tudelft/bebop2_optitrack.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack.xml
@@ -97,7 +97,6 @@
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
     <define name="SONAR_MIN_RANGE" value="0.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
     <define name="USE_GPS_ALT" value="1"/>
     <define name="USE_GPS_ALT_SPEED" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
index a5cd96aade..a5f4b0839d 100644
--- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -133,7 +133,6 @@
 
   <section name="INS" prefix="INS_">
     <!--<define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/tudelft/bebop2_undistort_front.xml b/conf/airframes/tudelft/bebop2_undistort_front.xml
index e98b4e5ad5..303c50013b 100644
--- a/conf/airframes/tudelft/bebop2_undistort_front.xml
+++ b/conf/airframes/tudelft/bebop2_undistort_front.xml
@@ -122,7 +122,6 @@
 
   <section name="INS" prefix="INS_">
     <!--<define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/> -->
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
index 0ee6f95ded..188a34d766 100644
--- a/conf/airframes/tudelft/bebop_OF_hover.xml
+++ b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -119,7 +119,6 @@
 
   <section name="INS" prefix="INS_">
     <!-- define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/-->
     <!-- Optitrack -->
     <define name="USE_GPS_ALT" value="1"/>
     <define name="VFF_R_GPS" value="0.01"/>
diff --git a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
index feb8126bed..02dd78efec 100644
--- a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
+++ b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
@@ -117,7 +117,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="5"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid.xml b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
index 274f40f610..a50fe3e084 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
@@ -148,7 +148,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
index 46b5673b1d..b7ccb7b032 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
@@ -166,7 +166,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
diff --git a/conf/airframes/tudelft/bebop_flip.xml b/conf/airframes/tudelft/bebop_flip.xml
index 130e909af9..d986ff1843 100644
--- a/conf/airframes/tudelft/bebop_flip.xml
+++ b/conf/airframes/tudelft/bebop_flip.xml
@@ -123,7 +123,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/bebop_indi.xml b/conf/airframes/tudelft/bebop_indi.xml
index ff49376b2e..e33525f41f 100644
--- a/conf/airframes/tudelft/bebop_indi.xml
+++ b/conf/airframes/tudelft/bebop_indi.xml
@@ -103,7 +103,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
index 6343f43132..61c924f2e6 100644
--- a/conf/airframes/tudelft/bebop_indi_actuators.xml
+++ b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -94,7 +94,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/bebop_mavlink.xml b/conf/airframes/tudelft/bebop_mavlink.xml
index 9bf5cd3439..11a4d1bf67 100644
--- a/conf/airframes/tudelft/bebop_mavlink.xml
+++ b/conf/airframes/tudelft/bebop_mavlink.xml
@@ -133,7 +133,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
 
diff --git a/conf/airframes/tudelft/bebop_opticflow.xml b/conf/airframes/tudelft/bebop_opticflow.xml
index c616e408f0..84336b72e8 100644
--- a/conf/airframes/tudelft/bebop_opticflow.xml
+++ b/conf/airframes/tudelft/bebop_opticflow.xml
@@ -106,7 +106,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
     <!--define name="USE_HFF"/-->
   </section>
 
diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor.xml b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
index 1c91686818..b1de8b4a3a 100644
--- a/conf/airframes/tudelft/bebop_opticflow_indoor.xml
+++ b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
@@ -88,7 +88,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
index 6f503db686..5096947e52 100644
--- a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
+++ b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
@@ -163,7 +163,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
diff --git a/conf/airframes/tudelft/fan_demo.xml b/conf/airframes/tudelft/fan_demo.xml
index 647dfeb817..56dde27a4d 100644
--- a/conf/airframes/tudelft/fan_demo.xml
+++ b/conf/airframes/tudelft/fan_demo.xml
@@ -108,7 +108,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
 
     <!-- Use GPS altitude measurments and set the R gain -->
     <define name="USE_GPS_ALT" value="1"/>
diff --git a/conf/airframes/tudelft/iris_indi.xml b/conf/airframes/tudelft/iris_indi.xml
index 18b029981d..c0dcaa5262 100644
--- a/conf/airframes/tudelft/iris_indi.xml
+++ b/conf/airframes/tudelft/iris_indi.xml
@@ -164,7 +164,6 @@
   </section>
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2" />
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
   </section>
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
     <!-- setpoint limits for attitude stabilization rc flight -->
diff --git a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
index 604f733c05..9b546cdd10 100644
--- a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
+++ b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
@@ -178,7 +178,6 @@
     <define name="USE_GPS_SPEED" value="true"/>
     <define name="INS_VFF_R_GPS" value="0.1"/>
     <define name="INS_VFF_VZ_R_GPS" value="0.1"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="true"/>
   </section>
 
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml
index 458efd390e..c503a53fd6 100644
--- a/conf/airframes/tudelft/robird.xml
+++ b/conf/airframes/tudelft/robird.xml
@@ -150,7 +150,6 @@ Flapping wing frame equiped with
   </section>
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2" />
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
   </section>
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
     <!-- setpoint limits for attitude stabilization rc flight -->
diff --git a/conf/airframes/tudelft/splash.xml b/conf/airframes/tudelft/splash.xml
index 81bc177a23..cf739a992f 100644
--- a/conf/airframes/tudelft/splash.xml
+++ b/conf/airframes/tudelft/splash.xml
@@ -135,7 +135,6 @@
   </section>
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2" />
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
   </section>
   <section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
     <!-- setpoint limits for attitude stabilization rc flight -->
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 41b4298cb1..167f13e13d 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -79,11 +79,6 @@
 #define VFF_R_SONAR_OF_M 0.2
 #endif
 
-#ifndef INS_SONAR_UPDATE_ON_AGL
-#define INS_SONAR_UPDATE_ON_AGL FALSE
-PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
-#endif
-
 #endif // USE_SONAR
 
 #if USE_GPS
@@ -217,10 +212,6 @@ void ins_int_init(void)
   AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
   ins_int.baro_initialized = false;
 
-#if USE_SONAR
-  ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
-#endif
-
   ins_int.vf_reset = false;
   ins_int.hf_realign = false;
 
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
index e4f42b06a5..4ef20080d5 100644
--- a/sw/airborne/subsystems/ins/ins_int.h
+++ b/sw/airborne/subsystems/ins/ins_int.h
@@ -61,10 +61,6 @@ struct InsInt {
   float baro_z;  ///< z-position calculated from baro in meters (z-down)
   float qfe;
   bool baro_initialized;
-
-#if USE_SONAR
-  bool update_on_agl; ///< use sonar to update agl if available
-#endif
 };
 
 /** global INS state */