Better Altitude Control

We need to put some filter on the altitude sensor as it has too much noise. A one dimensional kalman filter may be really good for this and simple. We may even be able to fuse the Z accelerometer into this as well.

**Update:** I'm actually going to disable altitude control for now and rely on manual thrust from the gamepad. Otherwise I won't be able to get the 0.1 release out tonight.