Estimator improvements + testing dressing

This PR mostly has estimator improvements, but also other testing-induced improvements (e.g. in MAVLink, rate config). Test flown and bench tested intensely.