diff --git a/conf/airframes/CDW/ChimuLisaFw.xml b/conf/airframes/CDW/ChimuLisaFw.xml
index a47ba13623..91e3c9b9b2 100644
--- a/conf/airframes/CDW/ChimuLisaFw.xml
+++ b/conf/airframes/CDW/ChimuLisaFw.xml
@@ -144,7 +144,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -157,7 +156,6 @@
     <!-- Sensors -->
     <subsystem name="gps" 		    type="ublox_utm"/>
     <subsystem name="navigation"/>
-
   </firmware>
 
 
diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml
index ac33640f6f..97502e621b 100644
--- a/conf/airframes/CDW/ChimuTinyFwSpi.xml
+++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml
@@ -163,14 +163,12 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
 			<!--      <define name="NB_CHANNELS" value="5" />  -->
     </target>
     <target name="sim" board="pc"/>
 
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="gps" type="ublox" />
-
     <!-- Communication -->
     <subsystem name="telemetry" type="xbee_api">
       <configure name="MODEM_BAUD" value="B9600"/>
diff --git a/conf/airframes/CDW/LisaAspirin2.xml b/conf/airframes/CDW/LisaAspirin2.xml
index aef11dfc20..6817edb260 100644
--- a/conf/airframes/CDW/LisaAspirin2.xml
+++ b/conf/airframes/CDW/LisaAspirin2.xml
@@ -214,7 +214,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
 
     <!-- Sensors -->
     <!--
@@ -227,6 +226,7 @@
       <configure name="USE_MAGNETOMETER" value="0"/>
     </subsystem>
 
+    <subsystem name="ins" type="alt_float"/>
 
     <subsystem name="radio_control" type="ppm"/>
 
diff --git a/conf/airframes/CDW/debug_i2c_lpc.xml b/conf/airframes/CDW/debug_i2c_lpc.xml
index a93292c82a..01233bd41d 100644
--- a/conf/airframes/CDW/debug_i2c_lpc.xml
+++ b/conf/airframes/CDW/debug_i2c_lpc.xml
@@ -178,7 +178,6 @@
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
     </target>
     <target name="sim" board="pc"/>
 
diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml
index edfe73741c..152832dd54 100644
--- a/conf/airframes/CDW/yapa3_aspirin2.xml
+++ b/conf/airframes/CDW/yapa3_aspirin2.xml
@@ -238,7 +238,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
 
     <!-- Sensors -->
     <!--
@@ -252,7 +251,7 @@
 <!--      <configure name="USE_MAGNETOMETER" /> -->
     </subsystem>
 
-
+    <subsystem name="ins" type="alt_float"/>
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Communication -->
diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml
index 2df423d061..4c8cc21882 100644
--- a/conf/airframes/ENAC/fixed-wing/firestorm.xml
+++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml
@@ -14,7 +14,6 @@
 
   <firmware name="fixedwing">
     <define name="AGR_CLIMB"/>
-    <define name="ALT_KALMAN"/>
     <define name="LOITER_TRIM"/>
     <!--define name="PITCH_TRIM"/-->
 
@@ -37,6 +36,7 @@
     <subsystem name="ahrs"    type="float_dcm">
       <define name="USE_HIGH_ACCEL_FLAG"/>
     </subsystem>
+    <subsystem name="ins" type="alt_float"/>
     <subsystem name="control" type="adaptive"/>
     <subsystem name="navigation"/>
     <!-- Sensors -->
diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml
index d886ac8dd5..f34fd0b78d 100644
--- a/conf/airframes/ENAC/fixed-wing/weasel.xml
+++ b/conf/airframes/ENAC/fixed-wing/weasel.xml
@@ -18,7 +18,6 @@
     <define name="USE_I2C0"/>
     <define name="USE_I2C1"/>
     <!--define name="AGR_CLIMB"/-->
-    <define name="ALT_KALMAN"/>
     <define name="USE_AIRSPEED"/>
     <!--define name="LOITER_TRIM"/-->
     <!--define name="PITCH_TRIM"/-->
