diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 322375293b..f4cc3b9ab5 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -166,6 +166,7 @@ static int16_t
 get_nav_throttle(int32_t z_error)
 {
 	static int16_t old_output = 0;
+	static int16_t rate_d = 0;
 
 	int16_t rate_error;
 	int16_t output;
@@ -185,6 +186,12 @@ get_nav_throttle(int32_t z_error)
 	// limit the rate - iterm is not used
 	output =  constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
 
+	// a positive climb rate means we're going up
+	rate_d =  ((rate_d + climb_rate)>>1) * 1; // replace with gain
+
+	// slight adjustment to alt hold output
+	output -= rate_d;
+
 	// light filter of output
 	output = (old_output * 3 + output) / 4;
 
@@ -222,15 +229,6 @@ get_rate_yaw(int32_t target_rate)
 	return (int)constrain(target_rate, -2500, 2500);
 }
 
-
-// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
-// Keeps outdated data out of our calculations
-//static void reset_hold_I(void)
-//{
-//	g.pi_loiter_lat.reset_I();
-//	g.pi_loiter_lon.reset_I();
-//}
-
 // Keeps old data out of our calculation / logs
 static void reset_nav(void)
 {