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

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

    Предположим, что входной сигнал описывает некоторый авторегрессионный процесс первого порядка: \[\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 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\]

    Симуляция работы фильтра

    Предполагаем, что первоначальное значение сигнала \(x_{t}\) и отфильтрованное значение \(\hat{x}^p_{t}\) равны нулю.

    В момент времени \( t = 1\) ,после получения первого наблюдения \(y_{1} = 2,86 \cdot 10^{-7}\) предсказание координаты сигнала определяется следующим образом: \[\hat{x}^e_{1} = 0 + k_{1} \cdot (y_{1} - 0 ) = 0,0280 \cdot (2,86 \cdot 10^{-7} - 0) = 2,8729 \cdot 10^{-7}\] На следующем шаге (\(t = 2\)) можно посчитать отфильтрованное значение - \(\hat{x}^p_{2}\): \[\hat{x}^p_{2} = 1 \cdot u + \phi \cdot \hat{x}^e_{1} = 1 \cdot 0,8 + 0,26 \cdot 2,8729 \cdot 10^{-7} = 0,8\] Предсказанное значение наблюдения \[\hat{y}_{2} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 0,8 = 0,567\] В момент времени \(t = 2\), в качестве предсказания сигнала выступает отфильтрованное значение, выполненное во время \(t = 1\). Данная оценка обновляется после наблюдения \(y_{2} = 0,5922\): \[\hat{x}^e_{2} = \hat{x}^p_{2} + k_{2} \cdot (y_{2} - \hat{y}_{2} ) = 0,8 + 0,0298 \cdot (0,5922 - 0,567) = 0,8209\] Отсюда \[\hat{x}^p_{3} = 2 \cdot u + \phi \cdot \hat{x}^e_{2} = 1 \cdot 0,8 + 0,26 \cdot 0,8209 = 1,8134\] \[\hat{y}_{3} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 1,8134 = 1,3057\] В общем случае, когда усиление Калмана сходиться, предсказанное значение \(\hat{x}^e_{t}\) и отфильтрованное значение \(\hat{x}^p_{t}\) в момент времени \(t = 1 \) определяются следующими выражениями: \[\label{lab22} \hat{x}^e_{t} = \hat{x}