Внедрение 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.
Имеет ли это смысл или я уже ошибаюсь?
Как мне отрегулировать матрицу переходов и измерений, а также соответственно обновить текущее состояние?
Решение
Вы на правильном пути, но вы, кажется, запутались в понятиях функции перехода и функции измерения.
Первое, что нужно сделать, это четко определить, какой вектор состояния вы хотите рассмотреть. В вашем случае достаточно положения [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 ]
При этом измерение скорости системы и разности ее положений практически одинаково. Поскольку вы никогда не измеряете положение, ваше состояние может быть незаметным, что, вероятно, будет проблемой. Посмотрите на следующие документы для более подробной информации:
Другие решения