Визуализация и отладка EKF

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

Иногда я удивляюсь, как какое-то измерение влияет на оценку. Когда я смотрю и вычисляю числа и матрицы, я могу понять, как был выполнен шаг обновления, что и почему именно произошло, но это очень утомительно.

Поэтому мне интересно, если кто-то использует какую-то технику, хитрость или умную визуализацию, чтобы лучше понять, что происходит на этапе обновления EKF?

ОБНОВЛЕНИЕ № 1 (будет более конкретным и покажет первое приближение того, что я имею в виду)

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

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

В настоящее время я работаю с EKF, где состояние является 2D-позой (x, y, angle), а измерения также являются 2D-позами.

На прилагаемом изображении (откройте его на новой странице / вкладке, чтобы увидеть в полном разрешении), (масштабированный) вектор K (1,1: 2) (синтаксис MATLAB, чтобы взять подматрицу из матрицы 3x3) должен дать представление о том, как первый компонент состояния EKF будет изменяться в зависимости от текущего вектора инноваций, K (2,1: 2), как изменится второй компонент EKF, и т. д. В этом примере вектор инноваций имеет относительно большой компонент x, и он выравнивается по вектор K (2,1: 2) - вторая компонента состояния (координата y) изменится наиболее.

Одна из проблем этого графика заключается в том, что он не дает ощущения того, как третий компонент (угол) вектора инноваций влияет на состояние. Первый компонент состояния немного увеличивается, в отличие от того, что указывает K (1: 1: 2) - это вызывает третий компонент инновации, но в настоящее время я не могу это визуализировать.

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

ОБНОВЛЕНИЕ № 2 Теперь у графика есть векторы в пространстве состояний, которые показывают, как каждый компонент измерения меняет свое положение. Из этого графика я вижу, что третий компонент измерения наиболее изменяет состояние.

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

Сделать это:

Я использую одну или несколько из этих функций для построения эллипса. Чтобы найти константы , отметим, что они являются квадратными корнями из собственных значений ковариационной матрицы. Затем сэмплируйте угол над и найдите диапазон из гипотезы. используя связанное уравнение. Я особенно рекомендую использовать это уравнение :θ [ 0 , 2 π $a,b$$\theta$$[0, 2\pi]$

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

Удачи, и не обновляйте свой вопрос слишком часто. Вместо этого возвращайтесь с новыми вопросами.

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

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