diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index a0ba5dd413..7edfe0c5c3 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-static int
+static int16_t
 get_stabilize_roll(int32_t target_angle)
 {
 	// angle error
@@ -29,7 +29,7 @@ get_stabilize_roll(int32_t target_angle)
 #endif
 }
 
-static int
+static int16_t
 get_stabilize_pitch(int32_t target_angle)
 {
 	// angle error
@@ -56,7 +56,7 @@ get_stabilize_pitch(int32_t target_angle)
 #endif
 }
 
-static int
+static int16_t
 get_stabilize_yaw(int32_t target_angle)
 {
 	// angle error
@@ -85,7 +85,7 @@ get_stabilize_yaw(int32_t target_angle)
 #endif
 }
 
-static int
+static int16_t
 get_acro_roll(int32_t target_rate)
 {
 	target_rate = target_rate * g.acro_p;
@@ -93,7 +93,7 @@ get_acro_roll(int32_t target_rate)
 	return get_rate_roll(target_rate);
 }
 
-static int
+static int16_t
 get_acro_pitch(int32_t target_rate)
 {
 	target_rate = target_rate * g.acro_p;
@@ -101,7 +101,7 @@ get_acro_pitch(int32_t target_rate)
 	return get_rate_pitch(target_rate);
 }
 
-static int
+static int16_t
 get_acro_yaw(int32_t target_rate)
 {
 	target_rate = g.pi_stabilize_yaw.get_p(target_rate);
@@ -109,78 +109,69 @@ get_acro_yaw(int32_t target_rate)
 	return get_rate_yaw(target_rate);
 }
 
-static int
+static int16_t
 get_rate_roll(int32_t target_rate)
 {
-		   int16_t rate_d1 		= 0;
-	static int16_t rate_d2 		= 0;
-	static int16_t rate_d3 		= 0;
-	static int32_t last_rate 	= 0;
+	static AverageFilterInt32_Size3 rate_d_filter;	// filtered acceleration
+	static int32_t last_rate = 0;					// previous iterations rate
+	int32_t current_rate;							// this iteration's rate
+	int32_t rate_d;  								// roll's acceleration
+	int32_t output;									// output from pid controller
+	int32_t rate_d_dampener;						// value to dampen output based on acceleration
 
-	int32_t current_rate 	= (omega.x * DEGX100);
+	// get current rate
+	current_rate 	= (omega.x * DEGX100);
 
-	// History of last 3 dir
-	rate_d3			= rate_d2;
-	rate_d2			= rate_d1;
-	rate_d1 		= current_rate - last_rate;
+	// calculate and filter the acceleration
+	rate_d 			= rate_d_filter.apply(current_rate - last_rate);
+
+	// store rate for next iteration
 	last_rate 		= current_rate;
 
-	// rate control
-	target_rate		= target_rate - current_rate;
-	target_rate 	= g.pid_rate_roll.get_pid(target_rate, G_Dt);
+	// call pid controller
+	output 			= g.pid_rate_roll.get_pid(target_rate - current_rate, G_Dt);
 
-	// Dampening
-	//int16_t d_temp	= (float)(current_rate - last_rate) * g.stabilize_d;
-	//target_rate 		-= constrain(d_temp, -500, 500);
-	//last_rate 		= current_rate;
+	// Dampening output with D term
+	rate_d_dampener = rate_d * g.stabilize_d;
+	rate_d_dampener = constrain(rate_d_dampener, -400, 400);
+	output -= rate_d_dampener;
 
-	// D term
-	// I had tried this before with little result. Recently, someone mentioned to me that
-	// MultiWii uses a filter of the last three to get around noise and get a stronger signal.
-	// Works well! Thanks!
-	int16_t d_temp =  (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
-	d_temp = constrain(d_temp, -400, 400);
-	target_rate -= d_temp;
-
-	// output control:
-	return constrain(target_rate, -2500, 2500);
+	// output control
+	return constrain(output, -2500, 2500);
 }
 
-static int
+static int16_t
 get_rate_pitch(int32_t target_rate)
 {
-		   int16_t rate_d1 		= 0;
-	static int16_t rate_d2 		= 0;
-	static int16_t rate_d3 		= 0;
-	static int32_t last_rate 	= 0;
+	static AverageFilterInt32_Size3 rate_d_filter;	// filtered acceleration
+	static int32_t last_rate = 0;					// previous iterations rate
+	int32_t current_rate;							// this iteration's rate
+	int32_t rate_d;  								// roll's acceleration
+	int32_t output;									// output from pid controller
+	int32_t rate_d_dampener;						// value to dampen output based on acceleration
 
-	int32_t current_rate 	= (omega.y * DEGX100);
+	// get current rate
+	current_rate 	= (omega.y * DEGX100);
 
-	// History of last 3 dir
-	rate_d3			= rate_d2;
-	rate_d2			= rate_d1;
-	rate_d1 		= current_rate - last_rate;
+	// calculate and filter the acceleration
+	rate_d 			= rate_d_filter.apply(current_rate - last_rate);
+
+	// store rate for next iteration
 	last_rate 		= current_rate;
 
-	// rate control
-	target_rate	 	= target_rate - current_rate;
-	target_rate 	= g.pid_rate_pitch.get_pid(target_rate, G_Dt);
+	// call pid controller
+	output 			= g.pid_rate_pitch.get_pid(target_rate - current_rate, G_Dt);
 
-	// Dampening
-	//int16_t d_temp	= (float)(current_rate - last_rate) * g.stabilize_d;
-	//target_rate 		-= constrain(d_temp, -500, 500);
-	//last_rate 		= current_rate;
+	// Dampening output with D term
+	rate_d_dampener = rate_d * g.stabilize_d;
+	rate_d_dampener = constrain(rate_d_dampener, -400, 400);
+	output -= rate_d_dampener;
 
-	// D term
-	int16_t d_temp =  (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
-	d_temp = constrain(d_temp, -400, 400);
-	target_rate -= d_temp;
-
-	// output control:
-	return constrain(target_rate, -2500, 2500);
+	// output control
+	return constrain(output, -2500, 2500);
 }
 
-static int
+static int16_t
 get_rate_yaw(int32_t target_rate)
 {
 	// rate control
@@ -346,7 +337,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
 	}
 }
 
-static int get_angle_boost(int value)
+static int16_t get_angle_boost(int16_t value)
 {
 	float temp = cos_pitch_x * cos_roll_x;
 	temp = 1.0 - constrain(temp, .5, 1.0);
@@ -496,7 +487,7 @@ get_of_roll(int32_t control_roll)
 {
 #ifdef OPTFLOW_ENABLED
 	static float tot_x_cm = 0;  // total distance from target
-    static unsigned long last_of_roll_update = 0;
+    static uint32_t last_of_roll_update = 0;
 	int32_t new_roll = 0;
 
 	// check if new optflow data available
@@ -530,7 +521,7 @@ get_of_pitch(int32_t control_pitch)
 {
 #ifdef OPTFLOW_ENABLED
     static float tot_y_cm = 0;  // total distance from target
-    static unsigned long last_of_pitch_update = 0;
+    static uint32_t last_of_pitch_update = 0;
 	int32_t new_pitch = 0;
 
 	// check if new optflow data available