Робастный Variational Bayes Fusion UKF в городском каньоне: RMSE и NIS при аномальных сигналах GPS / Хабр

Карта путешествий в каньоне фильтров Калмана со Стьюдентом и Гауссом в измерительных шумах — ***Карта путешествий в каньоне*** *фильтров* ***Калмана*** ***со Стьюдентом и Гауссом*** *в измерительных шумах*

Робастность через комплексирование измерений и замену модели измерительного шума
1.1. Синтез двух подходов
1.2. Вычислительная сложность
1.3. Аномалии измерений в задачах потоковой фильтрации
1.4. Вариационно-байесовская аппроксимация как способ не уходить далеко (немного формул, по желанию)
Особенности реализации и конфигурация имитационной модели
2.1. Динамика CTRV и параметры конфигурации
2.2. Структура данных
Анализ результатов моделирования
3.1. Траектории
3.2. Механизм адаптации весов
3.3. Переходный процесс
3.4. Панели метрик

Введение

В «городском каньоне» GPS-сигнал подвержен эффектам многолучевого распространения и экранирования, что порождает аномалии в измерениях псевдодальности.
Классические фильтры Калмана, предполагающие аддитивный гауссовский шум, в таких условиях демонстрируют резкое падение точности оценки позиционирования.
В статье сравниваются два нелинейных фильтра Калмана:

классический Fusion UKF (централизованный мультисенсорный UKF с фиксированной гауссовской моделью шума)
мультисенсорный Variational Bayes Fusion UKF, в котором шум измерений моделируется распределением Стьюдента через вариационное байесовское приближение, а итеративная оценка скрытой масштабирующей переменной позволяет автоматически подавлять аномальные GPS-измерения.

В сценариях с имитацией GPS-аномалий по типу городского каньона Variational Bayes Fusion UKF более чем вдвое превзошёл Fusion UKF по RMSE позиционирования.

1. Робастность через комплексирование измерений и замену модели шума

В статье про "болеющий" фильтр Калмана численное моделирование продемонстрировало, что при сложной нелинейности моделей и комплексных шумах с выбросами сигма точечный UKF был недостаточно согласован по метрике NEES (нормализованная квадратичная ошибка оценки). Победителем оказался Particle Filter. Цена победы - существенно большая вычислительная сложность, приводящая к проблематичности применения Particle Filter как, в потоковом режиме с высокой динамикой, так и во встраиваемых системах с ограничениями по производительности.

В реальном мире практические системы редко соответствуют ограничениям линейности и гауссовости. Это обстоятельство обусловливает потребность в производительном нелинейном робастном фильтре.
В академической статье «Variational Robust Kalman Filters: Unified Framework», 2025 предлагаются обзор работ соответствующей тематики и обобщенная структура вариационно-байесовских фильтров, рассматриваемая через призму компромисса между вычислительно-эффективной адаптивностью и устойчивостью к выбросам.

Можно выделить, по крайней мере, два подхода к созданию робастных фильтров:

1. Комплексирование измерительных данных (Sensor Fusion).

Когда причина в факторах внешней среды (экранирование, переотражение сигналов GPS в городской застройке или электромагнитные помехи), применяют нечувствительные к этим факторам датчики другой физической природы (приемник GPS дополняется инерциальным модулем или модулем коррекции по цифровой карте).
Но одного этого недостаточно. Нужен механизм, который подскажет фильтру, каким способом объединить разнородные измерения в конкретный момент времени.
До недавнего времени в Sensor Fusion доминировали адаптивные подходы, основанные на эвристических методах коррекции блочных ковариаций измерительных шумов.
Основной их недостаток - сложность формулирования эвристик, адекватных реальной динамике.

2. Использование робастных распределений.

Если первый подход пытается «починить датчики», то второй - «исправить математику» самого фильтра. В его основе - отказ от линейно-гауссовской парадигмы.

В основе классического фильтра Калмана лежит гауссовская модель шума, где вероятность событий в хвостах распределения относительно мала. Столкнувшись с большой невязкой, алгоритм неявно решает задачу вероятностной классификации: вызвано ли это расхождение реальным изменением вектора состояния или появлением аномалии в измерении.
Поскольку матрица ковариации измерительных шумов фиксирована (ее можно трактовать как доверие к датчику), а невязка ей не соответствует, алгоритм оценивает правдоподобие сильного выброса низким.
Чтобы максимизировать итоговую вероятность, у него остается один путь - «подвинуть» саму оценку состояния навстречу измерению.

Таким образом, отсутствие механизма адаптации матрицы к текущей невязке вынуждает классификатор трактовать аномалию как достоверную информацию, что ведет к избыточному калмановскому усилению и искажению траектории. Решение этого противоречия может быть найдено переходом к распределениям с более тяжелыми хвостами, например к распределению Стьюдента (рис. 1).

