Фильтр Калмана: оптимальное слияние модели и измерений

Жемчужина теории оценивания: оптимально сливаем модель и шумные измерения.

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

Зачем нужен фильтр Калмана

Обычный наблюдатель хорош, но как выбрать его коэффициенты, если и модель неточна, и датчик шумит? Фильтр Калмана (Рудольф Калман, 1960) отвечает на это оптимально: он математически находит наилучший баланс между доверием к модели и доверием к измерению, исходя из их шумов. Это один из самых влиятельных алгоритмов XX века — он вёл корабли Apollo на Луну, работает в GPS, инерциальных навигациях, стабилизации дронов, трекинге объектов. Понять его 1D-версию — значит ухватить идею, лежащую в основе всей современной навигации.

Две фазы: предсказание и коррекция

Фильтр Калмана хранит не только оценку x, но и свою неуверенность в ней P (дисперсию ошибки). Работает в два шага:

Предсказание. По модели предсказываем новое состояние. Неуверенность P растёт — модель не идеальна, со временем мы «расплываемся».
Коррекция. Приходит измерение. Вычисляем коэффициент Калмана K — насколько довериться измерению против предсказания. Подтягиваем оценку к измерению на величину K·(невязка). Неуверенность P падает — мы узнали новое.

Коэффициент K — сердце фильтра: если датчик точный (малый шум), K близок к 1 — верим измерению; если датчик шумный, K мал — больше верим модели. Фильтр сам, шаг за шагом, находит оптимальный баланс.

Как работает под капотом: 1D фильтр Калмана

Оценим постоянную истинную температуру по шумным показаниям датчика. Фильтр должен «отсеять» шум и сойтись к истине, причём неуверенность P будет уменьшаться.

import random
random.seed(42)
true_value = 25.0       # истинная температура (нам неизвестна)
meas_var = 4.0          # дисперсия шума датчика (R)
proc_var = 0.01         # дисперсия модели (Q): температура почти постоянна

x_est = 20.0            # начальная оценка (грубая)
P = 5.0                 # начальная неуверенность

print("шаг  измерение  оценка   K     P")
for step in range(1, 11):
    # --- ПРЕДСКАЗАНИЕ ---
    x_pred = x_est               # модель: значение не меняется
    P_pred = P + proc_var        # неуверенность растёт
    # --- ИЗМЕРЕНИЕ (с шумом) ---
    z = true_value + random.gauss(0, meas_var**0.5)
    # --- КОРРЕКЦИЯ ---
    K = P_pred / (P_pred + meas_var)   # коэффициент Калмана
    x_est = x_pred + K*(z - x_pred)
    P = (1 - K)*P_pred                 # неуверенность падает
    print(f"{step:3d}  {z:8.2f}  {x_est:6.2f}  {K:.3f}  {P:.3f}")
print(f"истина={true_value:.1f}, финальная оценка={x_est:.2f}")

Вывод:

шаг  измерение  оценка   K     P
  1     24.71   22.62  0.556  2.224
  2     24.65   23.35  0.358  1.434
  3     24.78   23.73  0.265  1.061
  4     26.40   24.29  0.211  0.845
  5     24.74   24.37  0.176  0.704
  6     22.01   24.01  0.151  0.606
  7     25.66   24.23  0.133  0.534
  8     24.47   24.26  0.120  0.479
  9     24.57   24.29  0.109  0.435
 10     25.23   24.39  0.100  0.401
истина=25.0, финальная оценка=24.39

Смотрите, что произошло. Оценка стартовала с грубых 20 и, несмотря на скачущие от шума измерения (то 22, то 26), плавно сошлась к истинным 25. Коэффициент K уменьшался от шага к шагу: чем увереннее фильтр (меньше P), тем меньше он реагирует на каждое новое шумное измерение. Это и есть оптимальное слияние модели и данных — фильтр доверяет измерениям ровно настолько, насколько они того заслуживают.

Почему это оптимально

Магия коэффициента K = P_pred/(P_pred + R) в том, что он минимизирует ошибку оценки в среднеквадратичном смысле — это математически доказано для линейных систем с гауссовым шумом. Никакой другой линейный фильтр не даст оценку точнее. Полная (многомерная) версия использует матрицы вместо чисел, но логика та же: предсказали, измерили, взвесили по неуверенности, слили. Эта же двухфазная схема лежит в GPS, где «модель движения» сливается с шумными спутниковыми измерениями.

Apollo, GPS и дальше

Стоит проникнуться масштабом этого алгоритма. Фильтр Калмана был впервые серьёзно применён в программе Apollo: бортовой компьютер сливал данные инерциальных датчиков с редкими измерениями положения, оценивая, где корабль находится и куда летит, — без этого высадка на Луну была бы невозможна. Сегодня он работает в каждом смартфоне: GPS-приёмник сливает шумные спутниковые измерения с моделью движения, давая плавную траекторию; модуль ориентации сливает гироскоп, акселерометр и магнитометр в устойчивую оценку положения телефона. Для нелинейных систем (а почти всё реальное нелинейно) применяют расширенный фильтр Калмана (EKF), линеаризующий модель на каждом шаге, и ансцентный (UKF), работающий через выборку точек. Но сердце у всех одно — двухфазная схема «предсказание плюс взвешенная коррекция», которую вы только что запрограммировали в десяти строках.

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

Неверно задать шумы Q и R. Если завысить доверие к датчику (малый R), фильтр будет дёргаться за шумом; если занизить — медленно реагировать на реальные изменения.
Забыть, что P должна расти на предсказании. Без роста неуверенности фильтр «застывает» и перестаёт слушать измерения.
Применять к сильно нелинейным системам напрямую. Для них нужны расширенный (EKF) или ансцентный (UKF) варианты.

Итоги

Фильтр Калмана оптимально сливает предсказание модели и шумное измерение.
Две фазы: предсказание (P растёт) и коррекция (P падает) через коэффициент Калмана K.
K взвешивает доверие к измерению против модели; это основа GPS и навигации.