GPS едва ли что-то поможет удерживать в приемлемых пределах. Знаю - ставят оптический датчик горизонта - зафиксироваться по тангажу и крену. Помоему аналогично ("по звёздам") и от "уплываний" - сдуваний страхуются.
все 6 сигналов обрабатываете?
"Я почему спрашиваю" (с) - интересно автопилот программный попробовать на ардуино... Для начала просто "точечный".