Replaced the AHRS based init with a self-contained initialization for the sake of generality of the framework