se.moevm.info

Суть EKF SLAM можно найти в статье

Общий алгоритм представлен на рисунке:

Во-первых, зафиксируемся некоторым “набором выборки”, в котором будут лежать все состояния системы за всё время. Каждое состояние - это вектор из трёх элементов: x - положение робота w - вес вектора m - карта на текущий момент времени

Итак, в полученном наборе St будет столько же элементов, сколько в S_(t-1), поскольку алгоритм предполагает прохождение по всем элементам набора S_(t-1), и на каждом шаге в этот набор добавляется по одному элементу. Набор St можно считать различными гипотезами о текущем положении робота. Совершенно необязательно иметь только одну гипотезу. Наоборот, выгоднее просчитывать несколько вариантов, потмоу что окружающий мир может быть фрактальным и в этом случае наличие нескольких гипотез может помочь определить реальное положение.

Теперь подробнее о том, как строится каждый элемент. Сначала к значению, полученному на предыдущем шаге прибавляется значение одометрии и получается x' - априорная оценка. Затем ищется argmax плотности распределения величины x в зависимости от текущего знания о карте m, текущего наблюдения z и априорной оценки x'.

Если апостериорная оценка сильно отличается от априорной, то апостериорная оценка отбрасывается, в качестве предположения о новом положении робота выбирается случайная величина, распределённая как p(x| x_(t-1), u_(t-1)), то есть исключительно на основе предыдущего положения и данных одометрии. Вес наблюдения считается также тривиально.

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

Вне зависимости от того, как считается x, на основании этого значения обновляется карта и новый посчитанный вектор добавляется в набор выборки.

По завершении работы цикла высчитывается количество элементов, которые должно содержать St, чтобы дисперсия весов была равна единице (чтобы они были распределены по стандартному нормальному закону). Если это количество оказалось меньше, чем реальное значение T, то всю операцию необходимо проделать заново, чтобы увеличить веса построенных предположений x.