diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index d49460fad8a7c4a8de7e117eded3de2c74e8bba0..507521d175406bd0443056e1c60fa33b3f4f3078 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -513,6 +513,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
 void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
 {
     Vector3l asum, gsum;
+    float tsum = 0;
         
     for (uint8_t i = 0; i < n_samples; i++) {
         uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
@@ -525,6 +526,7 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
+        tsum += temp;
         _last_temp = temp;
     }
 
@@ -539,6 +541,8 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
     
     _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+    _temp_filtered = _temp_filter.apply(tsum / n_samples);
 }
 
 /*
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index 031ec62b91601eb07cc45840320ead2cf918b13c..19b56ddcb30779b6206c0315654a2618e0b04bff 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -442,7 +442,8 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
 void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
 {
     Vector3l asum, gsum;
-        
+    float tsum = 0;
+
     for (uint8_t i = 0; i < n_samples; i++) {
         uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
         asum += Vector3l(int16_val(data, 1),
@@ -454,6 +455,7 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
+        tsum += temp;
         _last_temp = temp;
     }
 
@@ -468,6 +470,8 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
     
     _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+    _temp_filtered = _temp_filter.apply(tsum / n_samples);
 }