diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c
index 0d4eba473..53da3a3b1 100644
--- a/flight/AHRS/ahrs.c
+++ b/flight/AHRS/ahrs.c
@@ -361,8 +361,10 @@ int main()
             mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
             mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
             
-            INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
-            
+            INSStatePrediction(gyro, accel, 1 / (float) EKF_RATE);
+			process_spi_request();
+			INSCovariancePrediction(1 / (float) EKF_RATE);
+			
             if ( gps_data.updated && gps_data.quality == 1) 
             {
 				// Compute velocity from Heading and groundspeed
@@ -418,6 +420,8 @@ int main()
             attitude_data.quaternion.q2 = q[1];
             attitude_data.quaternion.q3 = q[2];
             attitude_data.quaternion.q4 = q[3];
+			process_spi_request();
+
         }
         
         ahrs_state = AHRS_IDLE;
@@ -455,7 +459,6 @@ int main()
         }
 #endif
 		
-		process_spi_request();
     }
     
     return 0;
@@ -644,7 +647,8 @@ void converge_insgps()
         mag[2] = -mag_data.raw.axis[2];
         
         RPY2Quaternion(rpy,Nav.q);
-        INSPrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
+        INSStatePrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
+		INSCovariancePrediction(1 / (float) EKF_RATE);
         FullCorrection(mag,pos,vel,BaroAlt); 
         process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
     } 
diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h
index fb60ee188..51fea79d7 100644
--- a/flight/AHRS/inc/insgps.h
+++ b/flight/AHRS/inc/insgps.h
@@ -50,7 +50,9 @@
 
 //  Exposed Function Prototypes
 void INSGPSInit();
-void INSPrediction(float gyro_data[3], float accel_data[3], float dT);
+void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
+void INSCovariancePrediction(float dT); 
+
 void INSSetPosVelVar(float PosVar);
 void INSSetGyroBias(float gyro_bias[3]);
 void INSSetAccelVar(float accel_var[3]);
diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c
index e9ba5f167..190df0bf6 100644
--- a/flight/AHRS/insgps.c
+++ b/flight/AHRS/insgps.c
@@ -136,7 +136,7 @@ void INSSetMagNorth(float B[3])
   Be[2] = B[2];
 }
 
-void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
+void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
 {
 	float U[6];
 	float qmag;
@@ -156,7 +156,7 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
     RungeKutta(X,U,dT);
     qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]);
     X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag;
-	CovariancePrediction(F,G,Q,dT,P);
+	//CovariancePrediction(F,G,Q,dT,P);
 
     // Update Nav solution structure
     Nav.Pos[0] = X[0];
@@ -171,6 +171,11 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
     Nav.q[3] = X[9];
 }
 
+void INSCovariancePrediction(float dT)
+{
+	CovariancePrediction(F,G,Q,dT,P);
+}
+
 float zeros[3] = {0,0,0};
 
 void MagCorrection(float mag_data[3])