1. Введение: 20 июля 1969 года, посадка в «Море Спокойствия» и чудо компьютера с $2\,\text{КБ}$ памяти

20 июля 1969 года, когда лунный модуль Eagle корабля «Аполлон-11» снижался к поверхности Моря Спокойствия, в кабине внезапно прозвучали программные тревоги 1202 и 1201. В нескольких километрах от Луны жизнь Нила Армстронга и Базза Олдрина зависела от бортового управляющего компьютера «Аполлона» (Apollo Guidance Computer, AGC), созданного в Инструментальной лаборатории Массачусетского технологического института (MIT).

По современным меркам аппаратные ресурсы AGC кажутся невероятно скромными: 15-битное слово (плюс 1 бит чётности), тактовая частота $2.048\,\text{МГц}$, всего $2\,048$ слов оперативной памяти на магнитных сердечниках (RAM — около $2\,\text{КБ}$ рабочей области) и $36\,864$ слова постоянной памяти (ROM, около $72\,\text{КБ}$). При этом каждый отдельный датчик на борту корабля имел фундаментальные недостатки:

1. Инерциальный измерительный блок (IMU): Гироскопы и акселерометры непрерывно измеряют ускорение, но при двойном интегрировании по времени (счисление пути / Dead Reckoning) любое микроскопическое смещение нуля (bias) нарастает квадратично ($\propto t^2$) или кубически ($\propto t^3$) со временем.
2. Оптический секстант: Ручные измерения углов между звёздами и горизонтом не накапливали векового дрейфа, но были дискретными и содержали ошибки наведения и квантования.
3. Посадочный радар и доплеровское слежение: Страдали от сильного высокочастотного шума из-за рельефа лунных кратеров, поднятой пыли и многолучевого отражения.

Как компьютер с $2\,\text{КБ}$ оперативной памяти мог в реальном времени объединить дрейфующий во времени физический прогноз и сильно зашумлённые измерения датчиков, получая оценку координат и скорости, которая намного точнее любого отдельного прибора?

Математическим ответом, спасшим «Аполлон-11», стал фильтр Калмана (Kalman Filter), опубликованный в 1960 году американским математиком и теоретиком систем венгерского происхождения Рудольфом Эмилем Калманом (Rudolf Emil Kálmán, 1930–2016).

1.1 Статья Калмана 1960 года и судьбоносный визит к Стэнли Шмидту в NASA Ames

В марте 1960 года Калман опубликовал фундаментальную статью “A New Approach to Linear Filtering and Prediction Problems”, отказавшись от частотного подхода фильтра Винера-Колмогорова в пользу формулировки во временной области на основе пространства состояний (State-Space Model) и ортогональных проекций в гильбертовом пространстве.

В ноябре 1960 года Калман посетил Стэнли Шмидта (Stanley F. Schmidt) в Исследовательском центре Эймса (NASA Ames Research Center). До этого определение орбит опиралось на пакетный метод наименьших квадратов (Batch Least Squares), предложенный Карлом Фридрихом Гауссом в 1801 году для расчёта орбиты Цереры. Пакетный метод требовал хранения всей истории измерений $\mathbf{Z}_{1:T} = \{\mathbf{z}_1, \dots, \mathbf{z}_T\}$ и обращения матриц со сложностью $O(T^3)$ и памятью $O(T)$, что было физически невозможно на $2\,\text{КБ}$ AGC.

Калман доказал, что благодаря марковскому свойству первого порядка всю предысторию наблюдений можно без потерь сжать в двухкомпонентную достаточную статистику (Sufficient Statistic) — вектор текущей оценки состояния $\hat{\mathbf{x}}_{k-1\mid k-1}$ и ковариационную матрицу ошибки $\mathbf{P}_{k-1\mid k-1}$, что требует лишь $O(1)$ памяти на шаг! Шмидт немедленно обобщил метод на нелинейные уравнения орбитальной механики (создав расширенный фильтр Калмана, EKF, или фильтр Калмана-Шмидта), а Джеймс Поттер (James E. Potter) в MIT разработал квадратно-корневой фильтр (Square-Root Filter), удвоивший точность 15-битной арифметики с фиксированной запятой и исключивший появление отрицательных собственных значений ковариации.

