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

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

Обозначим за \(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}