diff --git a/flight/INS/ins.c b/flight/INS/ins.c
index 85ef155c1..c6ace4e0e 100644
--- a/flight/INS/ins.c
+++ b/flight/INS/ins.c
@@ -135,13 +135,14 @@ float pressure, altitude;
 
 int32_t dr;
 
+static volatile bool init_algorithm = false;
+
 int32_t sclk, sclk_prev;
 int32_t sclk_count;
 uint32_t loop_time;
 int main()
 {	
 	gps_data.quality = -1;
-	static int8_t last_ahrs_algorithm;
 	ahrs_algorithm = INSSETTINGS_ALGORITHM_SIMPLE;
 	
 	reset_values();
@@ -195,7 +196,12 @@ int main()
 	HomeLocationConnectCallback(homelocation_callback);
 	//FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
 	
+	get_accel_gyro_data();   // This function blocks till data avilable
+	get_mag_data();
+	get_baro_data();
+
 	settings_callback(InsSettingsHandle());
+	ins_init_algorithm(); 
 	
 	/******************* Main EKF loop ****************************/
 	while(1) {
@@ -228,9 +234,10 @@ int main()
 		//print_ekf_binary();
 		
 		/* If algorithm changed reinit.  This could go in callback but wouldn't be synchronous */
-		if (ahrs_algorithm != last_ahrs_algorithm)
+		if (init_algorithm) {
 			ins_init_algorithm(); 
-		last_ahrs_algorithm = ahrs_algorithm; 
+			init_algorithm = false;
+		}
 		
 		time_val2 = PIOS_DELAY_GetRaw();
 		
@@ -645,6 +652,7 @@ void settings_callback(AhrsObjHandle obj)
 	InsSettingsData settings;
 	InsSettingsGet(&settings);
 
+	init_algorithm = ahrs_algorithm != settings.Algorithm;
 	ahrs_algorithm = settings.Algorithm;
 	bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE;
 
diff --git a/flight/INS/insgps_helper.c b/flight/INS/insgps_helper.c
index e84969d8f..910fdef79 100644
--- a/flight/INS/insgps_helper.c
+++ b/flight/INS/insgps_helper.c
@@ -232,8 +232,10 @@ void ins_indoor_update()
 /**
  * @brief Initialize the EKF assuming stationary
  */
+bool inited = false;
 void ins_init_algorithm()
 {
+	inited = true;
 	float Rbe[3][3], q[4], accels[3], rpy[3], mag;
 	float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
 	bool using_mags, using_gps;