@@ -38,6 +37,7 @@
     <subsystem name="ahrs"    type="float_dcm">
       <define name="USE_HIGH_ACCEL_FLAG"/>
     </subsystem>
+    <subsystem name="ins" typee="alt_float"/>
     <subsystem name="control" type="new"/>
     <subsystem name="navigation"/>
     <!-- Sensors -->
diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index df27f03f3d..243ac30c82 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -6,12 +6,10 @@
     <target name="sim" 			board="pc">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
     <target name="ap" 			board="tiny_0.99">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
 
     <subsystem name="radio_control" type="ppm"/>
@@ -26,7 +24,7 @@
     <subsystem name="ahrs" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox_utm"/>
     <subsystem name="navigation"/>
-
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/LAAS/mmlaas_N2.xml b/conf/airframes/LAAS/mmlaas_N2.xml
index 8eab40a5b3..f20510c9a8 100644
--- a/conf/airframes/LAAS/mmlaas_N2.xml
+++ b/conf/airframes/LAAS/mmlaas_N2.xml
@@ -6,12 +6,10 @@
     <target name="sim" 			board="pc">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
     <target name="ap" 			board="tiny_1.1">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
 
     <subsystem name="radio_control" type="ppm"/>
@@ -26,7 +24,7 @@
     <subsystem name="ahrs" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox_utm"/>
     <subsystem name="navigation"/>
-
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/LAAS/mmlaas_N3.xml b/conf/airframes/LAAS/mmlaas_N3.xml
index 9bac5b5830..2a601ee6af 100644
--- a/conf/airframes/LAAS/mmlaas_N3.xml
+++ b/conf/airframes/LAAS/mmlaas_N3.xml
@@ -6,12 +6,10 @@
     <target name="sim" 			board="pc">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
     <target name="ap" 			board="tiny_1.1">
       <define name="AGR_CLIMB" />
       <define name="LOITER_TRIM" />
-      <define name="ALT_KALMAN" />
     </target>
 
     <subsystem name="radio_control" type="ppm"/>
@@ -26,7 +24,7 @@
     <subsystem name="ahrs" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox_utm"/>
     <subsystem name="navigation"/>
-
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 54b0bc2af9..5560d16184 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -230,7 +230,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
 
     <!-- Sensors -->
     <!--
@@ -243,7 +242,7 @@
     <subsystem name="ahrs" type="float_dcm">
 <!--      <configure name="USE_MAGNETOMETER" /> -->
     </subsystem>
-
+    <subsystem name="ins" type="alt_float"/>
 
     <subsystem name="radio_control" type="ppm"/>
 
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_fw.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_fw.xml
index ed43bd8e61..95b1c5f8c9 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_fw.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_fw.xml
@@ -25,6 +25,7 @@
         </subsystem>
         <subsystem name="ahrs" type="float_dcm"/>
         <subsystem name="navigation"/>
+        <subsystem name="ins" type="alt_float"/>
     </firmware>
 
     <servos>
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_fw.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_fw.xml
index b2aea018a2..e692627b75 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_fw.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_fw.xml
@@ -24,6 +24,7 @@
             <configure name="GPS_BAUD"    value="B57600"/>
         </subsystem>
         <subsystem name="ahrs" type="float_dcm"/>
+        <subsystem name="ins" type="alt_float"/>
         <subsystem name="navigation"/>
     </firmware>
 
diff --git a/conf/airframes/examples/bixler_lisa_m_2.xml b/conf/airframes/examples/bixler_lisa_m_2.xml
index 92e8ad07b2..3436277918 100644
--- a/conf/airframes/examples/bixler_lisa_m_2.xml
+++ b/conf/airframes/examples/bixler_lisa_m_2.xml
@@ -21,7 +21,6 @@
 
     <target name="sim" 			board="pc"/>
 
-    <define name="ALT_KALMAN"/>
     <define name="USE_MAGNETOMETER" value="FALSE"/>
 
     <subsystem name="radio_control" type="ppm"/>
