diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp
index 0a4cd51c7d..8db2795054 100644
--- a/libraries/APM_Control/AP_PitchController.cpp
+++ b/libraries/APM_Control/AP_PitchController.cpp
@@ -62,23 +62,17 @@ int32_t AP_PitchController::get_servo_out(int32_t angle, float scaler, bool stab
         desired_rate = _max_rate_pos;
     }
 	
-	desired_rate *= roll_scaler;
-	
-	if(stabilize) {
-		desired_rate *= _stabilize_gain;
-	}
-	
-	int32_t rate_error = desired_rate - ToDeg(rate)*100;
-	
-	float roll_ff = _roll_ff * 1000 * (roll_scaler-1.0f);
-	if(roll_ff > 4500)
-		roll_ff = 4500;
-	else if(roll_ff < 0)
-		roll_ff = 0;
+	// Apply the turn correction offset
+	desired_rate = desired_rate + rate_offset;
+
+	// Get body rate vector (radians/sec)
+	float omega_y = _ahrs->get_gyro().y;
 	
-	float out = constrain_float(((rate_error * _kp_rate) + (desired_rate * _kp_ff) + roll_ff) * scaler,-4500,4500);
+	// Calculate the pitch rate error (deg/sec) and scale
+	float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
 	
-	//rate integrator
+	// Multiply pitch rate error by _ki_rate and integrate
+	// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
 	if (!stabilize) {
 		if ((fabsf(_ki_rate) > 0) && (dt > 0))
 		{
diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h
index 4ea2bae675..2b715e3b2a 100644
--- a/libraries/APM_Control/AP_PitchController.h
+++ b/libraries/APM_Control/AP_PitchController.h
@@ -31,18 +31,12 @@ private:
 	AP_Float _roll_ff;
 	AP_Float _stabilize_gain;
 	uint32_t _last_t;
-	float _last_rate;
+	float _last_out;
 	
 	float _integrator;
 	
 	AP_AHRS *_ahrs;
 	
-	/// Low pass filter cut frequency for derivative calculation.
-	///
-	/// 20 Hz becasue anything over that is probably noise, see
-	/// http://en.wikipedia.org/wiki/Low-pass_filter.
-	///
-	static const uint8_t _fCut = 20;
 };
 
 #endif // __AP_PITCH_CONTROLLER_H__
diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp
index 3a3ab5bfc2..fb067d8cb3 100644
--- a/libraries/APM_Control/AP_RollController.cpp
+++ b/libraries/APM_Control/AP_RollController.cpp
@@ -50,11 +50,11 @@ int32_t AP_RollController::get_servo_out(int32_t angle, float scaler, bool stabi
 	if (_max_rate && desired_rate < -_max_rate) desired_rate = -_max_rate;
 	else if (_max_rate && desired_rate > _max_rate) desired_rate = _max_rate;
 	
-	if(stabilize) {
-		desired_rate *= _stabilize_gain;
-	}
-
-	int32_t rate_error = desired_rate - ToDeg(rate)*100;
+    // Get body rate vector (radians/sec)
+	float omega_x = _ahrs->get_gyro().x;
+	
+	// Calculate the roll rate error (deg/sec) and apply gain scaler
+	float rate_error = (desired_rate - ToDeg(omega_x)) * scaler;
 	
 	float out = (rate_error * _kp_rate + desired_rate * _kp_ff) * scaler;
 	
diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h
index 48a9ed0ae4..9ac5d58cd7 100644
--- a/libraries/APM_Control/AP_RollController.h
+++ b/libraries/APM_Control/AP_RollController.h
@@ -29,18 +29,12 @@ private:
 	AP_Float _stabilize_gain;
 	AP_Int16 _max_rate;
 	uint32_t _last_t;
-	float _last_rate;
+	float _last_out;
 
 	float _integrator;
 
 	AP_AHRS *_ahrs;
 
-	/// Low pass filter cut frequency for derivative calculation.
-	///
-	/// 20 Hz becasue anything over that is probably noise, see
-	/// http://en.wikipedia.org/wiki/Low-pass_filter.
-	///
-	static const uint8_t _fCut = 20;
 };
 
 #endif // __AP_ROLL_CONTROLLER_H__
diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp
index 1030487dbd..66080de300 100644
--- a/libraries/APM_Control/AP_YawController.cpp
+++ b/libraries/APM_Control/AP_YawController.cpp
@@ -49,16 +49,28 @@ int32_t AP_YawController::get_servo_out(float scaler, bool stick_movement)
 			}
 		}
 	}
-	_stick_movement = stick_movement;
+	rate_offset = (9.807f / constrain(aspeed , float(aspd_min), float(aspd_max))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
 
-	Vector3f accels = _ins->get_accel();
+    // Get body rate vector (radians/sec)
+	float omega_z = _ahrs->get_gyro().z;
 	
-	// I didn't pull 512 out of a hat - it is a (very) loose approximation of
-    // 100*ToDeg(asinf(-accels.y/9.81f))
-	// which, with a P of 1.0, would mean that your rudder angle would be
-    // equal to your roll angle when
-	// the plane is still. Thus we have an (approximate) unit to go by.
-	float error = 512 * -accels.y;
+	// Subtract the steady turn component of rate from the measured rate
+	// to calculate the rate relative to the turn requirement in degrees/sec
+	float rate_hp_in = ToDeg(omega_z - rate_offset);
+	
+	// Apply a high-pass filter to the rate to washout any steady state error
+	// due to bias errors in rate_offset
+	// Use a cut-off frequency of omega = 0.2 rad/sec
+	// Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
+	float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;
+	_last_rate_hp_out = rate_hp_out;
+	_last_rate_hp_in = rate_hp_in;
+
+	// Get the accln vector (m/s^2)
+	Vector3f accel = _ins->get_accel();
+
+	// Calculate input to integrator
+	float integ_in = - _K_I * (_K_A * accel.y + rate_hp_out);
 	
 	// strongly filter the error
 	float RC = 1/(2*PI*_fCut);
diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h
index 33860da8a5..fea9ebde73 100644
--- a/libraries/APM_Control/AP_YawController.h
+++ b/libraries/APM_Control/AP_YawController.h
@@ -31,6 +31,10 @@ private:
 	AP_Int16 _imax;
 	uint32_t _last_t;
 	float _last_error;
+	float _last_out;
+	float _last_rate_hp_out;
+	float _last_rate_hp_in;
+	float _K_D_last;
 
 	float _integrator;
 	bool _stick_movement;