Sensor Fusion

Апрель 1970. Аполлон-13 на пути к Луне. Кислородный бак взрывается. Навигационный компьютер AGC работает только с гироскопами - ни GPS, ни внешних сигналов. Через 200 часов дрейф накопился бы на десятки километров. Астронавты выжили в том числе потому, что за 9 лет до этого Рудольф Калман нашёл математически оптимальный способ объединять шумные данные с несовершенной физической моделью. Сегодня этот же алгоритм работает в Tesla, SpaceX и каждом смартфоне с GPS. Ни один сенсор не может доверять себе в одиночку - вот почему sensor fusion стал основой автономных систем.

• **Tesla Autopilot** - 8 камер + IMU fusion в реальном времени, EKF для трекинга объектов со скоростью 36 кадров/сек
• **Waymo One** - 29 камер + 5 LIDAR + 6 радаров в едином KF-стеке; 20 миллионов км автономных поездок к 2024
• **SpaceX Falcon 9** - фильтр Калмана для посадки первой ступени: IMU + оптические сенсоры + GPS, точность 10 см на посадочной площадке
• **Смартфон GPS** - каждый чип фильтрует сигнал 4-12 спутников через взвешенный least-squares (упрощённый KF), отсюда плавное движение точки на карте
• **Хирургические роботы Da Vinci** - fusion данных энкодеров и силовых сенсоров для субмиллиметровой точности при дрожании рук хирурга

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

1970 год. Аполлон-13. Навигационный компьютер AGC работает только с инерциальной системой - акселерометры, гироскопы. Никакого внешнего сигнала. Через 200 часов полёта накопленный дрейф гироскопа давал ошибку в несколько десятков километров. Астронавты выжили не потому что сенсоры были точными - они выжили потому что за 9 лет до этого Рудольф Калман опубликовал статью, которую NASA применило в программе Аполлон. Та статья описывала, как объединять несовершенные измерения с несовершенной физической моделью - и получать лучший ответ, чем даёт любой источник по отдельности.

Фильтр Калмана - это не алгоритм фильтрации в смысле DSP. Это рекуррентный байесовский оценщик для линейных систем с гауссовским шумом. Каждый шаг состоит из двух фаз: предсказание (что ожидается исходя из модели движения) и коррекция (как измерение отклоняется от предсказания). Формально это уравнения:

Где $\hat{x}$ - вектор состояния (позиция, скорость), $P$ - матрица ковариации (неопределённость), $F$ - матрица перехода состояния, $Q$ - ковариация шума процесса. После предсказания приходит измерение $z_k$ - и начинается шаг коррекции.

$K_k$ - это коэффициент Калмана, или "усиление доверия к измерению". Когда сенсор шумный (большое $R_k$) - $K_k$ маленькое, фильтр доверяет модели. Когда модель неточная (большое $Q_k$) - $K_k$ большое, фильтр доверяет сенсору. Это автоматически найденный оптимальный баланс в классе линейных несмещённых оценок.

Kalman filter - это MMSE (Minimum Mean Square Error) оценщик для линейных гауссовских систем. Tesla Autopilot использует его вариации для трекинга объектов, SpaceX - для управления ракетами, GPS-чипы в каждом смартфоне - для сглаживания позиции. Одна математика, тысячи применений.

Extended Kalman Filter: нелинейный мир

Классический Kalman работает только с линейными системами. Но настоящий мир жесток: GPS даёт координаты в сферической системе, камера - пиксели через нелинейную проекцию, угол поворота колеса связан со скоростью через тригонометрию. Машина с IMU + GPS - нелинейная система насквозь. Extended Kalman Filter (EKF) - это инженерное решение: линеаризовать нелинейность в каждой точке с помощью матрицы Якоби.

Вместо линейного $F$ в EKF используется якобиан нелинейной функции перехода $f$, вычисленный в текущей оценке состояния:

Это первый порядок Тейлора. Хорошо работает когда функция плавная и состояние не меняется резко. Waymo использует EKF для fusion данных LIDAR + IMU в реальном времени - 100 Гц, 64 луча LIDAR, 6-осевой IMU, всё в один вектор состояния. Когда машина резко поворачивает, якобиан пересчитывается каждый шаг - именно поэтому EKF дороже линейного Kalman по вычислениям.

EKF - рабочая лошадь автономных систем. Недостаток: линеаризация вносит ошибки при сильной нелинейности (резкие манёвры, большие углы). Тогда на сцену выходит Unscented Kalman Filter (UKF) - он распространяет не производную, а набор "сигма-точек" через нелинейную функцию. Точнее, но вдвое дороже.

