ErikBob

C++ doesn't like Type Punning. You would have to deactive checking for strict-aliasing in your compiler, which I wouldn't recommend. Simply replace fastInvSquare() by math.h 1/sqrt(). The speedloss is insignificant.

Simply replace fastInvSquare() by math.h 1/sqrt()

math.h is the header containing sqrt() you can replace `float Madgwick::invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df...

Did you account for sensor-offsets?