Подобная замена наряду с отказом от константной матрицы позволяет выстроить механизм реагирования. Алгоритм обрабатывает измерение через взвешивание (устанавливает доверие к нему). В результате:

Измерения с большими невязками получают малый вес в процедуре обновления состояния.
Система перестает «тянуться» за выбросами, воспринимая их как шум, а не как полезный сигнал.

Рисунок 1. Сравнение веса хвостов распределений.

За подобную робастность приходится платить потерей аналитического решения.
Гауссовские распределения обладают удобным свойством сопряженности: при их перемножении в байесовском выводе распределения сохраняют класс, что дает готовые матричные формулы обновления. Но как только в рекурсивные уравнения фильтра вводится распределение Стьюдента аналитическое решение теряется и вычислить оптимальную оценку за один шаг становится невозможно.
В качестве альтернативы вычислительно тяжелым методам Монте-Карло (Particle Filter), используется вариационно-байесовский вывод (Variational Bayesian Inference). Этот подход позволяет аналитически аппроксимировать сложные невычислимые апостериорные распределения более простыми, сводя задачу к итеративной сходящейся процедуре оптимизации.
Именно этот класс методов сегодня считается золотой серединой между вычислительно-эффективной адаптивностью и устойчивостью к выбросам. О нем и пойдет речь далее.

Более подробно о вариационно-байесовском выводе:

1. Blei, Kucukelbir, McAuliffe (2017) "Variational Inference: A Review for Statisticians".
2. Tran, Nguyen, Dao (2021) "A Practical Tutorial on Variational Bayes".

1.1. Синтез подходов: робастное адаптивное комплексирование

Логичным этапом эволюции является объединение обоих направлений.

От первого подхода мы берем избыточность информации (разнородные датчики).
От второго - математический аппарат подавления аномалий.

В классическом Sensor Fusion разнотипные датчики, например, GPS и IMU связываются жесткими эвристическими правилами:

Матрица ковариации шума измерений задается заранее и фиксирует априорное «доверие» к каждому источнику.
Даже если условия меняются (сигнал GPS переотражается от зданий или IMU накапливает дрейф), веса в фильтре остаются неизменными. Алгоритм продолжает усреднять правильные и ошибочные показания, деградируя в точности.

Вариационно-Байесовский подход позволяет реализовать адаптивное комплексирование на принципиально ином уровне. В рамках VB-фильтрации матрицы шумов для каждого канала перестают быть константными. Они моделируются как случайные величины, параметры которых итеративно оцениваются в реальном времени вместе с вектором состояния системы.

Это создает динамический эффект «взвешивания доверия»:

Если GPS начинает выдавать неправдоподобные скачки, VB-фильтр фиксирует аномально высокую невязку/инновацию по этому каналу. Алгоритм пересчитывает апостериорное распределение для матрицы $R_{GPS}$ , увеличивая её ожидаемое значение (то есть «раздувает» дисперсию шума этого датчика).
В результате вес GPS-измерений в коэффициенте усиления снижается и фильтр плавно переносит приоритет на инерциальную навигацию IMU до тех пор, пока сигнал не стабилизируется.

Таким образом, мы получаем систему, которая не просто усредняет показания, а выбирает лучший источник правды в каждый момент времени, опираясь на байесовский вывод, а не на ручные настройки порогов или таймеров переключения.

Модель на базе распределения Стьюдента предоставляет удобный инструмент тонкой настройки фильтра: гиперпараметр $\nu$ (ню) — количество степеней свободы, который отвечает за тяжесть хвостов распределения.
Изменяя $\nu$ , можно плавно регулировать уровень толерантности фильтра к величине невязки. При $\nu \to \infty$ это эквивалент гауссовской модели, а при малых значениях, например, $\nu = 2$ фильтр агрессивно снижает веса для измерений с большими невязками.

Это компромиссная настройка между чувствительностью системы к реальной динамике объекта и её помехозащищенностью.

Чтобы лучше понять механизм взвешивания доверия, посмотрим на график зависимости веса от квадрата расстояния Махаланобиса (рис. 2).

Рисунок 2. Мягкое стробирование измерений.

Как его читать:

Вес рассчитывается по формуле: $w = \frac{\nu + m}{\nu + d^2}$

где — размерность вектора измерений (для графика ).

При $\nu = 300$ (синяя линия) график почти плоский. Фильтр ведет себя как классический линейный, сохраняя высокий вес даже при больших невязках.
При $\nu = 2$ (оранжевая линия) вес начинает быстро уменьшаться с ростом ошибки, асимптотически приближаясь к нулю.