@@ -31,6 +30,7 @@
     <subsystem name="ahrs" type="float_dcm"/>
     <subsystem name="gps" type="ublox"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <!-- commands section -->
diff --git a/conf/airframes/examples/easy_glider.xml b/conf/airframes/examples/easy_glider.xml
index e4573550a2..65c1d4799f 100644
--- a/conf/airframes/examples/easy_glider.xml
+++ b/conf/airframes/examples/easy_glider.xml
@@ -160,7 +160,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -175,6 +174,7 @@
     <subsystem name="ahrs" 		type="infrared"/>
     <subsystem name="gps" 		    type="ublox_utm"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/examples/easystar.xml b/conf/airframes/examples/easystar.xml
index 81d1dc49a9..2ff5df6cb0 100644
--- a/conf/airframes/examples/easystar.xml
+++ b/conf/airframes/examples/easystar.xml
@@ -10,15 +10,10 @@
 <airframe name="EasyStar TWOGv1">
 
   <firmware name="fixedwing">
-    <target name="ap" 			board="twog_1.0">
-      <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
-    </target>
+    <target name="ap" 			board="twog_1.0"/>
+    <target name="sim" 			board="pc"/>
 
-    <target name="sim" 			board="pc">
-      <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
-    </target>
+    <define name="LOITER_TRIM"/>
 
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="telemetry" type="transparent"/>
@@ -26,6 +21,7 @@
     <subsystem name="ahrs" type="infrared"/>
     <subsystem name="gps" type="ublox"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/examples/easystar_ets.xml b/conf/airframes/examples/easystar_ets.xml
index 0fc006f3f1..a66406c0c7 100644
--- a/conf/airframes/examples/easystar_ets.xml
+++ b/conf/airframes/examples/easystar_ets.xml
@@ -16,7 +16,6 @@
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
     <define name="WIND_INFO"/>
-    <define name="ALT_KALMAN"/>
     <define name="USE_I2C0"/>
     <define name="USE_AIRSPEED"/>
     <define name="USE_BARO_ETS"/>
diff --git a/conf/airframes/examples/funjet.xml b/conf/airframes/examples/funjet.xml
index d67a578bae..824ac1b0a6 100644
--- a/conf/airframes/examples/funjet.xml
+++ b/conf/airframes/examples/funjet.xml
@@ -16,7 +16,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
 
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="joystick"/>
@@ -111,7 +110,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/examples/funjet_cam.xml b/conf/airframes/examples/funjet_cam.xml
index 7d3ba6ceb1..befcb47f0c 100644
--- a/conf/airframes/examples/funjet_cam.xml
+++ b/conf/airframes/examples/funjet_cam.xml
@@ -16,7 +16,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
 
     <subsystem name="radio_control" type="ppm"/>
     <subsystem name="joystick"/>
@@ -120,7 +119,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/examples/microjet.xml b/conf/airframes/examples/microjet.xml
index 032cc32e89..a569c74d8c 100644
--- a/conf/airframes/examples/microjet.xml
+++ b/conf/airframes/examples/microjet.xml
@@ -170,7 +170,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -187,7 +186,7 @@
     <subsystem name="ahrs"      type="infrared"/>
     <subsystem name="gps"       type="ublox_utm"/>
     <subsystem name="navigation"/>
-
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <modules>
diff --git a/conf/airframes/examples/microjet_imu_xsens.xml b/conf/airframes/examples/microjet_imu_xsens.xml
index 990753c737..e7c7f47961 100644
--- a/conf/airframes/examples/microjet_imu_xsens.xml
+++ b/conf/airframes/examples/microjet_imu_xsens.xml
@@ -14,7 +14,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -30,6 +29,7 @@
     <subsystem name="gps" type="ublox"/>
     <subsystem name="control"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <modules>
@@ -197,7 +197,6 @@
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
-    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
   </section>
 
