Changed the PID derivative filter to a moving average with 4 samples. Having great luck with it and High kD when using noisy sensors.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2957 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/PID/PID.cpp

@@ -18,17 +18,30 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
 
 	// Compute derivative component if time has elapsed
 	if ((fabs(_kd) > 0) && (dt > 0)) {
-		float derivative = (error - _last_error) / delta_time;
+		float derivative;
+		// add in
+		_filter[_filter_index] = (error - _last_error) / delta_time;
+		_filter_index++;
+
+		if(_filter_index >= PID_FILTER_SIZE)
+			_filter_index = 0;
+
+		// sum our filter
+		for(uint8_t i = 0; i < PID_FILTER_SIZE; i++){
+			derivative += _filter[i];
+		}
+
+		// grab result
+		derivative /= PID_FILTER_SIZE;
 
 		// discrete low pass filter, cuts out the
 		// high frequency noise that can drive the controller crazy
-		float RC = 1/(2*M_PI*_fCut);
-		derivative = _last_derivative +
-		        (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
+		//float RC = 1/(2*M_PI*_fCut);
+		//derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
 
 		// update state
 		_last_error 		= error;
-		_last_derivative    = derivative;
+		//_last_derivative    = derivative;
 
 		// add in derivative component
 		output 				+= _kd * derivative;
@@ -83,7 +96,7 @@ PID::reset_I()
 {
 	_integrator = 0;
 	_last_error = 0;
-	_last_derivative = 0;
+	//_last_derivative = 0;
 }
 
 void

libraries/PID/PID.h

@@ -8,6 +8,7 @@
 
 #include <AP_Common.h>
 #include <math.h>		// for fabs()
+#define PID_FILTER_SIZE	4
 
 /// @class	PID
 /// @brief	Object managing one PID control
@@ -120,6 +121,7 @@ public:
 	void	kI(const float v)		{ _ki.set(v); }
 	void	kD(const float v)		{ _kd.set(v); }
 	void	imax(const int16_t v)	{ _imax.set(abs(v)); }
+	//void	filter_size(uint8_t v)	{ _filter_size = min(v, PID_FILTER_SIZE); }
 
 	float	get_integrator() const	{ return _integrator; }
 
@@ -129,10 +131,13 @@ private:
 	AP_Float16			_ki;
 	AP_Float16			_kd;
 	AP_Int16			_imax;
+	//uint8_t				_filter_size;
+	float				_filter[PID_FILTER_SIZE];
+	uint8_t				_filter_index;
 
 	float				_integrator;		///< integrator value
 	int32_t				_last_error;		///< last error for derivative
-	float				_last_derivative; 	///< last derivative for low-pass filter
+	//float				_last_derivative; 	///< last derivative for low-pass filter
 
 	/// Low pass filter cut frequency for derivative calculation.
 	///