При идеальном совпадении измерения с прогнозом, $d^2 \to 0$ теоретический вес превышает единицу. Формально это корректно, но неверно с инженерной точки зрения.
Вес означал бы, что мы доверяем датчику сильнее, чем заявлено в его паспортных характеристиках (матрице ).
Поэтому в реализациях применяют ограничения сверху и снизу.
💡 В коде это два предохранителя:

# 1. Защита от несоответствия модели
if self.cap_lambda_to_1:
    lamvec = np.minimum(lamvec, 1.0)
# 2. Защита от деления на ноль
lamvec = np.clip(lamvec, self.lambda_clip[0], self.lambda_clip[1])

Первый обеспечивает сохранение физики.
Второй спасает алгоритм от деления на ноль, если выброс окажется очень большим.

1.2. Вычислительная сложность или сколько это будет стоить

Классический UKF работает детерминировано. Для аппроксимации нелинейных преобразований он использует фиксированный набор из сигма-точек, где — размерность вектора состояния. Вычислительная сложность одного шага ограничена кубической зависимостью $\mathcal{O}(n^3)$ , которая возникает из-за матричных операций (разложения Холецкого и обращения матриц ковариации). Благодаря отсутствию стохастических элементов, время выполнения предсказуемо, алгоритм ложится на микроконтроллеры.
Вариационно-Байесовский UKF (VB-UKF) сохраняет базовую структуру UKF (те же сигма-точек), но оборачивает этап коррекции в итеративный цикл вариационно-байесовского вывода. Внутри одного такта времени фильтр делает $N_{iter}$ итераций (обычно от 3 до 5), на каждой из которых уточняет параметры распределения шума и вектор состояния. Асимптотическая сложность возрастает до $\mathcal{O}(N_{iter} \cdot n^3)$ , что на практике означает незначительное и контролируемое замедление по сравнению с базовым UKF.
Для Particle Filter (PF) вычислительная сложность одного шага составляет $\mathcal{O}(N_p \cdot n^2)$ (для классического варианта) или даже $\mathcal{O}(N_p \cdot n^3)$ (для Unscented-версий), где — количество частиц.
Связь необходимого числа частиц с размерностью пространства состояний обычно описывается эмпирическим правилом: $N_p \sim 10^n \dots 100^n.$
Для задачи с это дает требуемый диапазон $N_p \sim 10^5 \dots 10^6$ (от ста тысяч до миллиона частиц), что делает алгоритм непригодным для встроенных систем реального времени.

Практический вывод:

С точки зрения производительности VB-UKF — это тот же самый UKF, но замедленный в $N_{iter}$ раз (обычно от трех до пяти). Вычислительная сложность алгоритма остается ограниченной.
За небольшую контролируемую плату получают робастность, приближающуюся к тяжеловесному Particle Filter.

Важно понимать принципиальную разницу в механизмах этой робастности:

Particle Filter пытается «вытянуть» аномалию непрерывно исследуя неизвестное распределение.
VB-UKF умеет вовремя распознать «плохие» измерения, снизить к ним доверие и временно переключиться на «хорошие» сенсоры или прогноз модели, пока возмущение не исчезнет.

1.3. Аномалии измерений в задачах потоковой фильтрации

Поскольку в названии статьи фигурирует термин «аномалия», необходимо определить, с чем именно мы работаем. Будем опираться на классическое определение:

«Выброс — это наблюдение, которое настолько сильно отклоняется от других наблюдений, что вызывает подозрения в том, что оно было порождено другим механизмом».
— Hawkins, D. M. (1980). Identification of Outliers.

(В рамках этой статьи термины «аномалия» и «выброс» используются как синонимы, обозначающие сильное, статистически маловероятное отклонение).

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

В контексте временных рядов аномалии принято делить на два класса:

Точечные - одиночные выбросы. Они могут быть глобальными (просто физически неадекватное значение) или контекстуальными (значение в пределах нормы, но нелогичное в контексте конкретного момента времени или шага).
Длящиеся / Коллективные - серия ошибочных данных (например, датчик «залип» или начал стабильно дрейфовать).

Как с этим работают фильтры Калмана:

