I have built my own quadcopter and to estimate its attitude, I am using the modified Madgwick filter algorithm found on this GitHub page https://github.com/xioTechnologies/Fusion
I am using the NED convention and despite the fact that the algorithm directly estimates the orientation quaternion, the yaw angle seems to deviate when there are roll and/or pitch angles. I am trying to fix this and I thought that there should be a compensation for the sensor measurements when there are roll and pitch motions. I attempted to do so by implementing equations (4) and (5) of this paper https://www.sciencedirect.com/science/article/abs/pii/S0924424707003834
What I did was to obtain the roll and pitch angles from the madgwick filter estimation and then use these estimates to implement the aforementioned equations. However, it doesn't seem to work and I'm stuck trying to find a solution. It seems weird to me that this happens, since the algorithm I use is supposed to work pretty fine. I should point out that the same situation occurs regardless of whether the motors are spinning or not. Any help would be greatly appreciated.
I am providing the filter with the IMU measurements, i.e., gyroscope,accelerometer and magnetometer, obtained every $T_s = 0.025\text{sec}$.
