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

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

Обозначим за \(x_{t}\) величину, которую мы будем измерять, а потом фильтровать. Мы будем измерять координату падающего камня. Движение камня задано формулой: \[x_{t}=400\cdot t-4.9\cdot t^2\] Выразим координату камня через ускорение и предыдущую позицию камня: \[x_{t+1}=400\cdot (t+1)-4.9\cdot (t+1)^2=400\cdot t+400-4.9\cdot t^2-9.8\cdot t-4.9=x_{t}-9.8\cdot t+395.1\] Так как взяв два раза производную от \(x_{t}\) получим, что ускорение равно -9.8, то координата камня будет изменяться по закону: \[x_{t+1}=x_{t}+a\cdot t+395.1\] где \(a=-9.8\) Но в реальной жизни мы не можем учесть в наших расчетах маленькие возмущения, действующие на камень, такие как: ветер, сопротивление воздуха и т.п., поэтому настоящая координата камня будет отличаться от расчетной. К правой части написанного уравнения добавится случайная величина \(E_{t}\) \[x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}\] Мы установили на земле под камнем дальномер. Дальномер будет измерять координату \(x_{t}\), но, к сожалению, он не может точно измерить ее и мерит с ошибкой \(N_{t}\),которая тоже является случайной величиной: \[z_{t}=x_{t}+N_{t}\] Задача состоит в том, чтобы, зная неверные показания дальномера \(z_{t}\), найти хорошее приближение для истинной координаты камня \(x_{t}\). Это приближение мы будем обозначать \(x_{t}^{opt}\). Таким образом, уравнение для координаты и показания дальномера будут выглядеть следующим образом: \[\begin{cases} x_{t+1}=x_{t}+a\cdot t+395.1+E_{t}\\ z_{t}=x_{t}+N_{t} \end{cases}\]

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

Идея Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате \(x_{t+1}\),мы должны выбрать золотую середину между показанием \(z_{t+1}\) неточного сенсора и \(x^{opt}_{t}+a\cdot t+395.1\) — нашим предсказанием того, что мы ожидали от него увидеть. Показанию сенсора мы дадим вес \(K\), а на предсказанное значение останется вес \((1-K)\): \[x^{opt}_{t+1}=K_{t+1}\cdot z_{t+1}+(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+395.1)\] где \(K_{t+1}\) - коэффициент Калмана, зависящий от шага итерации.
Мы должны выбрать \(K_{t+1}\) таким, чтобы получившееся оптимальное значение координаты \(x^{opt}_{t+1}\) было бы наиболее близкое к истиной координате \(x_{t+1}\). В общем случае, чтобы найти точное значение коэффициента Калмана \(K_{t+1}\) , нужно просто минимизировать ошибку: \[e_{t+1}=x_{t+1}-x^{opt}_{t+1}\] Подставляем в уравнение (7) выражение (8) и упрощаем: \[e_{t+1}=x_{t+1}–K_{t+1}\cdot z_{t+1}-(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+404.9)=\\=x_{t+1}-K_{t+1}\cdot (x_{t+1}+N_{t+1})-(1-K_{t+1})\cdot (x^{opt}_{t}+a\cdot t+404.9)=\\=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+404.9)=\\=(1-K_{t+1})\cdot (x_{t+1}–x^{opt}_{t}–a\cdot t–404.9)–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (x_{t}+a\cdot t+404.9+E_{t}–x^{opt}_{t}–a\cdot t–404.9)–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}\] Таким образом, получаем: \[e_{t+1}=(1-K_{t+1})\cdot (e_{t}+E_{t})–K_{t+1}\cdot N_{t+1}\] Мы будем минимизировать среднее значение от квадрата ошибки: \(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\), то получаем: \[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})\] Где \(D(E_{t})\) и \(D(N_{t+1})\)-дисперсии случайных величин \(E_{t}\) и \(N_{t+1}\). Найдем минимальное значение для выражения (11) (т.е. найдем производную): \[-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})}\] Таким образом, получаем такое \(K_{t+1}\), что выражение \(E(e^{2}_{t+1})\) будет минимальным: \[K_{t+1}=\frac{E(e^{2}_{t})+D(E_{t})}{E(e^{2}_{t})+D(E_{t})+D(N_{t})}\] Случайные величины имеют нормальный закон распределения и мы знаем, что их дисперсии равны: \(\delta^{2}_{E}\) и \(\delta^{2}_{N}\). Заметим, что дисперсии не зависят от t, потому что законы распределения не зависят от него. Подставляем в выражение для среднеквадратичной ошибки