diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index cc6e349e3..d061589e1 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -78,6 +78,7 @@ volatile enum algorithms ahrs_algorithm;
  */
 struct mag_sensor {
     uint8_t id[4];
+	uint8_t updated;
     struct {
         int16_t axis[3];
     } raw;
@@ -296,6 +297,7 @@ int main()
 	// Get magnetic readings
 	if (PIOS_HMC5843_NewDataAvailable()) {
 		PIOS_HMC5843_ReadMag(mag_data.raw.axis);
+		mag_data.updated = 1;
 	}
 #endif        
         // Delay for valid data
@@ -358,12 +360,21 @@ int main()
 				// increase as data quality decreases but keep it bounded
 				// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
 				INSSetPosVelVar(0.004);
-				FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);        
+				if(gps_data.updated) {
+					//TOOD: add check for altitude updates
+					FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);        
+					gps_data.updated = 0;
+				} else {
+					GpsBaroCorrection(gps_data.NED,vel,altitude_data.altitude);
+				}
+
 				gps_data.updated = false;
+				mag_data.updated = 0;
             }
-            else if(gps_data.quality != -1)
+            else if(gps_data.quality != -1 && mag_data.updated == 1) {
                 MagCorrection(mag);  // only trust mags if outdoors
-			else {
+				mag_data.updated = 0;
+			} else {
 				// Indoors, update with zero position and velocity and high covariance
 				INSSetPosVelVar(0.1);
 				vel[0] = 0;
diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h
index 22f6d38a6..fb60ee188 100644
--- a/flight/AHRS/inc/insgps.h
+++ b/flight/AHRS/inc/insgps.h
@@ -61,6 +61,7 @@ void INSSetMagVar(float scaled_mag_var[3]);
 void MagCorrection(float mag_data[3]);
 void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
+void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
 void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
 void VelBaroCorrection(float Vel[3], float BaroAlt);
 
diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c
index 18766a747..e9ba5f167 100644
--- a/flight/AHRS/insgps.c
+++ b/flight/AHRS/insgps.c
@@ -183,6 +183,11 @@ void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
     INSCorrection(mag_data, zeros, Vel, BaroAlt, MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
 }
 
+void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
+{
+    INSCorrection(zeros, Pos, Vel, BaroAlt,  HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+}
+
 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt)
 {
     INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);