Theory:Stabilization:Angle:Complementary

Сделав приближение, согласно которому случайные силы F_вн., действующие на квадрокоптер, являются недолговременными, можно получить угол, фильтруя высокие частоты из показаний акселерометра. В данный момент используется итерационная формула RC-фильтра, пропускающего низкие частоты.

Реализация: angle' = (1 - α) * (angle+ gyro * Δt) + α * accel_angle, где

• α = Δt / (Δt + T / (2π)),
• T — константа RC для фильтра,
• angle — текущий угол (оставшийся с прошлой итерации),
• angle' — новый угол (для текущей итерации),
• Δt — время между текущей и предыдущей итерацией,
• accel_angle — углы поворота вектора показаний акселерометра относительно вертикали,
• gyro — показания гироскопа.

Член gyro * Δt (численный интеграл угловой скорости) необходим для учета быстрых изменений угла с гироскопа.