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

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

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