Attitude / position estimation and controller improvements

This (slightly misnamed) branch contains a number of improvements on the state estimation and control contributed by James Goppert.

Tested in HIL in flight and on a real plane handheld (including GPS). Good to merge though, not affecting other system parts.