per l'algoritmo di stabilità con una 6dof puoi usare questo algoritmo:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1295218329, l'importante è che l'input dai giroscopi dia in radianti al secondo.
L'algoritmo lo puoi testare al pc con il programmino java postato quì:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1295219565 , arduino deve stampare via serial i quattro valori di quaternione e poi andare a capo, java li legge e li rappresenta a schermo.
Noterai che lo YAW accumula errore: il problema lo puoi parzialmente risolvere con il GPS, semplicemente guardando in che direzione stai andando e estrapolando il tuo angolo YAW (in realtà può essere erroneo nel caso hai un vento frontale a 100km/h, perchè il GPS ti da lo spostamento rispetto alla terra, mentre a te serve lo spostamento rispetto all'aria...) oppure usando il classico magnetometro 3 assi(quindi 9DOF)
Anche io sono interessato a un autopilota del genere, ma ho deciso di partire prima dai multicotteri che hanno il vantaggio della partenza in verticale(a mio avviso il decollo/atterraggio è solo questione di qualche sensore in più) e algoritmi più complessi di stabilità (che invece sono la vera "anima" dell'autopilota)
qualche consiglio: non usare l'accelerometro per conoscere l'altezza, o almeno non fidarti, perchè in quel caso la lettura è relativa e effettui un'integrazione dei valori, che quindi comporta un'errore che si accumula (lo stesso problema dello YAW), mentre hai il GPS che ti da una lettura assoluta, e anche se canna di 20m non è un problema, tanto se ti avvicini troppo al suolo se ne accorge l'ultrasonico :-)
tieni aggiornato questa discussione che sono felice di aiutarti!
ps. nell'aria la velocità del suono è di 1.193,4 km/h... certo che perdi di precisione, a seconda di come sistemi il sensore (inclinato o dritto ecc..) leggerai dei valori più o meno piccoli in base alla velocità.. ma credo che se anche fosse una variazione notabile, con un paio di test trovi la proporzione che in base alla velocità e al valore letto ti restituisce la vera altitudine.