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}\)), фильтр Калмана остаётся вершиной математической инженерии.