added C++ version for Razor_AHRS_test