Комплементарный фильтр

Урок про лёгкий и популярный способ слить гироскоп с акселерометром одним коэффициентом.

Комплементарный фильтр берёт высокочастотную часть от гироскопа (интеграл скорости) и низкочастотную — от акселерометра, дополняя их до полного сигнала.

Имя «комплементарный» — от слова «дополнять»: один датчик закрывает диапазон частот, где другой слаб. Гироскопу верим на коротких интервалах, акселерометру — на длинных, и их вклады складываются в единицу.

Формула фильтра

На каждом шаге $\Delta t$:

$$ \theta_k = \alpha\,(\theta_{k-1} + \omega_k\,\Delta t) + (1 - \alpha)\,\theta_{acc} $$

Здесь $\omega_k\Delta t$ — приращение угла от гироскопа, $\theta_{acc}$ — угол из акселерометра, $\alpha$ обычно близко к 0.95–0.98: гироскопу доверяем сильно на коротком шаге, но медленно подтягиваемся к акселерометру.

import random, math
random.seed(7)

dt = 0.1
alpha = 0.95
true_angle = 0.0
comp = 0.0

for k in range(5):
    true_angle += 2.0 * dt                       # реально крутимся 2 °/с
    gyro_rate = 2.0 + random.uniform(-0.3, 0.3)  # шумная скорость
    acc_angle = true_angle + random.uniform(-3, 3)  # шумный абс. угол
    comp = alpha * (comp + gyro_rate*dt) + (1-alpha) * acc_angle
    print("истина", round(true_angle, 2),
          "акс", round(acc_angle, 2),
          "фильтр", round(comp, 2))

Вывод:

истина 0.2 акс -1.89 фильтр 0.09
истина 0.4 акс -2.17 фильтр 0.17
истина 0.6 акс -0.21 фильтр 0.34
истина 0.8 акс 0.84 фильтр 0.53
истина 1.0 акс 0.6 фильтр 0.7

Как читать результат

Видно, что акселерометр (столбец «акс») сильно скачет вокруг истины, а выход фильтра идёт гладко и постепенно приближается к настоящему углу. Гироскоп задаёт быстрый ход, акселерометр не даёт фильтру уплыть в долгую.

Как работает под капотом

По сути $\alpha (\theta_{k-1} + \omega\Delta t)$ — это фильтр высоких частот по гироскопу, а $(1-\alpha)\theta_{acc}$ — фильтр низких частот по акселерометру; вместе их частотные характеристики дополняют друг друга до единицы — отсюда «комплементарный». Параметр $\alpha$ задаёт частоту раздела: ближе к 1 — больше доверия гироскопу и медленнее коррекция дрейфа. Фильтр дёшев (пара умножений), не требует модели шума и потому стоит в большинстве любительских автопилотов и стабилизаторов.

Частые ошибки

• Брать $\alpha$ слишком близко к 1 — дрейф гироскопа не успеет скорректироваться акселерометром.
• Брать $\alpha$ слишком малым — выход начнёт дёргаться вслед за шумом акселерометра.
• Забыть умножить угловую скорость на $\Delta t$ — приращение угла будет неверным.

Итог

• Комплементарный фильтр сливает интеграл гироскопа и угол акселерометра.
• Формула: $\theta_k = \alpha(\theta_{k-1} + \omega\Delta t) + (1-\alpha)\theta_{acc}$.
• $\alpha$ задаёт баланс: ближе к 1 — доверие гироскопу, медленнее коррекция дрейфа.