IMU.cpp: Altitude hold fix by Happul3

See http://www.multiwii.com/forum/viewtopic.php?f=8&t=4992&start=10#p49929

Signed-off-by: Koen Kooi <koen@dominion.thruhere.net>