Improved robustness for attitude filter

This is not tested in flight yet, but should help substantially to improve the attitude estimation robustness. Not yet obeying constant turns, but this is on the roadmap.