Контекстуальность через динамику. Классический UKF и базовый VB-UKF умеют справляться с контекстуальными точечными аномалиями. Фильтр оценивает адекватность измерения в контексте прогноза, который диктуется матрицей динамики системы (физической моделью).
Мягкое взвешивание. Как мы разобрали выше, VB-UKF обрабатывает эти точечные аномалии не через пороговые детекторы, а через непрерывное мягкое взвешивание, плавно снижая доверие к датчику по модели на основе распределения Стьюдента.
Уязвимость к длящимся аномалиям (Bias/Drift). Хотя фильтр Калмана рекурсивно накапливает всю историческую информацию в своей матрице ковариации , его механизм оценки невязки является марковским — он принимает решение о «качестве» измерения строго в моменте, сравнивая текущий отсчет с текущим прогнозом.
По этой причине базовый UKF и VB-UKF уязвимы к систематическим смещениям. Если датчик будет методично «врать» 10 тактов подряд, фильтр постепенно «запишет» эту ошибку в свою ковариацию и в итоге поведет оценку по ложному следу.
Для защиты от таких сценариев требуются более сложные архитектуры, применяющие графовую оптимизационную модель вместо рекурсивной калмановской или анализ контекста. В качестве примера приведем алгоритм SWVKF-ST из статьи Liu, X., et al. (2025). "A Novel Variational Bayesian Method Based on Student’s t Noise for Underwater Localization." Sensors, 25(3), использующий скользящее контекстное окно буферизации с факторами затухания для обработки длящихся сбоев.

Продвинутые подходы: информационная избыточность через дополняющие измерения, экзогенные факторы и понимание причинности

Поскольку алгоритм сам по себе не видит истинного порождающего механизма аномалий, логичным путем развития навигационных систем является комплексирование разнородных измерений, о котором мы уже говорили в контексте VB-UKF.
Полезна и интеграция экзогенных (внешних) факторов, основанных на понимании причинности. Если известна физическая причинность возможных аномалий, появляется возможность в динамике управлять гиперпараметрами фильтра (агрессивностью $\nu$ ) еще до того, как они появятся:
- Городской каньон. Навигатор беспилотного автомобиля может использовать размеченную карту города. Приближаясь к зоне плотной высотной застройки, где сигнал GPS предсказуемо начнет переотражаться, система превентивно «агрессирует» параметры фильтра GPS, плавно передавая приоритет инерциальным датчикам.
- Солнечная активность. При фиксации вспышек на Солнце через предсказуемое время вероятность сбоев в работе спутниковой аппаратуры возрастает. Получив такой телеметрический флаг, алгоритм может превентивно «ужесточить» параметры фильтрации, переведя систему в режим большего недоверия к скачкам.

1.4. Вариационно-байесовская аппроксимация как способ не уходить далеко от линейно-гауссовской модели (немного формул, по желанию)

Классический фильтр Калмана и его нелинейное расширение UKF хороши своей вычислительной простотой. Это обусловлено марковской моделью и аддитивными гауссовскими шумами, что позволяет получать аналитические формулы для обновления среднего и ковариации.
Если мы заменим шум измерений на распределение Стьюдента:

$p(y|x) = \text{Student}(y; h(x), R, \nu)$ ,

то апостериорное распределение перестает быть гауссовским (и даже Стьюдентом). Оно превращается в сложную смесь, для которой нет аналитического решения.

Вариационно-байесовский подход (VB) позволяет заменить поиск истинного апостериорного распределения на поиск лучшего приближения.

1. Иерархическое представление шума (Gaussian Scale Mixture)

Распределение Стьюдента можно представить как бесконечную сумму Гауссиан с флуктуирующей дисперсией. Математически это выражается через введение дополнительной скрытой переменной $\lambda$ :

$\text{Student}(y; h(x), R, \nu) = \int_{0}^{\infty} \mathcal{N}\left(y; h(x), \frac{R}{\lambda}\right) \text{Gamma}\left(\lambda; \frac{\nu}{2}, \frac{\nu}{2}\right) d\lambda$ .

В терминах фильтра это значит, что эффективная матрица ковариации шума (например, $\tilde{R}_k = R / \lambda_k$ ) перестает быть константной. Скрытая переменная $\lambda_k$ , выступающая в роли веса датчика, становится случайной величиной с собственным априорным Гамма-распределением (или обратным распределением Уишарта, если оценивается матрица целиком).

2. Инверсная дивергенция Кульбака-Лейблера (Reverse DKL)

Цель — найти совместное апостериорное распределение состояния и параметров шума $p(x_k, R_k | y_{1:k})$ .

Поскольку истинное распределение не вычислимо, мы ищем его аппроксимацию , минимизируя дивергенцию Кульбака-Лейблера. Принципиально важно, что в VB используется обратная (инверсная) дивергенция:

$KL(q || p) = \int q(x_k, R_k) \log \frac{q(x_k, R_k)}{p(x_k, R_k | y_{1:k})} d x_k d R_k \rightarrow \min$

Почему именно инверсная (), а не прямая ()?

Вычислимость. Прямая дивергенция требует интегрирования по истинному, неизвестному апостериору . Инверсная KL усредняет по распределению , форму которого мы выбираем сами.
Поиск моды. Если истинная вероятность в какой-то зоне близка к нулю (например, это зона аномального выброса), то инверсная KL накладывает большой штраф, если в этой зоне отлично от нуля. Это заставляет аппроксимацию плотно прижиматься к наиболее вероятным, физически адекватным состояниям системы, игнорируя аномалии или младшие моды для многомодального случая.

