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

Постановка задачи

Обозначим за \(x_{t}\) величину, которую мы будем измерять, а потом фильтровать. Мы будем измерять координату бронепоезда, который может ехать только вперед и назад. Движение бронепоезда задано формулой:

\begin{equation} x_{t}=5+2\cdot t+0.1\cdot t^{2}\nonumber \\ \end{equation}

Выразим координату бронепоезда через ускорение и предыдущую позицию бронепоезда:

\begin{equation} x_{t+1}=5+2\cdot(t+1)+0.1\cdot(t+1)^{2}=5+2\cdot t+0.1\cdot t^{2}+2+0.2\cdot t+0.1=\\ =x_{t}+0.2\cdot t+2.1\nonumber \\ \end{equation}

т.к. взяв два раза производную от \(x_{t}\) получим, что ускорение равно 0.2, то координата бронепоезда будет изменяться по закону:

\begin{equation} x_{t+1}=x_{t}+a\cdot t+2.1\nonumber \\ \end{equation}

где \(a\)=0.2 Но в реальной жизни мы не можем учесть в наших расчетах маленькие возмущения, действующие на бронепоезд, такие как: ветер, качество рельс и т.п., поэтому настоящая координата бронепоезда будет отличаться от расчетной. К правой части написанного уравнения добавится случайная величина \(E_{t}\)

\begin{equation} x_{t+1}=x_{t}+a\cdot t+2.1+E_{t}\nonumber \\ \end{equation}

Мы установили на бронепоезд GPS сенсор. Сенсор будет измерять координату \(x_{t}\), но, к сожалению, он не может точно измерить ее и мерит с ошибкой \(N_{t}\),которая тоже является случайной величиной:

\begin{equation} z_{t}=x_{t}+N_{t}\nonumber \\ \end{equation}

Задача состоит в том, чтобы, зная неверные показания сенсора \(z_{t}\), найти хорошее приближение для истинной координаты бронепоезда \(x_{t}\). Это приближение мы будем обозначать \(x^{opt}_{t}\). Таким образом, уравнение для координаты и показания сенсора будут выглядеть следующим образом:

\begin{equation} \begin{cases}x_{t+1}=x_{t}+a\cdot t+2.1+E_{t}\\ z_{t}=x_{t}+N_{t}\end{cases}\nonumber \\ \end{equation}

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

Идея Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате \(x_{t+1}\),мы должны выбрать золотую середину между показанием \(z_{t+1}\) неточного сенсора и \(x^{opt}_{t}+a\cdot t+2.1\) — нашим предсказанием того, что мы ожидали от него увидеть. Показанию сенсора мы дадим вес \(K\), а на предсказанное значение останется вес \((1-K)\)(David 2016):

\begin{equation} \label{xopt}x^{opt}_{t+1}=K_{t+1}\cdot z_{t+1}+(1-K_{t+1})\cdot(x^{opt}_{t}+a\cdot t+2.1)\\ \end{equation}

где \(K_{t+1}\) - коэффициент Калмана, зависящий от шага итерации.
Мы должны выбрать \(K_{t+1}\) таким, чтобы получившееся оптимальное значение координаты \(x^{opt}_{t+1}\) было бы наиболее близкое к истиной координате \(x_{t+1}\). В общем случае, чтобы найти точное значение коэффициента Калмана \(K_{t+1}\) , нужно просто минимизировать ошибку:

\begin{equation} \label{et}e_{t+1}=x_{t+1}-x^{opt}_{t+1}\\ \end{equation}

Подставляем в уравнение (\ref{xopt}) выражение (\ref{et}) и упрощаем:

\begin{equation} e_{t+1}=x_{t+1}–K_{t+1}\cdot z_{t+1}-(1-K_{t+1})\cdot(x^{opt}_{t}+a\cdot t+2.1)=\\ =x_{t+1}-K_{t+1}\cdot(x_{t+1}+N_{t+1})-(1-K_{t+1})\cdot(x^{opt}_{t}+a\cdot t+2.1)=\\ =x_{t+1}\cdot(1-K_{t+1})-K_{t+1}\cdot(x_{t+1}+N_{t+1})-(1-K_{t+1})\cdot(x^{opt}_{t}+a\cdot t+2.1)=\\ =(1-K_{t+1})\cdot(x_{t+1}–x^{opt}_{t}–a\cdot t–2.1)–K_{t+1}\cdot N_{t+1}=\\ =(1-K_{t+1})\cdot(x_{t}+a\cdot t+2.1+E_{t}–x^{opt}_{t}–a\cdot t–2.1)–K_{t+1}\cdot N_{t+1}=\\ =(1-K_{t+1})\cdot(x_{t}–x^{opt}_{t}+E_{t})–K_{t+1}\cdot N_{t+1}=\\ =(1-K_{t+1})\cdot(e_{t}+E_{t})–K_{t+1}\cdot N_{t+1}\nonumber \\ \end{equation}

Таким образом, получаем:

\begin{equation} e_{t+1}=(1-K_{t+1})\cdot(e_{t}+E_{t})–K_{t+1}\cdot N_{t+1}\nonumber \\ \end{equation}

Мы будем минимизировать среднее значение от квадрата о