added LP filters (10Hz) on attitude rates in estimator

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -66,6 +66,8 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 
@@ -258,6 +260,11 @@ private:
 
     AttPosEKF                   *_ekf;
 
+    /* Low pass filter for attitude rates */
+    math::LowPassFilter2p _LP_att_P;
+    math::LowPassFilter2p _LP_att_Q;
+    math::LowPassFilter2p _LP_att_R;
+
 private:
     /**
      * Update our local parameter cache.

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _newRangeData(false),
 
     _mavlink_fd(-1),
-    _parameters {},
-    _parameter_handles {},
-    _ekf(nullptr)
+    _parameters{},
+    _parameter_handles{},
+    _ekf(nullptr),
+
+    _LP_att_P(100.0f, 10.0f),
+    _LP_att_Q(100.0f, 10.0f),
+    _LP_att_R(100.0f, 10.0f)
 {
 	_last_run = hrt_absolute_time();
 
@@ -819,9 +823,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
+	_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
 	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;