3. Приближение среднего поля и факторизация в сумму

Чтобы минимизировать инверсную KL-дивергенцию, VB применяет приближение среднего поля (Mean Field Approximation), разрывая статистическую зависимость между переменными:

$q(x_k, R_k) \approx q(x_k) q(R_k)$

Благодаря этому логарифм аппроксимирующего совместного распределения распадается на сумму независимых логарифмов:

$\log q(x_k, R_k) = \log q(x_k) + \log q(R_k)$

Минимизация KL-дивергенции при таком разделении дает строгие аналитические решения для каждого компонента по отдельности. Оптимальный логарифм распределения одного параметра равен математическому ожиданию логарифма совместного распределения по другому параметру:

Шаг оценки состояния (UKF):

$\log q^*(x_k) = \mathbb{E}_{q(R_k)} [\log p(y_k, x_k, R_k)] + \text{const}$

Здесь мы фиксируем дисперсию шума (берем математическое ожидание $\mathbb{E}[R_k^{-1}]$ из предыдущего шага) и делаем стандартный гауссовский проход UKF. Получаем новую оценку состояния $\hat{x}_k$ .

Шаг обновления шума (VB):

$\log q^*(R_k) = \mathbb{E}_{q(x_k)} [\log p(y_k, x_k, R_k)] + \text{const}$

Теперь мы фиксируем найденный вектор состояния и смотрим на невязку. Если прогнозируемое состояние сильно расходится с измерением , математика послушно «раздувает» матрицу .

Эти два шага зацикливаются и повторяются обычно 3–5 раз за такт.

Итог:
Вариационный Байес — это компромисс.
Физически VB-UKF ведет себя как сложный робастный фильтр, реализующий непрерывное «мягкое взвешивание» датчиков.
Но «под капотом» он вычисляет это через серию вычислительно дешевых аналитически точных шагов. Мы ушли от линейно-гауссовской модели ровно настолько, чтобы получить устойчивость к аномалиям, но не так далеко, чтобы потерять способность работать в реальном времени.

2. Особенности реализации и конфигурация имитационной модели

Для демонстрации жизнеспособности описанной вариационно-байесовской структуры, разработана имитационная модель фильтрации с комплексированием измерений для навигации в условиях "городского каньона".

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

Standart Fusion UKF: параметризован константными гауссовскими матрицами шумов измерений.
- Слияние сенсоров выполняется как единый шаг обновления по совмещённому вектору измерений
- После прогноза ( $\hat x^-_k,P^-_k$ ) фильтр строит прогноз измерения $\hat y_k$ через $h(\cdot)$ , формирует инновацию $v_k=y_k-\hat y_k$ и обновляет состояние стандартной UKF-формулой с калмановским усилением .
- Таким образом, вклад каждого измерительного канала определяется только заданной матрицей без адаптации к аномалиям.
Fusion VB-UKF: робастная вариационно-байесовская реализация со Стьюдентом и «мягким взвешиванием» измерений.

В коде VB-UKF реализовано несколько дополнительных полезных решений:

Разделение каналов (Diagonal Whitening).
Если реализовывать шаг обновления VB-весов Стьюдента через матрицу Холецкого (оценивая полную ковариационную матрицу $R_{eff}$ ), возникает проблема кросс-канального заражения. Аномалия по каналу позиционирования или в GPS через внедиагональные элементы матрицы «отравит» веса чистых данных IMU. В нашей реализации применен механизм независимого поканального взвешивания (whitening="diagonal"). Фильтр рассчитывает квадрат невязки строго для каждого сенсора, изолируя сбои GPS от данных IMU.

Итерированный проход.
Классический подход обновляет апостериорную ковариацию $\Sigma$ один раз за такт. В нашей реализации n_iter=5. Внутри этого цикла происходит перерасчет сигма-точек вокруг обновленного состояния. То есть с каждой итерацией, по мере того как фильтр снижает доверие к аномальному GPS, линеаризация модели подтягивается ближе к истинному вектору состояния, обеспечивая большую точность на нелинейной модели CTRV.

Раздельное управление доверием/агрессивностью фильтра.
Реализована раздельная установка параметра $\nu$ по блокам измерительных каналов: для GPS и IMU.

2.1. Динамика CTRV и параметры конфигурации

В качестве модели движения выбрана стандартизованная нелинейная кинематическая модель Constant Turn Rate and Velocity (рис. 3).

Рисунок 3 . Нелинейная кинематическая модель CTRV — Рисунок 3 . Нелинейная кинематическая модель **CTRV**