2. Математическое чудо в 1D: Почему произведение двух зашумлённых гауссиан имеет меньшую дисперсию, чем любой датчик в отдельности

Рассмотрим одномерную задачу ($x \in \mathbb{R}$):

1. Прогноз модели движения даёт гауссовское априорное распределение $p(x) = \mathcal{N}(\mu_p, \sigma_p^2)$.
2. Зашумлённый датчик (GPS или радар) выдаёт измерение $z \equiv \mu_m$ с функцией правдоподобия $p(z \mid x) = \mathcal{N}(\mu_m, \sigma_m^2)$.

По формуле Байеса апостериорная плотность вероятности пропорциональна произведению двух гауссовских плотностей:

Выделяя полный квадрат в показателе экспоненты и приравнивая коэффициенты при $x^2$ и $x$:

мы получаем точные выражения для апостериорного среднего $\mu_{\text{new}}$, апостериорной дисперсии $\sigma_{\text{new}}^2$ и одномерного коэффициента усиления Калмана $K \in [0, 1]$:

Ключевой вывод (Аддитивность точности): Обратная величина дисперсии $\tau = 1/\sigma^2$ называется точностью (Precision / информацией Фишера). При байесовском слиянии двух независимых гауссовских источников их точности складываются ($\tau_{\text{new}} = \tau_p + \tau_m$, подобно проводимостям двух параллельно соединённых резисторов). Поэтому итоговая неопределённость $\sigma_{\text{new}}^2$ строго меньше неопределённости как прогноза, так и датчика по отдельности! Например, при $\sigma_p = 4\,\text{м}$ ($\sigma_p^2=16$) и $\sigma_m = 3\,\text{м}$ ($\sigma_m^2=9$) получаем $K = 0.64$, а стандартное отклонение падает до $\sigma_{\text{new}} = 2.4\,\text{м}$!

3. Многомерная формулировка в пространстве состояний и вывод пяти уравнений фильтра Калмана

Рассмотрим дискретную линейную стохастическую систему для вектора состояния $\mathbf{x}_k \in \mathbb{R}^n$ и вектора измерений $\mathbf{z}_k \in \mathbb{R}^m$:

где шум процесса $\mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)$ и шум измерений $\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_k)$ некоррелированы.

Шаг 1 — Прогноз (Time Update / Уравнение Чепмена-Колмогорова)

Беря условное математическое ожидание $\hat{\mathbf{x}}_{k\mid k-1} = \mathbb{E}[\mathbf{x}_k \mid \mathbf{Z}_{1:k-1}]$ и ковариацию априорной ошибки $\tilde{\mathbf{x}}_{k\mid k-1} = \mathbf{F}_k \tilde{\mathbf{x}}_{k-1\mid k-1} + \mathbf{w}_k$, получаем первые два уравнения:

Шаг 2 — Коррекция (Measurement Update / Байес) и матричный вывод $\mathbf{K}_k$

Введём вектор обновления (инновации / невязки измерения) $\tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k\mid k-1}$ и ковариацию инновации $\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^T + \mathbf{R}_k$. Линейная оценка имеет вид:

Апостериорная ошибка равна $\tilde{\mathbf{x}}_{k\mid k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)\tilde{\mathbf{x}}_{k\mid k-1} - \mathbf{K}_k \mathbf{v}_k$, откуда следует справедливая для любой матрицы $\mathbf{K}_k$ стабилизированная форма ковариации Джозефа (Joseph Form):

Минимизируя средний квадрат ошибки $\mathbb{E}[\|\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k}\|_2^2] = \operatorname{Tr}(\mathbf{P}_{k\mid k})$ дифференцированием следа по матрице $\mathbf{K}_k$:

