AHRS code fixes by Jeroen van de Mortel
simplifications