Комплементарный фильтр: слияние гироскопа и акселерометра

Простой и надёжный способ объединить два датчика в одну хорошую оценку угла — комплементарный фильтр.

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

Идея и формула

На каждом шаге мы интегрируем гироскоп (получаем быстрое предсказание угла) и тут же чуть-чуть подтягиваем результат к показанию акселерометра (он не дрейфует). Формула одного шага:

$$ \theta_{k} = \alpha\,(\theta_{k-1} + \omega\,dt) + (1-\alpha)\,\theta_{\text{accel}} $$

Коэффициент $\alpha$ близок к единице (обычно 0.95–0.99): почти всё доверие — гироскопу на коротком шаге, но маленькая доля $(1-\alpha)$ постоянно «лечит» дрейф по акселерометру. Высокочастотную часть берём из гироскопа, низкочастотную — из акселерометра; вместе они дополняют друг друга (отсюда название).

Симуляция

Смоделируем разворот аппарата к 30° с шумным акселерометром и дрейфующим гироскопом и проверим, что фильтр сходится к истине.

import math, random
random.seed(42)
dt = 0.01
alpha = 0.98
angle = 0.0
gyro_bias = 0.5
true_angle = 0.0
for i in range(500):     # 5 секунд
    true_angle = 30.0 * (1 - math.exp(-i * dt / 1.0))
    rate_true = (30.0 / 1.0) * math.exp(-i * dt / 1.0)
    gyro = rate_true + gyro_bias + random.gauss(0, 0.3)
    accel = true_angle + random.gauss(0, 3.0)
    angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel
print("Истинный угол:", round(true_angle, 2), "град")
print("Оценка фильтра:", round(angle, 2), "град")
print("Ошибка:", round(abs(angle - true_angle), 2), "град")

Вывод:

Истинный угол: 29.80 град
Оценка фильтра: 29.69 град
Ошибка: 0.11 град

Несмотря на смещение гироскопа (0.5°/с) и сильный шум акселерометра (разброс 3°), фильтр выдал оценку с ошибкой всего около 0.1°. Дрейф погашен акселерометром, шум — сглажен интегралом гироскопа.

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

Коэффициент $\alpha$ задаёт постоянную времени фильтра: большое $\alpha$ — больше доверия гироскопу, медленнее реакция на коррекцию (хорошо гасит шум акселерометра, но дольше убирает дрейф); меньшее $\alpha$ — быстрее тянется к акселерометру, но пропускает больше шума. Это компромисс. Более сложный фильтр Калмана делает то же самое, но подбирает «коэффициент доверия» автоматически по статистике шумов.

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

• Брать $\alpha$ слишком маленьким — в оценку просочится шум акселерометра.
• Брать $\alpha=1$ — фильтр выродится в чистый гироскоп с дрейфом.
• Забыть, что $\alpha$ связан с шагом $dt$: при другой частоте опроса коэффициент надо пересчитать.

Итог

• Комплементарный фильтр: $\theta=\alpha(\theta+\omega dt)+(1-\alpha)\theta_{\text{accel}}$.
• Гироскоп даёт быстрые изменения, акселерометр гасит дрейф.
• $\alpha$ — компромисс между шумом и скоростью коррекции; фильтр Калмана подбирает его сам.