ROUGH DRAFT authorea.com/113418
Main Data History
Export
Show Index Toggle 8 comments
  •  Quick Edit
  • Алгоритмы локации и маршрутизации. Алгоритм Калмана-Салова

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

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

    Мы будем минимизировать среднее значение от квадрата ошибки: \(E(e^{2}_{t+1})\longrightarrow\min\) Т.к. все входящие в \(e_{t+1}\)) случайные величины независимые и средние значения ошибок сенсора и модели равны нулю: \(E[E_{t}]=E[N_{t+1}]=0\), и все перекрестные значения равны нулю: \(E[E_{t}\cdot N_{t+1}]=E[e_{t}\cdot E_{t}]=E[e_{t}\cdot N_{t+1}]=0\), то получаем:

    \begin{equation} \label{Ee}E(e^{2}_{t+1})=(1-K_{t+1})^{2}\cdot(E(e^{2}_{t})+D(E_{t}))+K^{2}_{t+1}\cdot D(N_{t})\\ \end{equation}

    Где \(D(E_{t})\) и \(D(N_{t+1})\)-дисперсии случайных величин \(E_{t}\) и \(N_{t+1}\). Найдем минимальное значение для выражения (\ref{Ee}) (т.е. найдем производную):

    \begin{equation} -2\cdot(1-K_{t+1})\cdot(E(e^{2}_{t})+D(E_{t}))+2\cdot K_{t+1}\cdot D(N_{t})=0\\ -E(e^{2}_{t})–D(E_{t})+K_{t+1}\cdot E(e^{2}_{t})+K_{t+1}\cdot D(E_{t})+K_{t+1}\cdot D(N_{t})=0\\ K_{t+1}=\frac{E(e^{2}_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}\nonumber \\ \end{equation}

    Таким образом, получаем такое \(K_{t+1}\), что выражение \(E(e^{2}_{t+1})\) будет минимальным:

    \begin{equation} K_{t+1}=\frac{E(e^{2}_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}\nonumber \\ \end{equation}

    Случайные величины имеют нормальный закон распределения и мы знаем, что их дисперсии равны: \(\delta^{2}_{E}\) и \(\delta^{2}_{N}\). Заметим, что дисперсии не зависят от t, потому что законы распределения не зависят от него. Подставляем в выражение для среднеквадратичной ошибки \(E(e^{2}_{t+1})\) минимизирующее ее значение коэффициента Калмана \(K_{t+1}\)(Ramazan Gencay 2002) и получаем:

    \begin{equation} E(e^{2}_{t+1})=(1-\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot(E(e^{2}_{t})+\delta^{2}_{E})+(\frac{E(e^{2}_{t})+\delta^{2}_{E}}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot\delta^{2}_{N}\nonumber \\ \end{equation}

    Пусть: \(E(e^{2}_{t})=a;\delta^{2}_{E}=b;\delta^{2}_{N}=c;\) Тогда:

    \begin{equation} E(e^{2}_{t+1})=(1-\frac{a+b}{a+b+c})^{2}\cdot(a+b)+(\frac{a+b}{a+b+c})^{2}\cdot c=\frac{c^{2}\cdot(a+b)}{(a+b+c)^{2}}+\frac{c\cdot(a+b)^{2}}{(a+b+c)^{2}}=\\ =\frac{c\cdot(a+b)\cdot(c+a+b)}{(a+b+c)^{2}}=\frac{c\cdot(a+b)}{a+b+c}=\frac{\delta^{2}_{N}\cdot(E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}}\nonumber \\ \end{equation}

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

    \begin{equation} E(e^{2}_{t+1})=\frac{\delta^{2}_{N}\cdot(E(e^{2}_{t})+\delta^{2}_{E})}{E(e^{2}_{t})+\delta^{2}_{E}+\delta^{2}_{N}}-среднее\ значение\ квадрата\ ошибки\nonumber \\ \end{equation}

    Таким образом, мы получили формулу для вычисления коэффициента Калмана.