diff --git a/conf/airframes/examples/microjet_lisa_m.xml b/conf/airframes/examples/microjet_lisa_m.xml
index 52c29d6572..7471c1acda 100644
--- a/conf/airframes/examples/microjet_lisa_m.xml
+++ b/conf/airframes/examples/microjet_lisa_m.xml
@@ -14,7 +14,6 @@
 
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
     <define name="STRONG_WIND"/>
 
     <subsystem name="radio_control" type="ppm"/>
diff --git a/conf/airframes/examples/microjet_lisa_m_xsens.xml b/conf/airframes/examples/microjet_lisa_m_xsens.xml
index 181fe7a979..824f7598de 100644
--- a/conf/airframes/examples/microjet_lisa_m_xsens.xml
+++ b/conf/airframes/examples/microjet_lisa_m_xsens.xml
@@ -14,7 +14,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -29,6 +28,7 @@
 
     <subsystem name="control"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <modules>
diff --git a/conf/airframes/examples/twinjet.xml b/conf/airframes/examples/twinjet.xml
index 9c8d125a98..801f521753 100644
--- a/conf/airframes/examples/twinjet.xml
+++ b/conf/airframes/examples/twinjet.xml
@@ -15,7 +15,6 @@
     <target name="sim" 			board="pc"/>
 
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
     <define name="AGR_CLIMB"/>
 
     <subsystem name="radio_control" type="ppm"/>
diff --git a/conf/airframes/examples/twinstar.xml b/conf/airframes/examples/twinstar.xml
index 777de7ff05..7d7803a210 100644
--- a/conf/airframes/examples/twinstar.xml
+++ b/conf/airframes/examples/twinstar.xml
@@ -16,7 +16,6 @@
     <target name="sim" 			board="pc"/>
 
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
     <define name="AGR_CLIMB"/>
 
     <subsystem name="radio_control" type="ppm"/>
@@ -127,7 +126,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/examples/twog_analogimu.xml b/conf/airframes/examples/twog_analogimu.xml
index 099a17ae9e..f93a178822 100644
--- a/conf/airframes/examples/twog_analogimu.xml
+++ b/conf/airframes/examples/twog_analogimu.xml
@@ -210,6 +210,7 @@
     <subsystem name="ahrs"  type="float_dcm"/>
     <subsystem name="gps"       type="ublox"/>
     <subsystem name="navigation"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
 </airframe>
diff --git a/conf/airframes/examples/umarim_lite_v2.xml b/conf/airframes/examples/umarim_lite_v2.xml
index a0dd542f26..3903337355 100644
--- a/conf/airframes/examples/umarim_lite_v2.xml
+++ b/conf/airframes/examples/umarim_lite_v2.xml
@@ -11,7 +11,6 @@
     <define name="USE_I2C0"/>
     <define name="USE_I2C1"/>
     <define name="AGR_CLIMB" />
-    <define name="ALT_KALMAN"/>
 
     <target name="sim" board="pc"/>
     <target name="ap" board="umarim_lite_2.0"/>
@@ -28,6 +27,7 @@
     <subsystem name="navigation"/>
     <!-- Sensors -->
     <subsystem name="gps" type="ublox"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
 
diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml
index 93df13fea6..ffaad6212b 100644
--- a/conf/airframes/flixr_discovery.xml
+++ b/conf/airframes/flixr_discovery.xml
@@ -27,7 +27,6 @@
     <define name="USE_GYRO_PITCH_RATE"/>
 
     <define name="WIND_INFO"/>
-    <define name="ALT_KALMAN"/>
 
     <!-- Communication -->
     <subsystem name="telemetry" type="transparent"/>
@@ -42,6 +41,7 @@
       <configure name="USE_MAGNETOMETER" value="0"/>
       <define name="AHRS_USE_GPS_HEADING" value="1"/>
     </subsystem>
+    <subsystem name="ins" type="alt_float"/>
 
     <!-- Sensors -->
     <subsystem name="imu" type="b2_v1.0">
diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index 588b36009a..3b7550bcc8 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -194,7 +194,6 @@
 
     <define name="AGR_CLIMB" />
     <define name="LOITER_TRIM" />
