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

Государственное представительство и прогнозирование

Ядро EKF включает представление состояния робота как вектора, обычно включающего положение и ориентацию. Шаг предсказания использует нелинейную модель движения для проецирования текущего состояния вперед во времени. Это включает вычисление якобианской матрицы модели движения для линейного построения нелинейных уравнений вокруг текущей оценки.

Обновление измерений и линейность

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

Математические уравнения

Уравнения предсказания:

Государственный прогноз:

x ⁇ k+1− = f(x ⁇ k, uk)

Ковариационный прогноз:

Pk+1 = Fk Pk Fkt + Qk

где f — нелинейная модель движения, Fk — её якобианская, P — ковариационная матрица, Q — ковариация шума процесса.

Уравнения обновления:

Калман выигрывает:

Kk = Pk-Hkt (Hk Pk-Hkt + Rk)-1

Обновление состояния:

x ⁇ k = x ⁇ k− + Kk (zk — h(x ⁇ k−))

Обновление коэффициента:

Pk = (I - Kk Hk) Pk-

Здесь h - нелинейная модель измерения, Hk - ее якобиан, Rk - ковариация шума измерения, а zk - фактическое измерение датчика.