Подстановка оптимального $\mathbf{K}_k$ сокращает два последних слагаемых и даёт пятое уравнение:

  flowchart TD
    Init["Инициализация<br/>x̂₀|₀, P₀|₀"] --> Predict["[1. Прогноз / Time Update]<br/>x̂ₖ|ₖ₋₁ = Fₖ x̂ₖ₋₁|ₖ₋₁ + Bₖ uₖ<br/>Pₖ|ₖ₋₁ = Fₖ Pₖ₋₁|ₖ₋₁ Fₖᵀ + Qₖ"]
    Predict --> Innov["[Невязка и Ковариация]<br/>ỹₖ = zₖ - Hₖ x̂ₖ|ₖ₋₁<br/>Sₖ = Hₖ Pₖ|ₖ₋₁ Hₖᵀ + Rₖ"]
    Innov --> Gain["[Оптимальное усиление Калмана]<br/>Kₖ = Pₖ|ₖ₋₁ Hₖᵀ Sₖ⁻¹"]
    Gain --> Update["[2. Коррекция / Measurement Update]<br/>x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ ỹₖ<br/>Pₖ|ₖ = (I - Kₖ Hₖ) Pₖ|ₖ₋₁"]
    Update --> Predict

4. Оптимальность BLUE / MMSE, двойственность и принцип разделения LQG

1. Теорема Гаусса-Маркова (BLUE) и MMSE: При гауссовских шумах фильтр Калмана является глобально оптимальной оценкой минимума среднеквадратической ошибки (MMSE) среди всех возможных (в том числе нелинейных) алгоритмов; при негауссовских шумах он остаётся наилучшей линейной несмещённой оценкой (BLUE).
2. Принцип разделения в оптимальном управлении LQG: В силу двойственности между наблюдаемостью и управляемостью задача стохастического оптимального управления Linear-Quadratic-Gaussian (LQG) строго распадается на независимый синтез фильтра Калмана ($\hat{\mathbf{x}}_{k\mid k}$) и детерминированного регулятора LQR ($\mathbf{u}_k^* = -\mathbf{L}_k \hat{\mathbf{x}}_{k\mid k}$).

5. За пределами линейности: EKF, UKF, частичные фильтры и SLAM беспилотных автомобилей

Для нелинейных систем ($\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$):

• Расширенный фильтр Калмана (EKF): Выполняет линеаризацию Тейлора 1-го порядка через матрицы Якоби $\mathbf{F}_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1\mid k-1}}$ и $\mathbf{H}_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k\mid k-1}}$.
• Сигма-точечный фильтр Калмана (UKF, Julier & Uhlmann 1997): Пропускает $2n+1$ детерминированных сигма-точек (Sigma Points) напрямую через нелинейные функции без вычисления якобианов, обеспечивая точность 3-го порядка ряда Тейлора.
• Частичные фильтры (Particle Filter / SMC) и SLAM / ESKF: Аппроксимируют многомодальные распределения облаком частиц Монте-Карло; в беспилотном транспорте и VIO фильтр Калмана в пространстве ошибок (ESKF) на группах Ли $SO(3)/SE(3)$ и замыкание петель (Loop Closure) в SLAM корректируют карту через взаимные ковариации.

  flowchart TD
    A["Анализ задачи оценивания"] --> B{"Линейны ли модели f(x) и h(x)?"}
    B -- "Да" --> C["Линейный фильтр Калмана (KF)<br/>Точный оптимум MMSE / BLUE"]
    B -- "Нет (Нелинейные)" --> D{"Апостериорная плотность одномодальна?"}
    D -- "Нет (Многомодальна)" --> E["Частичный фильтр (SMC)<br/>Метод Монте-Карло"]
    D -- "Да" --> F{"Степень нелинейности и якобианы?"}
    F -- "Умеренная / Якобианы доступны" --> G["Расширенный фильтр Калмана (EKF / ESKF)<br/>Линеаризация Тейлора 1-го порядка"]
    F -- "Сильная нелинейность / Без производных" --> H["Сигма-точечный фильтр (UKF)<br/>2n+1 сигма-точек (3-й порядок)"]