-    <define name="ALT_KALMAN" />
 
     <subsystem name="radio_control" type="ppm"/>
 
diff --git a/conf/airframes/mentor_tum.xml b/conf/airframes/mentor_tum.xml
index 81e0bb2323..25198861d3 100644
--- a/conf/airframes/mentor_tum.xml
+++ b/conf/airframes/mentor_tum.xml
@@ -17,7 +17,6 @@
     <define name="AGR_CLIMB"/>
     <define name="LOITER_TRIM"/>
     <define name="WIND_INFO"/>
-    <define name="ALT_KALMAN"/>
 
     <subsystem name="radio_control" type="ppm"/>
     <!--<define name="TELEMETRY_MODE_FBW" value="1"/>-->
@@ -26,6 +25,7 @@
     <subsystem name="ahrs" type="infrared"/>
     <subsystem name="gps" type="ublox"/>
     <subsystem name="navigation" type="extra"/>
+    <subsystem name="ins" type="alt_float"/>
   </firmware>
 
   <firmware name="setup">
diff --git a/conf/airframes/mm/fixed-wing/drops.xml b/conf/airframes/mm/fixed-wing/drops.xml
index 5c2775bf16..dd0993ade8 100644
--- a/conf/airframes/mm/fixed-wing/drops.xml
+++ b/conf/airframes/mm/fixed-wing/drops.xml
@@ -14,7 +14,6 @@
     <target name="ap" 			board="tiny_2.1">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
     </target>
@@ -137,7 +136,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/mm/fixed-wing/funjetdcb.xml b/conf/airframes/mm/fixed-wing/funjetdcb.xml
index 6a27e66e52..98328417bc 100644
--- a/conf/airframes/mm/fixed-wing/funjetdcb.xml
+++ b/conf/airframes/mm/fixed-wing/funjetdcb.xml
@@ -13,7 +13,6 @@
     <target name="ap"                  board="tiny_2.11">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
     </target>
@@ -21,7 +20,6 @@
     <target name="sim"                 board="pc">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
     </target>
@@ -132,7 +130,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi3.xml b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
index 2b36bd7e18..9118e22139 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi3.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
@@ -91,7 +91,6 @@
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
     <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
 <!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="150."/>
diff --git a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
index 9625176fc9..8896ce084a 100644
--- a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
+++ b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
@@ -12,7 +12,6 @@
     <target name="sim"                 board="pc">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
     </target>
@@ -20,7 +19,6 @@
     <target name="ap"                  board="tiny_2.11">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="USE_I2C0"/>
@@ -124,7 +122,6 @@
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
 <!--    <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/> -->
 <!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/mm/fixed-wing/merlin.xml b/conf/airframes/mm/fixed-wing/merlin.xml
index 920af2b49e..e0ea7c3b1c 100644
--- a/conf/airframes/mm/fixed-wing/merlin.xml
+++ b/conf/airframes/mm/fixed-wing/merlin.xml
@@ -89,7 +89,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
@@ -167,7 +166,6 @@
     <target name="sim"                 board="pc"/>
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
       <define name="USE_I2C0"/>
diff --git a/conf/airframes/mm/fixed-wing/slowfast.xml b/conf/airframes/mm/fixed-wing/slowfast.xml
index d2288298db..dab01b605b 100644
--- a/conf/airframes/mm/fixed-wing/slowfast.xml
+++ b/conf/airframes/mm/fixed-wing/slowfast.xml
@@ -14,7 +14,6 @@
     <target name="ap" 			board="tiny_2.1">
       <define name="AGR_CLIMB"/>
       <define name="LOITER_TRIM"/>
-      <define name="ALT_KALMAN"/>
       <define name="WIND_INFO"/>
       <define name="WIND_INFO_RET"/>
     </target>
@@ -121,7 +120,6 @@
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
 <!--    <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/> -->
 <!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/airframes/twinjet_overo.xml b/conf/airframes/twinjet_overo.xml
