Внедрение Data Fusion с помощью (расширенного) фильтра Калмана в OpenCV / Stack Overflow

Я работаю над проектом по отслеживанию положения камеры, которая установлена ​​на движущемся устройстве посредством объединения данных.
Данные, которые я получаю

1) скорость в x-, y- и z-направлении камеры от источника A

2) разница между позициями в текущем и последнем кадре (в 2D, Z не должна меняться в любом случае) от источника B

Я уже делал похожий проект, но без слияния данных или около того и использовал Kalman-Filter, который реализован в OpenCV.

В настоящее время меня все больше смущают различные методы реализации, которые я нашел в сети до сих пор.

Как бы я мог вставить / объединить полученные данные в различные компоненты KF / EKF?

У меня есть этот пример для изменения OpenCV-KF для работы в качестве EKF. Это очень похоже на то, что мне нужно, за исключением того, что мой объект на самом деле является самой камерой, а также возможностью перемещаться по оси Y и X, по очереди… Кроме того, вместо пиксельных координат объекта я получил вышеупомянутые данные.
https://sites.google.com/site/timecontroll/home/extended-kalman-filtering-with-opencv

К сожалению, я не совсем уверен, как определить мою функцию перехода и измерения и соответствующую матрицу.

Пусть x (k) будет текущим состоянием, deltaT — время, прошедшее с момента последнего обновления.

Для 1 это, вероятно, будет что-то вроде:

x (k) = x (k-1) + deltaT, умноженное на скорость (k-1)

Для 2:

x (k) = x (k-1) + разность между кадрами

Итак, давайте предположим, что оба источника данных имеют одинаковый вес, что заставляет меня думать, что в совокупности это будет что-то вроде:

x (k) = x (k-1) + (deltaT, умноженная на скорость (k-1) + разность между кадрами) / 2

Для измерений: я получаю скорость в направлении X, Y и Z через источник A и разницу между текущим и последним кадром в координатах X и Y через источник B.

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

4

Решение

Вы на правильном пути, но вы, кажется, запутались в понятиях функции перехода и функции измерения.

Первое, что нужно сделать, это четко определить, какой вектор состояния вы хотите рассмотреть. В вашем случае достаточно положения [px, py, pz] и скорости [vx, vy, vz]:

ИксК = [pxК, руК, пзК, ъхК, ууК, угК]

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

пК = [pxК, руК, пзКзнак равно
пK-1 + д.т. vK-1

vК знак равно vK-1

Это может быть представлено в виде матрицы следующим образом:

       [ 1  0  0  dT 0  0  ]
[ 0  1  0  0  dT 0  ]
[ 0  0  1  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  ] . x(k-1)
[ 0  0  0  0  1  0  ]
[ 0  0  0  0  0  1  ]

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

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

час( ИксК знак равно vК

Или в матричной форме:

           [ 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 1 0 ] . x(k)
[ 0 0 0 0 0 1 ]

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

часВ( ИксК знак равно пКпK-1 знак равно пK-1 + д.т. vK-1пK-1 = дт. vK-1 = дт. vК

Или в матричной форме:

             [ 0  0  0  dT 0  0  ]
hB{ x(k) } = [ 0  0  0  0  dT 0  ] . x(k)
[ 0  0  0  0  0  dT ]

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

ИксК = [pxК, руК, пзК, ПВK-1, руK-1, пзK-1,ъхК, ууК, угК]

Функция перехода станет тогда:

       [ 1  0  0  0  0  0  dT 0  0  ]
[ 0  1  0  0  0  0  0  dT 0  ]
[ 0  0  1  0  0  0  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  0  0  0  ] . x(k-1)
[ 0  0  0  0  1  0  0  0  0  ]
[ 0  0  0  0  0  1  0  0  0  ]
[ 0  0  0  0  0  0  1  0  0  ]
[ 0  0  0  0  0  0  0  1  0  ]
[ 0  0  0  0  0  0  0  0  1  ]

Функция измерения для источника A станет:

           [ 0 0 0 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 0 0 0 1 0 ] . x(k)
[ 0 0 0 0 0 0 0 0 1 ]

Функция измерения для источника B станет:

           [ 1  0  0 -1  0  0  0  0  0 ]
hB{x(k)} = [ 0  1  0  0 -1  0  0  0  0 ] . x(k)
[ 0  0  1  0  0 -1  0  0  0 ]

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

  • «Оценка состояния с помощью фильтра Калмана» Ф. Хаугена (PDF)
  • «Управляемость и наблюдаемость: инструменты для
    Kalman Filter Design «от Саутхолла, Бакстона и Маршана (PDF)
3

Другие решения


По вопросам рекламы [email protected]