diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp
index 69132c8a02..a44d5f3e13 100644
--- a/libraries/PID/PID.cpp
+++ b/libraries/PID/PID.cpp
@@ -18,30 +18,17 @@ 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 = 0;
-		// 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;
+		float derivative = (error - _last_error) / delta_time;
 
 		// 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;
@@ -64,39 +51,12 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
 	return output;
 }
 
-
-long
-PID::get_pi(int32_t error, uint16_t dt, float scaler)
-{
-	float output		= 0;
- 	float delta_time	= (float)dt / 1000.0;
-
-	// Compute proportional component
-	output += error * _kp;
-
-	// scale the P components
-	output *= scaler;
-
-	// Compute integral component if time has elapsed
-	if ((fabs(_ki) > 0) && (dt > 0)) {
-		_integrator 		+= (error * _ki) * scaler * delta_time;
-		if (_integrator < -_imax) {
-			_integrator = -_imax;
-		} else if (_integrator > _imax) {
-			_integrator = _imax;
-		}
-		output 				+= _integrator;
-	}
-
-	return output;
-}
-
 void
 PID::reset_I()
 {
 	_integrator = 0;
 	_last_error = 0;
-	//_last_derivative = 0;
+	_last_derivative = 0;
 }
 
 void
diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h
index 8def9e8d0e..6cf9919688 100644
--- a/libraries/PID/PID.h
+++ b/libraries/PID/PID.h
@@ -8,7 +8,6 @@
 
 #include <AP_Common.h>
 #include <math.h>		// for fabs()
-#define PID_FILTER_SIZE	6
 
 /// @class	PID
 /// @brief	Object managing one PID control
@@ -86,9 +85,6 @@ public:
 	///
 	long 	get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
 
-	long 	get_pi(int32_t error, uint16_t dt, float scaler = 1.0);
-
-
 	/// Reset the PID integrator
 	///
 	void	reset_I();
@@ -121,7 +117,6 @@ 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; }
 
@@ -131,12 +126,10 @@ private:
 	AP_Float16			_ki;
 	AP_Float16			_kd;
 	AP_Int16			_imax;
-	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.
 	///