[Multicotteri] Elettronica : IMU, MCU, Sensori ed algoritmi di controllo

Salve a tutti,
sapreste indicarmi come procedere per implementare l'algoritmo di controllo della stabilità del mio quadricottero?
Il primo goal che vorrei raggiungere è quello della stabilità in volo statico, ossia che resti fermo in aria, senza oscillazioni o rollii.
Ovviamente a bordo ho un gyro-acc-magn con 3 gradi di libertà
ringrazio anticipatamente