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

Постановка задачи и начальные условия

Предположим, что входной сигнал описывает некоторый авторегрессионный процесс первого порядка: \[\label{lab1} x_{t} = \phi \cdot x_{t-1} + u \cdot t + \upsilon_{t} = 0,26 \cdot x_{t-1} + 0,8 \cdot t + \upsilon_{t} ,\] где \(\upsilon_{t} \sim N(\bar{m_{\upsilon}},\sigma^2_{\upsilon})\) - некоторая помеха сигнала, произвольная случайная величина, распределенная нормально с параметрами \(\bar{m_{\upsilon}} = 0\) ( мат. ожидание), а \( \sigma^2_{\upsilon} = 0,2\) (дисперсия помехи сигнала). Уравнение наблюдение будет выглядеть следующим образом: \[\label{lab2} y_{t} = \gamma \cdot x_{t} + \epsilon_{t} = 0,72 \cdot x_{t} + \epsilon_{t} ,\] где \(\epsilon_{t} \sim N( \bar{m_{\epsilon}},\sigma^2_{\epsilon} )\) также случайная величина - помеха при наблюдении, распределенная нормально с параметрами \(\bar{m_{\epsilon}} = 0\) (мат. ожидание), а \( \sigma^2_{\epsilon} = 5\) (дисперсия помехи наблюдения).

Более того предполагается, что помехи наблюдения и сигнала некоррелированы ( т.е \(E(\epsilon_{t-i} \upsilon_{t-j} , \forall i,j)\) ).

Целью данной работы являлась симуляция работы фильтра Калмана (Gençay 2002) на некотором сигнале (\ref{lab1}),где в качестве входных данных выступают дисперсии помехи сигнала \(\sigma^2_{\upsilon}\) и помехи наблюдения \(\sigma^2_{\epsilon}\).

Расчет основных параметров фильтра

Прежде всего заметим, что значения усиления Калмана (Kalman Gain) \(K_{t}\) и дисперсии ошибки фильтрации \(p^p_{t}\)(prediction variable) и дисперсии ошибки предсказания \(p^e_{t}\)(estimation variable) сходятся. Усиление Калмана представляет собой следующее выражение: \[\label{lab3} K_{t} = \frac{\gamma \cdot S_{t} }{\gamma^2 \cdot S_{t} + \sigma^2_{\epsilon}} = \frac{(0,72) \cdot S_{t}}{(0,72)^2 \cdot S_{t} + 5}\] где \(S_{t} \) - дисперсия ошибки фильтрации \(x_{t}\) в момент \(t-1\), и определяется следующим выражением: \[\label{lab4} S_{t} = \phi^2 \cdot p^e_{t-1} + \sigma^2_{\upsilon} = (0,26)^2 \cdot p^e_{t-1} + 0,2\] Дисперсия ошибки предсказания \(p^e_{t}\) и дисперсия ошибки фильтрации \(p^p_{t+1}\) соответсвенно равняются: \[\label{lab5} p^e_{t} = (1 - \gamma \cdot K_{t}) \cdot S_{t} = (1 - 0,72 \cdot K_{t}) \cdot S_{t}\] \[\label{lab6} p^p_{t+1} = p^e_{t} \cdot \phi^2 + \sigma^2_{\upsilon} = (0,26)^2 \cdot p^e_{t-1} + 0,2 = S_{t+1}\] Предположим, что в начальный момент времени ( \(t = 1 \) ) дисперсия ошибки фильтрации равна дисперсии сигнала \(p^p_{1} = S_{1} = \sigma^2_{\upsilon}\). Тогда:

\[K_{1} = \frac{0,72 \cdot 0,2}{0,72^2 \cdot 0,2 + 5} = 0,0280\]

\[p^e_{1} = (1 - 0,72 \cdot 0,0280) \cdot 0,2 = 0,1960\] \[p^p_{2} = 0,1960 \cdot 0,26^2 + 0,2 = 0,2132\] В момент времени \(t=2\): \[K_{2} = \frac{0,72 \cdot 0,2132}{0,72^2 \cdot 0,2132 + 5} = 0,0298\] \[p^e_{2} = (1 - 0,72 \cdot 0,0298) \cdot 0,2132 = 0,2087\] \[p^p_{3} = 0,2087 \cdot 0,26^2 + 0,2 = 0,2141\] В момент времени \(t=3\): \[K_{3} = \frac{0,72 \cdot 0,2141}{0,72^2 \cdot 0,2141 + 5} = 0,0299\] \[p^e_{3} = (1 - 0,72 \cdot 0,0299) \cdot 0,2141 = 0,2095\] \[p^p_{4} = 0,2095 \cdot 0,26^2 + 0,2 = 0,2142\] После выполнения нескольких шагов, усиление Калмана, дисперсия ошибки предсказания и дисперсия ошибки фильтра сошлись к следующим значениям: \[\bar k_{mean} = 0,0297\] \[\bar p^e_{mean} = 0,2083\] \[\bar p^p_{mean} = 0,2141\]