Алгоритмы локации и маршрутизации. Алгоритм Калмана-Бойцова

Алгоритм Калмана

Фильтр Калмана — последовательный рекурсивный алгоритм, использующий принятую модель динамической системы для получения оценки, которая может быть существенно скорректирована в результате анализа каждой новой выборки измерений во временной последовательности. Этот алгоритм находит применение в процессе управления многими сложными динамическими системами, например, непрерывными производственными процессами, самолетами, кораблями и космическими аппаратами. При управлении динамической системой, прежде всего, необходимо полностью знать её фазовое состояние в каждый момент времени. Но измерение всех переменных, которыми необходимо управлять, не всегда возможно, и в этих случаях фильтр Калмана является тем средством, которое позволяет восстановить недостающую информацию посредством имеющихся неточных (зашумленных) измерений. Balacrishnan (1988)

Предполагаем, что стохастическая система \(x(t)=F(t)x(t)+\xi(t)\) может быть описана моделями динамики и измерений \(z=H(t)x(t)+\eta(t)\).
Здесь \(x(t)\) - верктор состояния динамической системы, который является случайным Гауссовским процессом, \(z_{k}\) - измерения, полученные в момент времени \(t_{k}\). Ошибки измерений \(\xi_{k}\) и \(\eta_{k}\) также являются случайными процессами с нулевым математическим ожиданием и независимы друг от друга

\begin{equation} E\xi_{k}=E\eta_{k}=0\\ \end{equation}

Задача фильтрации состоит в том, чтобы найти оценку вектора \(\hat{x}_{k}\) состояния системы \(x_{k}\), который является функцией измерений \(z_{i}...z_{k}\) и которая минимизирует средне квадратичную ошибку (\ref{e3})

\begin{equation} \label{e3}E\langle[x_{k}-\hat{x}_{k}]^{T}M[x_{k}-\hat{x}_{k}]^{T}\rangle\\ \end{equation}

где \(M\) - симметричная положительно-определенная матрица.
Фильтр Калмана работает по системе прогноз-коррекция. Допустим, что в момент времени \(t_{k-1}\) получена оценка вектора состояния системы \(\hat{x}_{k-1}\). Теперь для того, чтобы получить оценку в момент