Вектор состояния из пяти компонент:

$x_k = [p_x, p_y, v, \psi, \dot{\psi}]^T$ ,

где — координаты, — линейная скорость, $\psi$ — угол рыскания (курс), а $\dot{\psi}$ — угловая скорость.

Измерения поступают от двух независимых блоков:

GPS: измеряет координаты . Вне каньона датчик имеет стандартное отклонение $R_{gps} = 1.0$ м.
IMU: гироскоп измеряет угловую скорость $\dot{\psi}$ с СКО $R_{gyro} = 0.01$ рад/с, а датчик скорости — линейную скорость с СКО $R_{speed}$ = 0.2 м/с.

Стресс-тест среда для фильтров задается эффектом городского каньона. Эффект моделируется через кратный рост стандартного отклонения шумов (в 7 раз) и введение систематической ошибки (+15 и -15 метров) по обоим каналам GPS.

Параметр доверия $\nu$ :

Для GPS установлено агрессивное значение параметра nu_gps = 2.0. Это означает, что фильтр априори не доверяет приемнику и готов девальвировать его данные при подозрении на выброс.
Для IMU установлено консервативное значение параметра nu_imu = 25.0. Алгоритм считает, что модуль не столкнется с аномалиям (условия каньона не имеют специфичного влияния на датчики), поэтому фильтр будет держать вес его измерений стабильным, опираясь на IMU в моменты, когда GPS будет девальвирован.

2.2. Структура данных

Структура данных описана в комментариях к коду модуля симуляции.

cfg = m.FusionConfig(
    seed=55,
    T=150,        # Длина траектории
    dt=0.1,

    # Процессные шумы
    q_pos=0.05,   # Шум интегрирования координат
                  # (учитывает погрешности дискретизации при расчете позиции)
    q_vel=0.5,    # Шум линейного ускорения (м/с²)
                  # (неучтенные изменения скорости модели CTRV)
    q_psi=0.001,  # Шум угла курса / рыскания
                  # (случайный дрейф угла)
    q_dpsi=0.1,   # Шум углового ускорения
                  # (Неучтенные изменения скорости поворота модели CTRV)

    # Измерительные шумы
    r_gps_std=1.0,       # СКО GPS в условиях прямой видимости (LOS condition) (м)
    r_gyro_std=0.01,     # СКО гироскопа (рад/с)
    r_speed_std=0.2,     # СКО спидометра / одометра (м/с)

    # Параметры городского каньона (Urban Canyon)
    canyon_y_min=20.0,            # Задаем границы каньона по диапазону y
    canyon_y_max=50.0,
    multipath_bias=(15.0, -15.0), # Смещение из-за переотражений по X и Y в "городском каньоне"
                                  # (cистематическая ошибка переотражений луча)
    multipath_std=7.0             # СКО шума с "тяжелыми хвостами"
                                  # (увеличенное cтандартное отклонение аномальных измерений)
)
# 2. Запуск симуляции
sim_data, trajs = m.run_experiment(cfg,
                                   n_iter=5,     # количество вариационных итераций для оценки
                                                 # апостериорного распределения (3-5)
                                   # Настройки уровня "агрессивности" к невязке измерения
                                   nu_gps=2.0,   # Высокий (предполагаем аномалии в измерениях)
                                   nu_imu=25.0,  # Низкий  (предполагаем обычные шумы)
                                   )

# 3. Распаковка данных для анализа
y_meas = sim_data.y_meas              # Shape (T, 4): [gps_x, gps_y, gyro, speed]
x_true = sim_data.x_true              # Shape (T, 5): [px, py, v, psi, dpsi]
out_mask = sim_data.outlier_flags     # (T, 2), где [:,0]==1 это GPS outliers
traj_naive = trajs.traj_naive         # (T, 5)
traj_robust = trajs.traj_robust       # (T, 5) временной ряд компонент состояния
weights_robust = trajs.weights_robust # (T, 4) временной ряд весов измерений: [gps_x, gps_y, gyro, speed]


innov_naive = trajs.innov_naive        # (T, 4) базовая инновация стандартного фильтра
innov_robust = trajs.innov_robust      # (T, 4) инновация VB-фильтра → для визуализации
innov0_robust = trajs.innov0_robust    # (T, 4) номинальная инновация → для расчета NIS

S_naive = trajs.S_naive                # (T, 4, 4) 3D стек ковариаций инноваций-доверие измерению
S_robust = trajs.S_robust              # (T, 4, 4) с инфляцией → для визуализации реакции на аномалию
S0_robust     = trajs.S0_robust        # (T, 4, 4) номинальная без инфляции → для расчета NIS

