Алгоритм Марковской локализации с выборочными вычислениями
Вычисление прогноза для перемещения робота:
• Применить выражение (6.14) для всех состояний ξz.1, находящихся внутри областей обновления, полученных на основе сенсорных данных предыдущей итерации алгоритма локализации.
• Если на предыдущей итерации сенсорные данные отсутствовали (а следовательно, не вычислялись области обновления и не выполнялось согласование плотности распределениято в (6.14) используется традиционная методика выборочных вычислений.
Согласование прогноза с текущими данными сенсоров:
• Выбрать на карте одиночные ориентиры или пары ориентиров подобные ориентирам, обнаруженных роботом.
• Используя координаты выбранных ориентиров определить связанные с ними области обновления, вычислить их параметры — их координаты в рабочем пространстве и их размеры (вычислить матрицы Cr), спроецировать области обновления на дискретное пространство состояний Ξ (дискретную сетку).
• Вычислить локальные сенсорные функции плотности условной вероятностидля каждой области обновления и на их основе, используя (6.20),
получить общую сенсорную функцию плотности условного распределения
• Применить уравнение (6.15) для всех состояний внутри областей обнов
ления. Если априорная вероятностьто установить
гдеобщее число состояний.
• Вычислить нормирующий множитель η.
6.3.3.