diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
index 05930bca91..dc19ef42dd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
@@ -65,10 +65,21 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
 
     // add accel bias and noise
     Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
-    float xAccel = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
-    float yAccel = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
-    float zAccel = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
-
+    float xAccel = sitl->state.xAccel + accel_bias.x;
+    float yAccel = sitl->state.yAccel + accel_bias.y;
+    float zAccel = sitl->state.zAccel + accel_bias.z;
+    const Vector3f &vibe_freq = sitl->vibe_freq;
+    if (vibe_freq.is_zero()) {
+        xAccel += accel_noise * rand_float();
+        yAccel += accel_noise * rand_float();
+        zAccel += accel_noise * rand_float();
+    } else {
+        float t = AP_HAL::micros() * 1.0e-6;
+        xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
+        yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
+        zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
+    }
+    
     // correct for the acceleration due to the IMU position offset and angular acceleration
     // correct for the centripetal acceleration
     // only apply corrections to first accelerometer
@@ -119,9 +130,17 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
     float q = radians(sitl->state.pitchRate) + gyro_drift();
     float r = radians(sitl->state.yawRate) + gyro_drift();
 
-    p += gyro_noise * rand_float();
-    q += gyro_noise * rand_float();
-    r += gyro_noise * rand_float();
+    const Vector3f &vibe_freq = sitl->vibe_freq;
+    if (vibe_freq.is_zero()) {
+        p += gyro_noise * rand_float();
+        q += gyro_noise * rand_float();
+        r += gyro_noise * rand_float();
+    } else {
+        float t = AP_HAL::micros() * 1.0e-6;
+        p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
+        q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
+        r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
+    }
 
     Vector3f gyro = Vector3f(p, q, r);