il filtro MADGWICK, o DCM, è basato su matrici e quaternioni di rotazione.
Le matrici di rotazione sono abbastanza semplici, per i quaternioni ci sono di mezzo i numeri complessi perchè in realtà la rotazione viene fatta in 4 dimensioni invece che tre per evitare il gimbal lock, quindi sono un pò più complicate.
Però, visto che queste "formule" sono usate per la grafica 3d, trovi in rete tantissime guide, tutorial etc...
Per esempio, questo codice GitHub - Goffredo/Quad3D: Lab to test various stabilization controllers for a flying quadricopter. Implemented in JME3. l'ho fatto con un mio amico, è una simulazione 3d di un quad con tanto di pid di stabilità e altezza funzionante. Manca ancora il calcolo dello YAW, perchè il motore fisico non simula le eliche, ma solo le forze. Forse la cosa sarà implementta, forse no 