mTECS

This is Thomas' total energy control system. Heavily tested in the last days, its now a good time to start ironing out the last kinks and establishing trust logs. Should also be made the default on master.

@thomasgubler @sjwilks @AndreasAntener Please provide additional comments here.  Less branches will mean less confusion in the next weeks testing 8).
Covariance values can be pulled from the filter as:

_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];

To get the whole 2-by-2 matrix (north-east covariance), also grab the _ekf->P[14][15] term.