Візуалізація та налагодження EKF

Наразі я налагоджую та налаштовую EKF (Extended Kalman Filter). Завдання - класичне відстеження позиції мобільного робота, де орієнтирами є маркери AR.

Іноді я дивуюсь, як якесь вимірювання впливає на оцінку. Коли я переглядаю і обчислюю кількість і матриці, що стосуються, я можу розібратися, як виконано крок оновлення, що і чому саме відбулося, але це дуже втомливо.

Тож мені цікаво, чи хтось використовує якусь техніку, фокус чи розумну візуалізацію, щоб краще зрозуміти, що відбувається на кроці оновлення EKF?

ОНОВЛЕННЯ №1 (буде більш конкретним і покаже перше наближення до того, що я маю на увазі)

Що я шукаю - це певний спосіб візуалізувати один крок оновлення таким чином, що дає мені відчути, як кожен компонент вимірювання впливає на кожен компонент стану.

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

В даний час я працюю з EKF, де стан є 2D пози (x, y, кут), а вимірювання також 2D пози.

У доданому зображенні (відкрийте його на новій сторінці / вкладці, щоб побачити повну роздільну здатність) (масштабований) вектор K (1,1: 2) (синтаксис MATLAB для отримання підматриці з матриці 3x3) повинен дати уявлення, як перший компонент стану EKF буде змінюватися з поточним інноваційним вектором, K (2,1: 2), як зміниться другий компонент EKF і т. д. У цьому прикладі вектор інновації має відносно великий х компонент і він вирівнюється з вектор K (2,1: 2) - другий компонент стану (y координата) зміниться найбільше.

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

Першим вдосконаленням було б візуалізувати, як третій компонент інновації впливає на державу. Тоді було б непогано додати дані коваріації, щоб відчути, як створюється матриця K.

ОНОВЛЕННЯ № 2 Тепер на графіку є вектори в просторі стану, які показують, як кожен компонент вимірювання змінює положення. З цього сюжету я бачу, що третя складова вимірювання найбільше змінює стан.

Дуже інформативний спосіб візуалізації ефекту вимірювань (для мене) - побудувати графік стану робота (середній, з коваріаційним еліпсом) до та після кожного вимірювання. Потім візьміть окремі компоненти вимірювання (підшипник, діапазон для маркерів AR) і застосуйте їх окремо, щоб відчути це.

Зробити це:

Я використовую одну або кілька з цих функцій для побудови еліпса. Щоб знайти константи , зауважте, що вони є квадратними коренями власних значень матриці коваріації. Потім відібрати вибір кута на та знайти діапазон з гіпотези. використовуючи зв’язане рівняння. Я особливо рекомендую використовувати це рівняння :θ [ 0 , 2 π $a,b$$\theta$$[0, 2\pi]$

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

Удачі, і не оновлюйте своє запитання занадто часто. Натомість повертайтеся з новими питаннями.

Що часто робиться, це побудувати графіки змінних стану за часом, а також їх 3-сигмальні інтервали. Точки, де цей інтервал скорочується, є оновленнями, де ви могли б анотувати джерело проведених вимірювань.

Крім помилок у впровадженні, які слід перевірити (не тільки неправильні рівняння, але й чисельно нестабільні рівняння), на вплив оновлень безпосередньо впливає різниця між тим, що "очікується" та "вимірюється" та їх відповідними невизначеностями. Тож вам може бути цікаво з'ясувати спосіб візуалізації цього балансу з точки зору прогресування часу в першому сюжеті.