Множественные оценки положения слияния

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

Первая система менее точна, но вторая система дрейфует. Первая система выдает выходные данные примерно раз в секунду, а вторая - намного чаще (100-200 раз в секунду).

Я предполагаю, что должен быть лучший подход, чем просто сбросить позицию с оценкой первой системы (поскольку она не является точной на 100%), но также использовать накопленную позицию от второй системы датчиков и объединить ее с новыми данными из Первая система. Также возникает вопрос, как объединить 3 оценки первой системы? Должен существовать лучший способ, чем чисто среднее, поскольку может получиться, что две оценки абсолютно одинаковы, а третья - совершенно другая (имеется в виду, что она, вероятно, более ошибочна)?

Есть ли у вас какие-либо алгоритмы слияния, которые можно рекомендовать использовать в такой системе? Я знаю о фильтре Калмана, но мне трудно понять, как его использовать, поскольку две системы выводят данные на разных частотах.

Я надеюсь, что вопрос достаточно ясен, каков наилучший подход, чтобы объединить оценки в более правильную и точную оценку?

Спасибо

То, что вы описываете, по сути является учебным пособием для использования фильтра Калмана. Сначала вам нужен шаг прогнозирования. Давайте предположим, что вы предсказываете позу робота$(x,y,\theta)$, учитывая предыдущую оценку позы и ваши высокочастотные измерения скорости $(v,\omega)$, где $v$ линейная скорость и $\omega$ угловая скорость.

$P$ ковариационная матрица 3x3, которая представляет неопределенность позы робота. $Q$ ковариация ваших входных данных (т. е. насколько шумны эти измерения скорости?) $F$ Якобиан модели движения по отношению к состоянию и $G$ является якобианом по отношению к входам, т.е.

Теперь у вас есть менее частые обновления коррекции, которые фактически измеряют полное состояние, делая это довольно простым, т.е.

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

Чтобы ответить на ваш вопрос о различных ставках, вы можете просто запускать обновление движения до тех пор, пока не прибудет обновление прогноза. Например, может случиться так, что обновление движения происходит 100 раз перед выполнением коррекции.

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