Using the correct AHRS Algorithm

Bx, Bz were not being used correctly