Диссертация (1090939), страница 15
Текст из файла (страница 15)
При этом значение β = 2116характеризует нормальное распределение. Более подробное описаниепараметров сигма-точечного преобразования содержится в работе [94].Затем, на основе значений преобразованных сигма-точек оцениваютсяматематическое ожидание2Ly Wi ( m ) yii 0и ковариация2LPy Wi ( c ) [ yi yi ][ yi yi ]Ti 0случайной величины y.3.3.2. Алгоритм сигма-точечного фильтра КалманаПусть вектор состояния системы x имеет длину n: x = (x1, x2, ..., xn)T.При этом вектор шума процесса имеет вид ν = (ν1, ν2, ..., νn)T. Векторыизмерений z и шума измерений q имеют длину m: z = (z1, z2, ..., zm)T, q = (q1,q2, ..., qn)T.
Динамика системы описывается уравнениями:xk f ( xk 1 , vk 1 ),z k h( xk , qk ).В самом общем случае, когда известно, как шумы процесса иизмерений влияют на динамику системы, можно дополнить вектор состояниясистемы соответствующими шумами. Тогда дополненный вектор состояниясистемы с числом компонент 2n+m примет вид:x ( x1 , x2 ,..., xn , v1 , v2 ,..., vn , q1 , q2 ,..., qm ) ( xT , vT , qT )T .В этом случае уравнение перехода состояния от шага k-1 к шагу kможно представить в виде: xk F ( xk 1 ) , а функцию предсказания измерениядля дополненного вектора состояния x k на шаге k можно обозначить как:zk H ( xk ). Матрица ковариацииPkпримет вид:117дополненного вектора состоянияcov( x, x) kPk cov( v, x) kcov( q, x) kcov( x, v) kcov( v, v) kcov( q, v) kcov( x, q) k cov( v, q) k .cov( q, q) k Однако, как правило, величины шума не коррелирован, поэтому сучетом матриц ковариации процесса Pv,k и измерений Pq,k, общую матрицуковариации можно записать как: PkPk 0 00Pv ,k0,Pq ,k 00где нулевыми элементами являются нулевые матрицы соответствующегоразмера.Как и в классической реализации фильтра Калмана, сигма-точечныйфильтр Калмана содержит этапы прогноза и коррекции.
Перед началомработы основной процедуры фильтра, на шаге инициализации задаютсяосновные параметры работы алгоритма.Инициализация.На шаге k=0 задаются начальные значения для математическогоожидания и матрицы ковариации: x0 M [ x0 ] M [ x0 ] x0 M [ x0 ] M v0 M [v0 ] 0 , q0 M [q0 ] 0 PkPk 0 00Pv ,k00 0 .Pq ,k Этап предсказания.Производитсясигма-точечноепреобразованиедляоценкиматематического ожидания вектора состояния – вычисление списка k из2L+1 сигма-точек:118 k, 0 xk , k, 0 xk ( Pk )i , i 1,.., L, k, 0 xk ( Pk )i , i L,..,2 L,трансформация сигма-точек с помощью функции перехода системы: kT,i F ( k,i ), i 0,..,2L,оценказначенияматематического2L2Li 0i 0ожидания:xk Wi ( m ) kT,i и оценка матрицы ковариации PTk Wi ( c ) [ kT,i xk ][ kT,i xk ]T .Аналогичным образом на основе списка трансформированных сигма-точекформируетсясписокпредсказанныхзначенийzkT,i H ( k,i ), i 0,..,2L,производится оценка математического ожидания, ковариации измерений, атакже оценка взаимной ковариации состояния и измерений:2LPxz ,k Wi ( c ) [ kT,i xk ][ kT,i zk ]T .i 0Этап коррекции.Вычисляется коэффициент усиления фильтра Калмана Kk Pxz ,k Pz,k ,оптимальная оценка xˆk xk K k ( zk z k ) на основе нового измерения zk.Также обновляется матрица ковариации Pk PkT Kk Pz,k KkT .3.3.3.
Реализация сигма-точечного фильтра КалманаДля решения задачи оценки положения пространственных ориентиров,движение которых характеризуется сильно нелинейной природой, сигматочечный фильтр Калмана был реализован на языке C# с применениембиблиотеки Math.NET Numerics для выполнения ряда операций линейнойалгебры. В задаче нахождения матрицы ковариации применяется разложениеХолецкого[3].Вектороцениваемыхпараметровпространственныхориентиров, представляющий вектор состояния, включает в себя 6 величин:координаты ориентира в трехмерном пространстве, а также скорость ихизменения.119Таккакпроцессгенерациисигма-точекобладаетпостояннойвычислительной сложностью, итоговая сложность алгоритма одновременнойлокализации и картирования FastSLAM с использованием сигма-точечногофильтра Калмана также определяется числом частиц и зависит него линейно.Таким, образом вычислительная сложность алгоритма остается такой же, каки в классическом подходе FastSLAM.
Если говорить непосредственно опроцессе фильтрации положения ориентиров, то разработанный сигматочечный фильтр Калмана показывает в среднем напроизводительностьотносительнореализации6% худшуюрасширенногофильтраКалмана, присутствующей в библиотеке OpenCV. Однако, результатыподобныхсравненийдействительностинедостаточноскоростьрепрезентативны,конкретнойпрограммнойтаккак,вреализациикардинально зависит от различных низкоуровневых оптимизаций, непроизводившихся в данном исследовании. Как показывает анализ рядапубликаций, производительности расширенного фильтра Калмана и сигматочечного фильтра Калмана в большинстве случаев равны [53].Тестированиеалгоритмапроизводилосьсприменениемнаборовтестовых данных, аналогичных используемым в главе 2. Как показалапрактика, в случае наблюдения величин строго нелинейной природы,использование сигма-точечного фильтра Калмана позволяет значительно (внекоторыхслучаях–нанесколькопорядков)уменьшитьсреднеквадратическую ошибку оценивания в сравнении с расширеннымфильтром Калмана.
На рисунке 3.12 приведен график средней ошибкивизуальной одометрии при оценке движения камеры в зависимости от числараспознанных ключевых точек. Значения нормированы на максимальнуювеличину ошибки.120Рис. 3.12. Зависимость средней ошибки при оценке движения камеры взависимости от числа распознанных ключевых точек для расширенногофильтра Калмана и сигма-точечного фильтра КалманаВ таблице 3.1 приведены результаты оценки среднеквадратическойошибки полной траектории движения камеры с использованием фильтрачастиц для оценки положения камеры при использовании расширенногофильтра Калмана (РФК) и сигма-точечного фильтра Калмана (СТФК) дляоценки координат пространственных ориентиров.Таблица 3.1.
Сравнение точности алгоритмов на основе РФК и СТФККол-во особых точек102050100Относит.FastSLAM+РФК 25.620.812.710.5ошибка, % FastSLAM+СТФК 14.112.19.78.4Таким образом, наибольшее преимущество сигма-точечного фильтраКалмана при оценке движения ориентиров наблюдается в случаях, когдаколичество особых точек на изображениях является небольшим. Этохарактерно для случаев, когда изображения не обладают достаточным121количеством высококонтрастных ключевых особенностей, что может иметьместо, если окружающее пространство изобилует крупными однотоннымиобластями. Либо, что характерно для систем прикладного объемноготелевидения, для большого числа ориентиров могут быть недоступны данныео глубине, если объект, к которому принадлежат особые точки находится внерабочего диапазона карты глубины.
Кроме того, если к разрабатываемойпрограммно-аппаратной системе предъявляются строгие требования кпроизводительности, это также может привести к необходимости сократитьколичество отслеживаемых ориентиров.3.4.Адаптивный алгоритм генерации частицФильтр частиц представляет собой моделирующий метод оценкисостояния зашумленной системы, хранящий взвешенное, нормализованноемножествовыборкисостояний,называемыхчастицами.Врамкахрассматриваемой задачи каждая частица представляет собой предполагаемыекоординаты камеры в пространстве в совокупности с вектором состояниянаблюдаемых пространственных ориентиров.
В течение каждого циклаработы фильтра после получения вектора измерений создается выборкановых состояний, каждое из которых взвешивается согласно Марковскоймодели наблюдений. После чего веса нормализуются для нового множествасостояний [4]. Описание принципа работы фильтра частиц приводится впервой главе диссертации.3.4.1. Пороговое значение эффективного числа частицОднимизнаиболееважныхэтаповработыфильтрачастиц,оказывающим сильное влияние на производительность и точность работыфильтра, является процесс обновления вектора частиц. Задача при этомсостоит в замене частиц с меньшими весами частицами с большими весами,при котором часть значений отсеивается по определенному закону.
В даннойработе для этого используется метод, называемый колесом отсева122(Resampling wheel), подробно рассмотренный в работах [27, 34]. Моментцикла работы фильтра, при котором необходимо обновить набор частицопределяется величиной, называемой эффективным размером выборки,которая определяется как:N eff 1 wˆ [k ] 2N,k 1где N – это общее количество частиц, а wˆ [ k ] – нормализованный вес k–йчастицы. Когда разброс весов частиц относительно среднего значенияувеличивается, величина Neff уменьшается. В том случае, когда веса частицˆ [ k ] 1/ N ), эффективный размер выборки будет равен числуодинаковы (т.е.
wчастиц. В противоположном случае, когда одна единственная частицаобладает ненулевым весом, эффективный размер равен единице Neff = 1.В большинстве решений необходимость обновления набора частицопределяется жестко заданным порогом υ отношения величины Neff к общемучислу частиц N, при достижении которого выполняется отсев частиц снаименьшими весами и генерация новых. Величина порога, как правило,находится эмпирическим путем. В большей части работ при этом величинапорога выбирается на уровне 50% [42].
Это позволяет ограничить ростпогрешности работы фильтра.Однако, фиксированное задание значения порога не учитываетэволюцию процесса изменения величины Neff, что может приводить как кчрезмерной сложности алгоритма, так и к вырождению выборки частиц.Достаточно рассмотреть пример такого случая, когда эффективный размервыборкиотносительноневелик,однакоегозначениеосциллируетотносительно некоторого постоянного значения, что приводит к хорошемууровню аппроксимации искомого процесса.
В этом случае порог необходимопонизить для предотвращения частого обновления выборки частиц. Также,среднееколичествоэффективныхчастиц123втечениеопределенноговременного интервала характеризует качество аппроксимации процесса втечение этого интервала.Необходимость адаптивного задания порога обновления выборкичастицвособенностихарактернадляразрабатываемогоалгоритмаодновременной локализации и картирования, где основным методомизмерения движения камеры является визуальная одометрия. Точностьработы алгоритма визуальной одометрии напрямую зависит от количествапространственных ориентиров, которое постоянно меняется в зависимости отхарактера сцены и сопутствующих условий. Даже если детектор особыхточек позволяет найти достаточное количество ориентиров, не все онинаходятся в рабочем диапазоне карты глубины.