diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index 8a3103e9c..199f984fd 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -71,87 +71,68 @@ struct data {
     bool inited;
 
     PiOSDeltatimeConfig dtconfig;
+    bool navOnly;
 };
 
-// Private variables
-static bool initialized = 0;
-
 
 // Private functions
 
-static int32_t init13i(stateFilter *self);
-static int32_t init13(stateFilter *self);
-static int32_t maininit(stateFilter *self);
+static int32_t init(stateFilter *self);
 static filterResult filter(stateFilter *self, stateEstimation *state);
 static inline bool invalid_var(float data);
 
-static void globalInit(void);
+static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly);
 
 
-static void globalInit(void)
+static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly)
 {
-    if (!initialized) {
-        initialized = 1;
-        EKFConfigurationInitialize();
-        EKFStateVarianceInitialize();
-        HomeLocationInitialize();
-    }
+    handle->init      = &init;
+    handle->filter    = &filter;
+    handle->localdata = pios_malloc(sizeof(struct data));
+    struct data *this = (struct data *)handle->localdata;
+    this->usePos = usePos;
+    this->navOnly = navOnly;
+    EKFConfigurationInitialize();
+    EKFStateVarianceInitialize();
+    HomeLocationInitialize();
+    return STACK_REQUIRED;
 }
 
 int32_t filterEKF13iInitialize(stateFilter *handle)
 {
-    globalInit();
-    handle->init      = &init13i;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
+    return globalInit(handle, false, false);
 }
+
 int32_t filterEKF13Initialize(stateFilter *handle)
 {
-    globalInit();
-    handle->init      = &init13;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
+    return globalInit(handle, true, false);
 }
+
+int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle)
+{
+    return globalInit(handle, false, true);
+}
+
+int32_t filterEKF13NavOnlyInitialize(stateFilter *handle)
+{
+    return globalInit(handle, true, true);
+}
+
+#ifdef FALSE
 // XXX
 // TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
 // XXX
 int32_t filterEKF16iInitialize(stateFilter *handle)
 {
-    globalInit();
-    handle->init      = &init13i;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
+    return filterEKFi13Initialize(handle);
 }
 int32_t filterEKF16Initialize(stateFilter *handle)
 {
-    globalInit();
-    handle->init      = &init13;
-    handle->filter    = &filter;
-    handle->localdata = pios_malloc(sizeof(struct data));
-    return STACK_REQUIRED;
+    return filterEKF13Initialize(handle);
 }
+#endif
 
