Altitude hold working

Position KF needs to be updated to three axes