P_naive = trajs.P_naive                # (T, 5, 5) 3D стек апостериорных ковариаций состояния
P_robust = trajs.P_robust              # Показывает уверенность фильтра в текущих положении и скорости объекта

Для детального анализа поведения алгоритмов скрипт логирует и распаковывает:

Глобальные метрики: истинный вектор состояния объекта (x_true) и сырые измерения сенсоров (y_meas).
Результаты фильтрации: отфильтрованные траектории (traj_naive, traj_robust) и апостериорные ковариации (P_naive, P_robust), отражающие уверенность алгоритма в собственных оценках.
Внутренняя диагностика VB-UKF: временные ряды весов $\lambda$ (weights_robust), невязки (innov_robust), а также матрицы $S_{robust}$ , отражающие процесс динамической инфляции ковариации при обнаружении аномалии в каньоне.

Этот объем данных позволит поканально разобрать реакцию фильтров на аномалии в следующем подразделе.

3. Анализ результатов моделирования

Результаты моделирования демонстрируют принципиальные различия в поведении классического и вариационного фильтров при возникновении аномальных измерений типа «городской каньон».

3.1. Траектории

На графике навигации (рис.4) отчетливо видно разное поведение алгоритмов в зоне каньона, где систематические ошибки измерений GPS (зеленые точки) смещены на 15 метров вправо и 15 метров вниз, а стандартное отклонение увеличилось в 7 раз.

Рисунок 4. Траектории фильтров в "городском каньоне".

Standart Fusion UKF (синяя линия): демонстрирует неустойчивость. Поскольку матрица ковариации измерительного шума фиксирована ( $R_{gps} = 1.0^2$ ), фильтр воспринимает смещенные измерения как достоверные. Оценка состояния «утягивается» за аномалией, формируя значительную ошибку позиционирования. Заметно незначительное позитивное влияние IMU (траектория смещена в правильном направлении относительно аномальных измерений).
Robust Fusion VB-UKF (красная линия): существенно более стабилен. Алгоритм детектирует несогласованность измерений GPS с прогнозируемой динамикой объекта. Снизив доверие к спутниковому сигналу, фильтр переходит в режим счисления координат, опираясь преимущественно на данные инерциального модуля (гироскоп и датчик скорости). Траектория остается гладкой и ближе к истинной.

3.2. Механизм адаптации весов

Детальную картину работы вариационного механизма раскрывает график адаптивных весов (рис. 5).

Веса $w_i \in (0, 1]$ выступают множителем, масштабирующим ковариацию шума $R_{eff} = R / w_i$ .

Изоляция каналов
Благодаря диагональной схеме независимого взвешивания, падение весов GPS $W_{GPS}$ до значений в зоне каньона (шаги 45–100) не влияет на доверие к инерциальным датчикам. Веса гироскопа $W_{gyro}$ и датчика скорости $W_{speed}$ остаются в диапазоне , подтверждая их надежность в рамках модели.

Априорные настройки ( $\nu$ )
Резкое и глубокое падение $W_{GPS}$ обусловлено агрессивным параметром $\nu_{gps}=2.0$ (распределение с «тяжелыми хвостами»), заставляющим фильтр жестко девальвировать малейшие отклонения. Напротив, консервативное значение $\nu_{imu}=25.0$ удерживает веса IMU от ложных срабатываний.

Малая вариативность
Небольшие просадки веса скорости $W_{speed}$ до корректно отражают несоответствие идеальной модели постоянной скорости (CTRV) реальной динамике объекта с ускорениями.

3.3. Переходный процесс

На выходе из каньона (шаги 100–117) наблюдается эффект инерционности восстановления. Хотя GPS начинает выдавать корректные данные, веса $W_{GPS}$ не возвращаются мгновенно к , а испытывают колебания.

Это следствие накопленного за время «слепого полета» дрейфа инерциальной системы. Когда «чистые» измерения GPS начинают поступать, они противоречат смещенному прогнозу фильтра. Алгоритм, настроенный на подозрительность ( $\nu=2.0$ ), поначалу воспринимает истинные данные как новые аномалии. Постепенная (итеративная) коррекция оценки состояния приводит к плавному, а не скачкообразному (как у Standard UKF), схождению красной траектории к истинной, что критически важно для безопасности управления динамическими объектами.

Затем, примерно со 130-го шага (рис. 4) начинается левый поворот и агрессивный параметр $\nu_{gps}=2.0$ проявляется в динамике весов (рис. 5). Вернувшись к траектории (рис. 4) можно заметить, что робастный фильтр на этом участке проигрывает стандартному в точности. Нужен тюнинг.

3.4. Количественный анализ панелей метрик: компромисс робастности и адаптивности

