Стохастические системы и фильтр Калмана

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

Линейная стохастическая система

Дискретная модель: x_{t+1} = A·x_t + B·u_t + w_t (системный шум), y_t = C·x_t + v_t (шум измерений).

Шумы: w_t ~ N(0, Q), v_t ~ N(0, R), независимы между собой и от x_t. Q (n×n), R (p×p) — ковариационные матрицы.

Задача: на основе всех измерений y_0, y_1, ..., y_t восстановить наилучшую оценку x̂_{t|t} = E[x_t | y_0, ..., y_t].

В байесовской интерпретации: вычислить апостериорное распределение p(x_t | y_{0:t}). Для линейно-гауссовых систем оно остаётся гауссовым, и фильтр сводится к рекуррентному обновлению среднего и ковариации.

Алгоритм фильтра Калмана

Каждый шаг разбивается на два подшага.

Шаг предсказания (time update):

x̂_{t|t−1} = A·x̂_{t−1|t−1} + B·u_{t−1} (среднее по динамике).
P_{t|t−1} = A·P_{t−1|t−1}·Aᵀ + Q (ковариация: динамика + шум).

Шаг обновления (measurement update):

Невязка (innovation): r_t = y_t − C·x̂_{t|t−1}.
Калман-гэйн: K_t = P_{t|t−1}·Cᵀ·(C·P_{t|t−1}·Cᵀ + R)⁻¹.
Обновлённая оценка: x̂_{t|t} = x̂_{t|t−1} + K_t·r_t.
Обновлённая ковариация: P_{t|t} = (I − K_t·C)·P_{t|t−1}.

Интерпретация K_t. Это «доверие» к измерению против предсказания. Если шум измерений R мал, K_t ≈ C⁻¹ — почти полностью полагаемся на y_t. Если R велик, K_t ≈ 0 — больше верим динамической модели.

Свойства

Оптимальность. Среди всех линейных оценщиков фильтр Калмана минимизирует E[||x_t − x̂_t||²] (BLUE — Best Linear Unbiased Estimator). Для гауссовых шумов — оптимален среди всех (нелинейных) оценщиков.

Стационарный фильтр. При t → ∞ (для time-invariant A, B, C) ковариация P_{t|t} → P_∞ — решение алгебраического уравнения Риккати фильтра. K → K_∞ — постоянный gain. Это даёт устройство фиксированной структуры — крайне важно для встраиваемых систем.

Численный пример: отслеживание объекта

Состояние x = (px, py, vx, vy) — позиция и скорость. Динамика постоянной скорости с шумом ускорения: x_{t+1} = [I_2, dt·I_2; 0, I_2]·x_t + w_t, dt = 0.1 c, Q = diag(0, 0, 0.01, 0.01). Измерения y = (px, py) (только позиция, например, GPS): C = [I_2, 0], R = diag(5, 5) — шум 5 м (стандартное отклонение).

Стартовая оценка: x̂_{0|0} = (0, 0, 0, 0), P_{0|0} = diag(100, 100, 10, 10).

После 100 шагов фильтр стабилизируется: K_∞ ≈ [[0.78, 0; 0, 0.78; 5.7, 0; 0, 5.7]] — сильное доверие к измерениям для позиции, оценка скорости — производная сглаженных измерений. Среднеквадратичная ошибка позиции ≈ 1.5 м (вместо 5 м без фильтрации), скорости ≈ 0.3 м/с.

Нелинейные расширения

Расширенный фильтр Калмана (EKF). Система: x_{t+1} = f(x_t, u_t) + w_t, y_t = h(x_t) + v_t. Линеаризация на каждом шаге: F_t = ∂f/∂x|{x̂_t}, H_t = ∂h/∂x|{x̂_{t|t−1}}. Применяем алгоритм Калмана с F_t, H_t вместо A, C.

EKF субоптимален из-за ошибок линеаризации. Может расходиться при сильных нелинейностях.

Unscented Kalman Filter (UKF, Julier-Uhlmann 1997). Вместо линеаризации — пропускание набора детерминированных «sigma-points» через нелинейные f, h, и оценка статистики выхода. Точность до 3-го момента включительно. Часто превосходит EKF при умеренных вычислительных затратах.

Particle Filter (Sequential Monte Carlo). Аппроксимация апостериорного распределения p(x_t | y_{0:t}) набором взвешенных «частиц» {x_t^{(i)}, w_t^{(i)}}. Не предполагает гауссовости, работает для любых распределений. Дороже вычислительно (1000-10000 частиц), но универсален.

Реальные применения

GPS и инерциальная навигация (INS). Объединение акселерометров (короткое время — точно, дрейф) и GPS (раз в секунду — точно, без дрейфа) через EKF. Точность 1-10 м в смартфоне, < 0.1 м в авиации.
Apollo (1969) и Space Shuttle. Навигация к Луне рассчитывалась на бортовом компьютере фильтром Калмана. Программа MIT (Battin), руководитель — Kalman лично консультировал.
Финансы. Оценка скрытой волатильности (stochastic volatility models) — нелинейный фильтр Калмана. Trading-стратегии market-making используют KF для оценки fair value.
Робототехника и SLAM. Simultaneous Localization and Mapping: робот строит карту и одновременно локализуется в ней. EKF-SLAM, GraphSLAM — основа автономных пылесосов Roomba, складских роботов Amazon Kiva.
Биомедицина. Носимые ECG-мониторы с фильтрацией артефактов движения. Оценка сатурации кислорода в пульсоксиметрах.

Задание. Корабль с состоянием x = (px, py, vx, vy). Динамика постоянной скорости + процессный шум Q = 0.1·I (на скорости). Измерения GPS: y = (px, py) + N(0, R = 25·I). Шаг dt = 1 с, симуляция 200 с. Истинная траектория: круговое движение радиуса 100 м с угловой скоростью 0.05 рад/с (начало (100, 0)). (а) Реализуйте дискретный фильтр Калмана. (б) Сгенерируйте истинные данные, зашумлённые GPS-измерения, и оценки фильтра. (в) Постройте на одном графике три траектории. (г) Вычислите RMSE позиции для измерений и для фильтра. (д) Эксперимент: измените Q = 0.01·I (мы «уверены», что скорость постоянна) — как это повлияет на отслеживание манёвров?