removed redundant normilization in AHRS
Changed illogical startup unstable gain
changed some stuff to a more clear notation