IMU + GPS + LIDAR: fusion в реальных системах

Tesla Vision Stack 2023: 8 камер, акселерометр, гироскоп - и никакого LIDAR (Tesla убрала ультразвук в 2022). Waymo One: 29 камер + 5 LIDAR + 6 радаров + IMU. Разные архитектуры fusion - но оба делают одно: превращают 10+ потоков данных с разной частотой, задержкой и шумом в единый трёхмерный вектор состояния мира.

Практический pipeline выглядит так: IMU работает на 200-1000 Гц и даёт высокочастотные данные об ускорении и угловой скорости. GPS - 10 Гц, точность 3-5 метров на открытом небе, до нуля в тоннеле. LIDAR - 10-20 Гц, облако точек 100K+ в секунду. Камеры - 30-60 Гц, 2D пиксели, нужна геометрическая калибровка для перевода в 3D.

IMU используется как основной источник предсказания - высокая частота, малая задержка. GPS и LIDAR - как источники коррекции с разной частотой. Когда GPS теряется (тоннель, городское ущелье), EKF продолжает работать только на IMU и LIDAR - это называется dead reckoning. Ошибка накапливается, но медленно. Когда GPS возвращается - коррекция прыгает, сглаживается интегратором.

LIDAR + camera fusion - отдельная история. LIDAR даёт точное расстояние, но не знает цвет и нет семантики. Камера даёт цвет и семантику (это пешеход, это знак), но плохо с расстоянием. Fusion: для каждой точки облака LIDAR находим соответствующий пиксель камеры через матрицу экструзии (внешняя калибровка). Получаем "цветное облако точек" - 3D позиция + RGB + semantic class. Именно так Waymo строит HD-карту в реальном времени.

Узкое место sensor fusion - калибровка. Матрица экструзии LIDAR-камера должна быть точной до долей градуса. Смещение на 0.5 градуса при расстоянии 50м = ошибка 43 см. Именно поэтому все производители автономных авто используют специальные калибровочные стенды и перекалибруются после каждого технического обслуживания.

Ключевые идеи

• **Kalman filter** - оптимальный MMSE-оценщик для линейных гауссовских систем: два шага predict-update, коэффициент $K_k$ автоматически балансирует доверие к модели и сенсору
• **Extended Kalman Filter** - линеаризует нелинейные функции через якобиан в текущей точке; это первый порядок Тейлора, хорошо работает при плавных изменениях
• **Sensor fusion на практике** - IMU как высокочастотный предиктор (200+ Гц), GPS/LIDAR как корректоры; при потере GPS система продолжает работать на dead reckoning
• **LIDAR + camera fusion** - 3D позиция от LIDAR + семантика от камеры через матрицу экструзии; точность зависит от калибровки до долей градуса
• **Усреднение - не fusion**: оптимальный вес каждого источника определяется его матрицей шума $R$, не эвристикой

Связанные темы

Sensor fusion опирается на теорию вероятностей и ведёт к более сложным задачам восприятия:

• SLAM — Одновременная локализация и картирование строится поверх EKF fusion
• LiDAR и Point Clouds — Сырые данные LIDAR - входной сигнал для LIDAR+camera fusion
• Нормальное распределение — Гауссовская модель шума - математическая основа фильтра Калмана
• Автономные авто — Производственный кейс: IMU + GPS + LIDAR + camera fusion в одном стеке

Вопросы для размышления

• Почему Waymo не убрала LIDAR в пользу чистого camera-only подхода (как Tesla), если LIDAR стоит $7500+ за единицу?
• Kalman filter предполагает гауссовский шум. Что происходит с точностью, когда шум не гауссовский - например, внезапное отражение GPS-сигнала от здания (multipath)?
• Particle filter - альтернатива EKF для произвольных (не гауссовских) распределений. Когда имеет смысл использовать его вместо EKF, учитывая, что particle filter требует 1000+ частиц?

Связанные уроки

• prob-11-normal — Гауссовский шум - основа модели ошибок в фильтре Калмана
• prob-08-variance — Ковариация - язык неопределённости в Kalman filter
• rob-07 — SLAM строится поверх Sensor Fusion как фундамента
• rob-13 — Автономные авто - главный продакшн-кейс EKF и fusion
• dsp-01 — Сигналы и шум - DSP-база для понимания измерений сенсоров
• prob-04-bayes