When I implemented this on my machine I had major instabilities when I used a real IMU. After some research it looks like there are some problems with this implementation of the invSqrt function. Fixed when I replaced it with this one:
https://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root
When I implemented this on my machine I had major instabilities when I used a real IMU. After some research it looks like there are some problems with this implementation of the invSqrt function. Fixed when I replaced it with this one:
https://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root