index 48ece3020e..a3b66c33cc 100644
--- a/conf/airframes/twinjet_overo.xml
+++ b/conf/airframes/twinjet_overo.xml
@@ -18,7 +18,6 @@
     <target name="sim"          board="pc"/>
 
     <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN"/>
     <define name="AGR_CLIMB"/>
 
     <subsystem name="radio_control" type="ppm"/>
diff --git a/conf/airframes/usb_test.xml b/conf/airframes/usb_test.xml
index 1b7deee367..f0b73c12a2 100644
--- a/conf/airframes/usb_test.xml
+++ b/conf/airframes/usb_test.xml
@@ -109,7 +109,6 @@
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
-    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
diff --git a/conf/firmwares/fixedwing.xml b/conf/firmwares/fixedwing.xml
index ad2527f967..f717d276b6 100644
--- a/conf/firmwares/fixedwing.xml
+++ b/conf/firmwares/fixedwing.xml
@@ -7,7 +7,6 @@
    <define name="AGR_CLIMB" />
    <define name="TRAFFIC_INFO"/>
    <define name="LOITER_TRIM"/>
-   <define name="ALT_KALMAN"/>
  </target>
 
  <target name="sim" description="The simulator">
diff --git a/conf/settings/estimation/fw_baro_kalman.xml b/conf/settings/estimation/fw_baro_kalman.xml
index 66012cb48d..f53112aca2 100644
--- a/conf/settings/estimation/fw_baro_kalman.xml
+++ b/conf/settings/estimation/fw_baro_kalman.xml
@@ -6,7 +6,7 @@
   <dl_settings>
 
     <dl_settings NAME="alt_baro">
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="subsystems/ins/ins_float"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="subsystems/ins/ins_alt_float"/>
     </dl_settings>
 
   </dl_settings>
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 211b317117..e7f71534d5 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -374,9 +374,6 @@ void handle_ins_msg(void) {
 #endif /* USE_IMU */
 
 #if USE_GPS_XSENS
-  #ifndef ALT_KALMAN
-  #warning NO_VZ
-  #endif
 
   // Horizontal speed
   float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c
index a407e14889..1d5441c09b 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.c
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.c
@@ -142,8 +142,6 @@ void pbn_read_event( void ) {
 #if USE_AIRSPEED
       stateSetAirspeed_f(&pbn_airspeed);
 #endif
-      //alt_kalman(pbn_altitude);
-
     }
 
   }
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
index bbcbe6e293..79ca2163d0 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.c
+++ b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -58,9 +58,7 @@ void ins_init() {
 
   stateSetPositionUtm_f(&utm0);
 
-#ifdef ALT_KALMAN
   alt_kalman_init();
-#endif
 
 #if USE_BAROMETER
   ins_qfe = 0;;
@@ -137,8 +135,6 @@ void ins_update_sonar() {
 
 bool_t alt_kalman_enabled;
 
-#ifdef ALT_KALMAN
-
 #ifndef ALT_KALMAN_ENABLED
 #define ALT_KALMAN_ENABLED FALSE
 #endif
@@ -232,5 +228,3 @@ void alt_kalman(float z_meas) {
 #endif
 }
 
-#endif // ALT_KALMAN
-
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h
index 4304b3cb4a..499d748adb 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.h
+++ b/sw/airborne/subsystems/ins/ins_alt_float.h
@@ -56,13 +56,9 @@ extern float estimator_z; ///< altitude above MSL in meters
 extern float estimator_z_dot;
 
 extern bool_t alt_kalman_enabled;
-#ifdef ALT_KALMAN
 extern void alt_kalman_reset( void );
 extern void alt_kalman_init( void );
 extern void alt_kalman( float );
-#endif
-
-#ifdef ALT_KALMAN
 
 #if USE_BAROMETER
 /* Kalman filter cannot be disabled in this mode (no z_dot) */
@@ -77,8 +73,4 @@ extern void alt_kalman( float );
 }
 #endif /* ! USE_BAROMETER */
 
-#else /* ALT_KALMAN */
-#define EstimatorSetAlt(z) { estimator_z = z; }
-#endif
-
 #endif /* INS_FLOAT_H */