diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 76f203056a..d297818de1 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -658,19 +658,19 @@ MPU6000::init()
 	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
 
 	if (_accel_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
 
 	if (_gyro_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	ret = -EIO;
 
 	if (reset() != OK) {
-		goto out;
+		return ret;
 	}
 
 	/* Initialize offsets and scales */
@@ -688,6 +688,34 @@ MPU6000::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+	// set software low pass filter for controllers
+	param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
+	float accel_cut = MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) {
+		PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
+
+		_accel_filter_x.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_y.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_z.set_cutoff_frequency(MPU6000_ACCEL_DEFAULT_RATE, accel_cut);
+
+	} else {
+		PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
+	}
+
+	param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
+	float gyro_cut = MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
+		PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
+
+		_gyro_filter_x.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
+		_gyro_filter_y.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
+		_gyro_filter_z.set_cutoff_frequency(MPU6000_GYRO_DEFAULT_RATE, gyro_cut);
+
+	} else {
+		PX4_ERR("IMU_GYRO_CUTOFF param invalid");
+	}
 
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
@@ -711,10 +739,9 @@ MPU6000::init()
 					   &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_accel_topic == nullptr) {
-		warnx("ADVERT FAIL");
+		PX4_WARN("ADVERT FAIL");
 	}
 
-
 	/* advertise sensor topic, measure manually to initialize valid report */
 	struct gyro_report grp;
 	_gyro_reports->get(&grp);
@@ -723,10 +750,9 @@ MPU6000::init()
 			     &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
 
 	if (_gyro->_gyro_topic == nullptr) {
-		warnx("ADVERT FAIL");
+		PX4_WARN("ADVERT FAIL");
 	}
 
-out:
 	return ret;
 }
 
@@ -2137,6 +2163,10 @@ MPU6000::print_info()
 	}
 
 	::printf("temperature: %.1f\n", (double)_last_temperature);
+	float accel_cut = _accel_filter_x.get_cutoff_freq();
+	::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut));
+	float gyro_cut = _gyro_filter_x.get_cutoff_freq();
+	::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut));
 }
 
 void
diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 48da91aa38..0fde95e832 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -282,18 +282,18 @@ MPU9250::init()
 	ret = -ENOMEM;
 
 	if (_accel_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
 
 	if (_gyro_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	if (reset_mpu() != OK) {
 		PX4_ERR("Exiting! Device failed to take initialization");
-		goto out;
+		return ret;
 	}
 
 	/* Initialize offsets and scales */
@@ -311,22 +311,51 @@ MPU9250::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+	// set software low pass filter for controllers
+	param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
+	float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
+		PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
+
+		_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
+		_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
+
+	} else {
+		PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
+	}
+
+	param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
+	float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
+
+	if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
+		PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
+
+		_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
+		_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
+		_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
+
+	} else {
+		PX4_ERR("IMU_GYRO_CUTOFF param invalid");
+	}
+
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
 
 	/* if probe/setup failed, bail now */
 	if (ret != OK) {
 		DEVICE_DEBUG("gyro init failed");
-		goto out;
+		return ret;
 	}
 
 #ifdef USE_I2C
 
-	if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
-		warnx("failed to setup ak8963 interface");
+	if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
+		PX4_ERR("failed to setup ak8963 interface");
 	}
 
-#endif
+#endif /* USE_I2C */
 
 	/* do CDev init for the mag device node, keep it optional */
 	if (_whoami == MPU_WHOAMI_9250) {
@@ -336,7 +365,7 @@ MPU9250::init()
 	/* if probe/setup failed, bail now */
 	if (ret != OK) {
 		DEVICE_DEBUG("mag init failed");
-		goto out;
+		return ret;
 	}
 
 
@@ -364,7 +393,7 @@ MPU9250::init()
 
 	if (_accel_topic == nullptr) {
 		PX4_ERR("ADVERT FAIL");
-		goto out;
+		return ret;
 	}
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -376,10 +405,9 @@ MPU9250::init()
 
 	if (_gyro->_gyro_topic == nullptr) {
 		PX4_ERR("ADVERT FAIL");
-		goto out;
+		return ret;
 	}
 
-out:
 	return ret;
 }
 
@@ -1565,6 +1593,10 @@ MPU9250::print_info()
 	}
 
 	::printf("temperature: %.1f\n", (double)_last_temperature);
+	float accel_cut = _accel_filter_x.get_cutoff_freq();
+	::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut));
+	float gyro_cut = _gyro_filter_x.get_cutoff_freq();
+	::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut));
 }
 
 void
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 16b2e83f1d..ee84e8a3f5 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -1144,3 +1144,33 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
  * @group Sensor Enable
  */
 PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
+
+/**
+* Driver level cut frequency for gyro
+*
+* The cut frequency for the 2nd order butterworth filter on the gyro driver. This features
+* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
+* controllers, not the estimators. 0 disables the filter.
+*
+* @min 5
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
+
+/**
+* Driver level cut frequency for accel
+*
+* The cut frequency for the 2nd order butterworth filter on the accel driver. This features
+* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
+* controllers, not the estimators. 0 disables the filter.
+*
+* @min 5
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);
\ No newline at end of file