-
-static int32_t init13i(stateFilter *self)
-{
-    struct data *this = (struct data *)self->localdata;
-
-    this->usePos = 0;
-    return maininit(self);
-}
-
-static int32_t init13(stateFilter *self)
-{
-    struct data *this = (struct data *)self->localdata;
-
-    this->usePos = 1;
-    return maininit(self);
-}
-
-static int32_t maininit(stateFilter *self)
+static int32_t init(stateFilter *self)
 {
     struct data *this = (struct data *)self->localdata;
 
@@ -303,13 +284,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
             // Copy the attitude into the state
             // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
-            state->attitude[0] = Nav.q[0];
-            state->attitude[1] = Nav.q[1];
-            state->attitude[2] = Nav.q[2];
-            state->attitude[3] = Nav.q[3];
-            state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
-            state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
-            state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
+            if(!this->navOnly){
+                state->attitude[0] = Nav.q[0];
+                state->attitude[1] = Nav.q[1];
+                state->attitude[2] = Nav.q[2];
+                state->attitude[3] = Nav.q[3];
+
+                state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
+                state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
+                state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
+            }
             state->pos[0]   = Nav.Pos[0];
             state->pos[1]   = Nav.Pos[1];
             state->pos[2]   = Nav.Pos[2];
@@ -338,13 +322,21 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
     // Copy the attitude into the state
     // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
-    state->attitude[0] = Nav.q[0];
-    state->attitude[1] = Nav.q[1];
-    state->attitude[2] = Nav.q[2];
-    state->attitude[3] = Nav.q[3];
-    state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
-    state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
-    state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
+    if(!this->navOnly){
+
+        state->attitude[0] = Nav.q[0];
+        state->attitude[1] = Nav.q[1];
+        state->attitude[2] = Nav.q[2];
+        state->attitude[3] = Nav.q[3];
+        state->gyro[0]    -= RAD2DEG(Nav.gyro_bias[0]);
+        state->gyro[1]    -= RAD2DEG(Nav.gyro_bias[1]);
+        state->gyro[2]    -= RAD2DEG(Nav.gyro_bias[2]);
+    }
+    {
+        float tmp[3];
+        Quaternion2RPY(Nav.q, tmp);
+        state->debugNavYaw = tmp[2];
+    }
     state->pos[0]   = Nav.Pos[0];
     state->pos[1]   = Nav.Pos[1];
     state->pos[2]   = Nav.Pos[2];
diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index c34ba9140..7de46bb10 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/flight/modules/StateEstimation/inc/stateestimation.h
@@ -71,6 +71,7 @@ typedef struct {
     float   baro[1];
     float   auxMag[3];
     float   boardMag[3];
+    float   debugNavYaw;
     uint8_t magStatus;
     bool    armed;
     sensorUpdates updated;
@@ -95,6 +96,8 @@ int32_t filterCFInitialize(stateFilter *handle);
 int32_t filterCFMInitialize(stateFilter *handle);
 int32_t filterEKF13iInitialize(stateFilter *handle);
 int32_t filterEKF13Initialize(stateFilter *handle);
+int32_t filterEKF13NavOnlyInitialize(stateFilter *handle);
+int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle);
 int32_t filterEKF16iInitialize(stateFilter *handle);
 int32_t filterEKF16Initialize(stateFilter *handle);
 
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index a727e215e..9d2afd09f 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -157,6 +157,8 @@ static stateFilter cfFilter;
 static stateFilter cfmFilter;
 static stateFilter ekf13iFilter;
 static stateFilter ekf13Filter;
+static stateFilter ekf13iNavFilter;
+static stateFilter ekf13NavFilter;
 
 // this is a hack to provide a computational shortcut for faster gyro state progression
 static float gyroRaw[3];
@@ -260,7 +262,30 @@ static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) {
             .next   = &(filterPipeline) {
                 .filter = &baroFilter,
                 .next   = &(filterPipeline) {
-                    .filter = &ekf13Filter,
+                    .filter = &ekf13NavFilter,
+                    .next   = &(filterPipeline) {
+                        .filter = &velocityFilter,
+                        .next   = &(filterPipeline) {
+                            .filter = &cfmFilter,
+                            .next   = NULL,
+                        }
+                    }
+                }
+            }
+        }
+    }
+};
+
+static const filterPipeline *ekf13iNavCFAttQueue = &(filterPipeline) {
+    .filter = &magFilter,
+    .next   = &(filterPipeline) {
+        .filter = &airFilter,
+        .next   = &(filterPipeline) {
+            .filter = &baroiFilter,
+            .next   = &(filterPipeline) {
+                .filter = &stationaryFilter,
+                .next   = &(filterPipeline) {
+                    .filter = &ekf13iNavFilter,
                     .next   = &(filterPipeline) {
                         .filter = &velocityFilter,
                         .next   = &(filterPipeline) {
@@ -343,6 +368,8 @@ int32_t StateEstimationInitialize(void)
     stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
     stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
     stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
+    stack_required = maxint32_t(stack_required, filterEKF13NavOnlyInitialize(&ekf13NavFilter));
+    stack_required = maxint32_t(stack_required, filterEKF13iNavOnlyInitialize(&ekf13iNavFilter));
 
     stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required);
 
@@ -424,12 +451,16 @@ static void StateEstimationCb(void)
             case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:
                 newFilterChain = ekf13NavCFAttQueue;
                 break;
+            case REVOSETTINGS_FUSIONALGORITHM_TESTINGINSINDOORCF:
+                newFilterChain = ekf13iNavCFAttQueue;
+                break;
             default:
                 newFilterChain = NULL;
             }
             // initialize filters in chain
             current = newFilterChain;
             bool error = 0;
+            states.debugNavYaw = 0;
             while (current != NULL) {
                 int32_t result = current->filter->init((stateFilter *)current->filter);
                 if (result != 0) {
@@ -524,6 +555,7 @@ static void StateEstimationCb(void)
         s.q3 = states.attitude[2];
         s.q4 = states.attitude[3];
         Quaternion2RPY(&s.q1, &s.Roll);
+        s.NavYaw = states.debugNavYaw;
         AttitudeStateSet(&s);
     }
     // throttle alarms, raise alarm flags immediately
diff --git a/shared/uavobjectdefinition/attitudestate.xml b/shared/uavobjectdefinition/attitudestate.xml
index 0d5dc0c08..045f8bc22 100644
--- a/shared/uavobjectdefinition/attitudestate.xml
+++ b/shared/uavobjectdefinition/attitudestate.xml
@@ -8,6 +8,7 @@
         <field name="Roll" units="degrees" type="float" elements="1"/>
         <field name="Pitch" units="degrees" type="float" elements="1"/>
         <field name="Yaw" units="degrees" type="float" elements="1"/>
+        <field name="NavYaw" units="degrees" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="130"/>
diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml
index a99ad9c52..2cddb0f9d 100644
--- a/shared/uavobjectdefinition/revosettings.xml
+++ b/shared/uavobjectdefinition/revosettings.xml
@@ -2,7 +2,7 @@
     <object name="RevoSettings" singleinstance="true" settings="true" category="State">
         <description>Settings for the revo to control the algorithm and what is updated</description>
         <field name="FusionAlgorithm" units="" type="enum" elements="1" 
-        options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF)" 
+        options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF)" 
         defaultvalue="Basic (Complementary)"/>
 
         <!-- Low pass filter configuration to calculate offset of barometric altitude sensor.