diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c
index 1c959f94c..eb3fe1b75 100644
--- a/flight/modules/Altitude/revolution/altitude.c
+++ b/flight/modules/Altitude/revolution/altitude.c
@@ -53,7 +53,8 @@
 
 // Interval in number of sample to recalculate temp bias
 #define TEMP_CALIB_INTERVAL 10
-#define TEMP_ALPHA          0.9f
+// 5Hz @120Hz update
+#define TEMP_ALPHA          0.9979f
 
 // Private types
 
@@ -64,7 +65,7 @@ static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
 static volatile bool tempCorrectionEnabled;
 
 static float baro_temp_bias   = 0;
-static float baro_temperature = 0;
+static float baro_temperature = NAN;
 static uint8_t temp_calibration_count = 0;
 
 // Private functions
@@ -176,6 +177,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
         temp  = PIOS_MS5611_GetTemperature();
         press = PIOS_MS5611_GetPressure();
 
+        if (isnan(baro_temperature)) {
+            baro_temperature = temp;
+        }
+
         baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp;
 
         if (tempCorrectionEnabled && !temp_calibration_count) {
diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c
index 6cfea7e34..d79699d65 100644
--- a/flight/modules/Attitude/attitude.c
+++ b/flight/modules/Attitude/attitude.c
@@ -89,7 +89,9 @@ PERF_DEFINE_COUNTER(counterAtt);
 
 // Interval in number of sample to recalculate temp bias
 #define TEMP_CALIB_INTERVAL 30
-#define TEMP_ALPHA          0.9f
+
+// LPF 5Hz at 500Hz rate
+#define TEMP_ALPHA          0.999504f
 
 #define UPDATE_EXPECTED     (1.0f / 500.0f)
 #define UPDATE_MIN          1.0e-6f
@@ -136,7 +138,7 @@ static bool apply_accel_temp = false;
 static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
 static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
 static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
-static float temperature = 0;
+static float temperature = NAN;
 static float accel_temp_bias[3] = { 0 };
 static float gyro_temp_bias[3] = { 0 };
 static uint8_t temp_calibration_count = 0;
@@ -499,11 +501,14 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     accels[2]  *= accel_scale.Z * invcount;
     temp *= invcount;
 
+    if(isnan(temperature)){
+        temperature = temp;
+    }
     temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp;
 
     if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
         temp_calibration_count = TEMP_CALIB_INTERVAL;
-        float ctemp = boundf(temp, temp_calibrated_extent.max, temp_calibrated_extent.min);
+        float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min);
         if (apply_gyro_temp) {
             gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
             gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
index 608741423..67b063b24 100644
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@@ -70,7 +70,8 @@
 
 // Interval in number of sample to recalculate temp bias
 #define TEMP_CALIB_INTERVAL 30
-#define TEMP_ALPHA          0.9f
+// LPF 5Hz at 500Hz rate
+#define TEMP_ALPHA          0.999504f
 
 #define ZERO_ROT_ANGLE      0.00001f
 // Private types
@@ -108,8 +109,8 @@ static float mag_transform[3][3] = {
 static volatile bool gyro_temp_calibrated  = false;
 static volatile bool accel_temp_calibrated = false;
 
-static float accel_temperature  = 0;
-static float gyro_temperature   = 0;
+static float accel_temperature  = NAN;
+static float gyro_temperature   = NAN;
 static float accel_temp_bias[3] = { 0 };
 static float gyro_temp_bias[3] = { 0 };
 static uint8_t temp_calibration_count = 0;
@@ -373,9 +374,14 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             PIOS_DEBUG_Assert(0);
         }
 
-        accel_temperature = TEMP_ALPHA * accelSensorData.temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature;
-        gyro_temperature  = TEMP_ALPHA * gyroSensorData.temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature;
+        if (isnan(accel_temperature)) {
+            accel_temperature = accelSensorData.temperature;
+            gyro_temperature  = gyroSensorData.temperature;
+        }
 
+        accel_temperature = TEMP_ALPHA * accel_temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature;
+        gyro_temperature  = TEMP_ALPHA * gyro_temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature;
+        gyroSensorData.temperature = gyro_temperature;
         if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
             temp_calibration_count = TEMP_CALIB_INTERVAL;
             if (accel_temp_calibrated) {
@@ -467,7 +473,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
         PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
 #endif
         vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
-        lastSysTime = xTaskGetTickCount();
     }
 }
 
diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c
index 1921c5b09..fdbdde5a0 100644
--- a/flight/targets/boards/coptercontrol/firmware/pios_board.c
+++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c
@@ -121,7 +121,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
     .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
     // Clock at 8 khz, downsampled by 8 for 1000 Hz
     .Smpl_rate_div_no_dlp = 7,
-    // Clock at 1 khz, downsampled by 2 for 1000 Hz
+    // Clock at 1 khz, downsampled by 1 for 1000 Hz
     .Smpl_rate_div_dlp    = 0,
     .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
     .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,