Для объективного сравнения алгоритмов мы рассчитали позиционную ошибку RMSE и метрики согласованности NIS на двух наборах параметров. Первая панель (рис. 6) отражает базовый сценарий: $R_{gps} = 1.0$ , вторая (рис. 7) — сценарий с увеличенным доверием к априорному шуму GPS: $R_{gps} = 2.5$ .

Рисунок 6. Панель метрик для . — Рисунок 6. Панель метрик для $R_{gps} = 1.0$ .

Рисунок 6. Панель метрик для $R_{gps} = 1.0$ .

Рисунок 7. Панель метрик для . — Рисунок 7. Панель метрик для $R_{gps} = 2.5$ .

Рисунок 7. Панель метрик для $R_{gps} = 2.5$ .

Про специализированную метрику рабочего режима NIS можно почитать здесь.

Базовый сценарий

$R_{gps} = 1.0$

В режиме Normal оба фильтра показывают сопоставимую позиционную ошибку: м у стандартного против м у робастного. Однако при попадании в зону Canyon разрыв становится очевидным:

Standard UKF: кратное ухудшение, RMSE м.
Robust VB-UKF: успешно парирует аномалии, RMSE на уровне м (в Normal было ).

Важно обратить внимание на метрику согласованности NIS Cov_95%. В классической теории она отражает долю шагов, на которых невязка укладывается в 95%-й доверительный интервал $\chi^2$ - распределения (в идеале должна стремиться к 95%). В каньоне этот показатель у обоих алгоритмов падает ниже 7%.

Для базового UKF это означает расходимость фильтра — его оценка неопределенности не соответствует реальным ошибкам.

Однако для VB-UKF огромные значения среднего NIS () и низкое покрытие — это не ошибка, а маркер срабатывания защиты. Именно несогласованность прогноза с номинальной матрицей служит для вариационного алгоритма триггером для снижения весов и включения режима недоверия.

Влияние тюнинга

$R_{gps} = 2.5$

Вторая панель демонстрирует результаты, соответствующие одному измененному параметру конфигурации — мы увеличили номинальное СКО GPS $R_{gps}$ до м.

Сравнение режимов выявляет парадоксальную реакцию алгоритмов:

Standard UKF улучшил показатели: в зоне каньона его ошибка немного снизилась (с до м). Заложив большую дисперсию измерительного шума, мы сделали базовый фильтр менее доверчивым к GPS, что слегка смягчило реакцию на выбросы.
Robust VB-UKF ухудшил показатели: его позиционная ошибка в каньоне ухудшилась с до м.

Почему увеличение навредило робастному фильтру?

Расширив априорные границы допустимого шума, мы сделали фильтр более толерантным. Механизм защиты срабатывает позже и слабее. В то же время и в нормальном режиме (Normal) ошибка VB-UKF также выросла (с до м), так как алгоритм стал хуже сглаживать обычные флуктуации.

Глобальная проблема настройки

Этот простой пример подводит нас к фундаментальной проблеме вариационной фильтрации, которая активно обсуждается в профильных публикациях, например, в упомянутой ранее работе: настройка VB-UKF — это компромисс между робастностью и адаптивностью.

Мы рассмотрели тюнинг всего по одному параметру $R_{gps}$ , и увидели, что алгоритмы реагируют на него разнонаправленно. На практике инженеру необходимо настраивать целый ансамбль параметров: матрицы и , уровни агрессивности $\nu$ для каждого датчика, количество итераций.

Сделать фильтр максимально "жестким" (маленькие, низкие $\nu$ ) означает защититься от аномалий в каньоне, но пожертвовать способностью алгоритма адаптироваться к легитимным изменениям динамики в нормальном режиме. И наоборот.

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

Подведем итоги

Variational Bayes Fusion UKF в условиях эксперимента продемонстрировал значимое превосходство по отношению к классическому Fusion UKF, оставаясь сопоставимым по вычислительной сложности.
Практическая реализация робастного фильтра требует внимания к архитектурным деталям. Например, отказ от оценки полной матрицы ковариации инновации в пользу диагонального взвешивания предотвращает кросс-канальное «заражение» и позволяет инерциальным датчикам уверенно вести объект, пока GPS находится в зоне «слепоты».
Несмотря на высокую эффективность, VB-UKF не является алгоритмом из серии «настроил и забыл». Его конфигурация - это компромисс между способностью работать в условиях аномальных измерений (робастность) и умением распознавать легитимное маневрирование (адаптивность). Многопараметрический тюнинг конфигурации, подходящий для существенно разных режимов - задача не тривиальная. Возможное направление решения: информационный выход за ограничения марковской модели (например, буферизация и учет экзогенных факторов).

Робастный Variational Bayes Fusion UKF в городском каньоне: RMSE и NIS при аномальных сигналах GPS