I recently tried to implement a magnetometer into my EKF algorithm by using an MPU6050 (Gyro and Accelerometer measurements) and a QMC5883l (Magnetometer measurements.) Without the magnetometer, the EKF works wonderful, and the state estimation flows like butter. However, when adding the magnetometer, things get messed up. When in real life yaw = pitch = roll = 0 degrees, the state estimation shows significant non-zero values.