Initial commit of AHRS code