Various EKF updates from Paul & Jon

This contains everything from Paul's ekfUpdates branch, plus a bit of cleanup from me, plus a couple little bugfixes from me (ekf gyro bias logging, fixing coning correction)