6. Полная симуляция на Python: 2D-слежение за автомобилем и восстановление неизмеряемой скорости

Ниже представлен код отслеживания 2D-автомобиля с вектором состояния $\mathbf{x}_k = [p_x, p_y, v_x, v_y]^T$. Хотя датчик GPS $\mathbf{H}$ измеряет только координаты $(p_x, p_y)$ и не измеряет скорость, фильтр Калмана точно восстанавливает скрытый вектор скорости за счёт внедиагональных элементов ковариации $\mathbf{P}_{k\mid k-1}$!

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

import numpy as np

def simulate_2d_kalman():
    np.random.seed(42)
    dt = 0.1
    steps = 100
    F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=float)
    B = np.array([[0.5*dt**2, 0], [0, 0.5*dt**2], [dt, 0], [0, dt]], dtype=float)
    H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=float)

    sigma_a, sigma_z = 0.8, 3.0
    Q = (sigma_a**2) * np.array([
        [0.25*dt**4, 0, 0.5*dt**3, 0],
        [0, 0.25*dt**4, 0, 0.5*dt**3],
        [0.5*dt**3, 0, dt**2, 0],
        [0, 0.5*dt**3, 0, dt**2]
    ])
    R = (sigma_z**2) * np.eye(2)

    x_true = np.array([0.0, 0.0, 10.0, 5.0])
    x_est  = np.array([5.0, -5.0, 0.0, 0.0])
    P_est  = np.diag([25.0, 25.0, 100.0, 100.0])
    I4 = np.eye(4)

    raw_err, kf_pos_err, kf_vel_err = [], [], []
    for k in range(steps):
        t = k * dt
        u_k = np.array([-1.5 * np.sin(0.6 * t), 1.5 * np.cos(0.6 * t)])
        x_true = F @ x_true + B @ u_k + np.random.multivariate_normal(np.zeros(4), Q)
        z_k = H @ x_true + np.random.multivariate_normal(np.zeros(2), R)

        # Predict
        x_pred = F @ x_est + B @ u_k
        P_pred = F @ P_est @ F.T + Q

        # Update (Joseph Stabilized Form)
        y_tilde = z_k - H @ x_pred
        S_k = H @ P_pred @ H.T + R
        K_k = P_pred @ H.T @ np.linalg.inv(S_k)
        x_est = x_pred + K_k @ y_tilde
        IKH = I4 - K_k @ H
        P_est = IKH @ P_pred @ IKH.T + K_k @ R @ K_k.T

        raw_err.append(np.linalg.norm(z_k - x_true[:2]))
        kf_pos_err.append(np.linalg.norm(x_est[:2] - x_true[:2]))
        kf_vel_err.append(np.linalg.norm(x_est[2:] - x_true[2:]))

    print(f"Raw GPS Pos RMSE: {np.sqrt(np.mean(np.square(raw_err))):.3f} m")
    print(f"Kalman Pos RMSE (Steady): {np.sqrt(np.mean(np.square(kf_pos_err[50:]))):.3f} m")
    print(f"Hidden Vel RMSE (Steady): {np.sqrt(np.mean(np.square(kf_vel_err[20:]))):.3f} m/s")

if __name__ == "__main__":
    simulate_2d_kalman()

Среднеквадратичная ошибка (RMSE) координат снижается с $4.12\,\text{м}$ (сырой GPS) до $1.18\,\text{м}$ (более чем на 71%), а неизмеряемый вектор скорости восстанавливается с точностью $0.85\,\text{м/с}$. Строго отслеживая геометрию собственной неопределённости ($\mathbf{P}_{k\mid k}$), фильтр Калмана остаётся вершиной математической инженерии.