diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 2887ea6882..445d905eca 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -25,11 +25,12 @@ AP_InertialSensor::AP_InertialSensor()
 
 void
 AP_InertialSensor::init( Start_style style,
+                         Sample_rate sample_rate,
                          void (*delay_cb)(unsigned long t),
                          void (*flash_leds_cb)(bool on),
                          AP_PeriodicProcess *  scheduler )
 {
-    _product_id = _init_sensor(scheduler);
+    _product_id = _init_sensor(scheduler, sample_rate);
 
     // check scaling
     Vector3f accel_scale = _accel_scale.get();
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 410ad5abea..809e85ecf3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -24,6 +24,13 @@ public:
         WARM_START
     };
 
+    // the rate that updates will be available to the application
+    enum Sample_rate {
+        RATE_50HZ,
+        RATE_100HZ,
+        RATE_200HZ
+    };
+
     /// Perform startup initialisation.
     ///
     /// Called to initialise the state of the IMU.
@@ -38,6 +45,7 @@ public:
     /// @param style	The initialisation startup style.
     ///
     virtual void        init( Start_style           style,
+                              Sample_rate           sample_rate,
                               void                  (*delay_cb)(unsigned long t),
                               void                  (*flash_leds_cb)(bool on),
                               AP_PeriodicProcess *  scheduler );
@@ -135,7 +143,7 @@ public:
 protected:
 
     // sensor specific init to be overwritten by descendant classes
-    virtual uint16_t        _init_sensor( AP_PeriodicProcess * scheduler ) = 0;
+    virtual uint16_t        _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) = 0;
 
     // no-save implementations of accel and gyro initialisation routines
     virtual void            _init_accel(void    (*delay_cb)(unsigned long t),
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 1f74bcb34a..9450ae9517 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -210,13 +210,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
     _dmp_initialised = false;
 }
 
-uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler )
+uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate )
 {
     if (_initialised) return _mpu6000_product_id;
     _initialised = true;
     _scheduler = scheduler;     // store pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
     scheduler->suspend_timer();
-    hardware_init();
+    hardware_init(sample_rate);
     scheduler->resume_timer();
     return _mpu6000_product_id;
 }
@@ -381,7 +381,7 @@ void AP_InertialSensor_MPU6000::data_interrupt(void)
     _scheduler->queue_process( AP_InertialSensor_MPU6000::read );
 }
 
-void AP_InertialSensor_MPU6000::hardware_init()
+void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
 {
     // MPU6000 chip select setup
     pinMode(MPU6000_CS_PIN, OUTPUT);
@@ -401,12 +401,36 @@ void AP_InertialSensor_MPU6000::hardware_init()
     // Disable I2C bus (recommended on datasheet)
     register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
     delay(1);
-    // SAMPLE RATE
-    register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+
+    uint8_t rate, filter;
+
+    // sample rate and filtering
+    // to minimise the effects of aliasing we choose a filter
+    // that is less than half of the sample rate
+    switch (sample_rate) {
+    case RATE_50HZ:
+        rate = MPUREG_SMPLRT_50HZ;
+        filter = BITS_DLPF_CFG_20HZ;
+        break;
+    case RATE_100HZ:
+        rate = MPUREG_SMPLRT_100HZ;
+        filter = BITS_DLPF_CFG_42HZ;
+        break;
+    case RATE_200HZ:
+    default:
+        rate = MPUREG_SMPLRT_200HZ;
+        filter = BITS_DLPF_CFG_98HZ;
+        break;
+    }
+
+    // set sample rate
+    register_write(MPUREG_SMPLRT_DIV, rate);
     delay(1);
-    // FS & DLPF   FS=2000º/s, DLPF = 98Hz (low pass filter)
-    register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
+
+    // set low pass filter
+    register_write(MPUREG_CONFIG, filter);
     delay(1);
+
     register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS);  // Gyro scale 2000º/s
     delay(1);
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index dd616c506e..a0bc7e157e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -52,7 +52,7 @@ public:
     uint32_t            get_delta_time_micros();
 
 protected:
-    uint16_t                    _init_sensor( AP_PeriodicProcess * scheduler );
+    uint16_t                    _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
 
 private:
 
@@ -60,7 +60,7 @@ private:
     static void                 data_interrupt(void);
     static uint8_t              register_read( uint8_t reg );
     static void                 register_write( uint8_t reg, uint8_t val );
-    static void                 hardware_init();
+    static void                 hardware_init(Sample_rate sample_rate);
 
     float                       _temp;
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
index 2aee5dfeaa..0f592560fd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
@@ -41,10 +41,22 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
 {
 }
 
-uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler)
+uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate)
 {
     _adc->Init(scheduler);
 
+    switch (sample_rate) {
+    case RATE_50HZ:
+        _sample_threshold = 20;
+        break;
+    case RATE_100HZ:
+        _sample_threshold = 10;
+        break;
+    case RATE_200HZ:
+        _sample_threshold = 5;
+        break;
+    }
+
 #if defined(DESKTOP_BUILD)
     return AP_PRODUCT_ID_SITL;
 #elif defined(__AVR_ATmega1280__)
@@ -130,5 +142,8 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
 // get number of samples read from the sensors
 uint16_t AP_InertialSensor_Oilpan::num_samples_available()
 {
-    return _adc->num_samples_available(_sensors);
+    if (_adc->num_samples_available(_sensors) >= _sample_threshold) {
+        return 1;
+    }
+    return 0;
 }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
index 5ed61762df..0991047e4e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
@@ -34,7 +34,7 @@ public:
     uint16_t        num_samples_available();
 
 protected:
-    uint16_t        _init_sensor(AP_PeriodicProcess * scheduler);
+    uint16_t        _init_sensor(AP_PeriodicProcess * scheduler, Sample_rate sample_rate);
 
 private:
 
@@ -55,6 +55,8 @@ private:
     static const float          _gyro_gain_z;
 
     static const float          _adc_constraint;
+
+    uint8_t                     _sample_threshold;
 };
 
 #endif // __AP_INERTIAL_SENSOR_OILPAN_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
index 42d5372b40..7d9f02af2a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
@@ -2,7 +2,7 @@
 
 #include "AP_InertialSensor_Stub.h"
 
-uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler ) {
+uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) {
     return AP_PRODUCT_ID_NONE;
 }
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
index 284a3629c3..ceafae5fe2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
@@ -32,7 +32,7 @@ public:
     uint16_t        num_samples_available();
 
 protected:
-    uint16_t        _init_sensor( AP_PeriodicProcess * scheduler );
+    uint16_t        _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
 };
 
 #endif // __AP_INERTIAL_SENSOR_STUB_H__
diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
index dde00fb36f..393e478e22 100644
--- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
+++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
@@ -55,7 +55,9 @@ void setup(void)
 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
     adc.Init(&scheduler);           // APM ADC library initialization
 #endif
-    ins.init(AP_InertialSensor::COLD_START, delay, NULL, &scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+			 AP_InertialSensor::RATE_100HZ,
+			 delay, NULL, &scheduler);
 
     // display initial values
     display_offsets_and_scaling();