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

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

    Фильтр Калмана — эффективный рекурсивный фильтр, который оценивает состояние линейной динамической системы по серии неточных измерений. Назван в честь Рудольфа Калмана. (G. Welch 2001)

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

    Обозначим за \(x_k\) величину которую мы считываем с датчика и которую необходимо отфильтровать. Уравнение этой величины:
    \[x_{k+1} = x_{k} + a\cdot s\cdot tdt + \xi_k\] Где a,s – параметры, \(\xi_k\) – случайная величина, внешние влияния на движение.
    Поскольку у датчика есть погрешность, мы получаем координату с погрешностью \(\eta_k\):
    \[z_{k} = x_{k} + \eta_k\] Далее нам нужно найти фильтрованную величину \(x^{opt}\). Для этого воспользуемся формулой:
    \[x^{opt}_k = K\cdot z_{k+1} + (1-K)\cdot (x^{opt}_k + u_{k})\] где \(K_{k+1}\) - показания сенсора, \(1-K_{k+1}\) - предсказанное значение. Необходимо выбрать коэффициент Калмана \(K_{k+1}\) таким, чтобы \(x^{opt}_{k+1} \approx x_{k+1}\). Для этого минимизируем ошибку:
    \[e_{k+1} = x_{k+1} - x^{opt}_{k+1}\] \[e_{k+1} = (1-K_{k+1})\cdot (e_{k+1} + \xi_{k+1}) - K_{k+1}\cdot \eta_{k+1}\] Минимизируем среднее значение от квадрата ошибки:
    \[E(e^{2}_{k+1})\rightarrow min\\\] В итоге Коэффициент Калмана примет вид:
    \[K_{k+1}=\frac{Ee^{2}_{k}+\sigma_{\xi}^{2}}{Ee^{2}_{k}+\sigma_{\xi}^{2}+\sigma_{\eta}^{2}}\\\]
    (David 2016)

    Результаты

    Для тестирования фильтра были выбраны следующие значения параметров:
    \(N = 100\)
    \(a = 0.1\)
    \(s = -0.8\)
    И реализован пример на языке R: N=100; a = 0.1; s = -0.8; sigmaPsi=1; sigmaEta=30; k=1:N; x=k; x(1)=0; z(1)=x(1)+normrnd(0,sigmaEta); for t=1:(N-1) x(t+1)=x(t)+a*s*t+normrnd(0,sigmaPsi); z(t+1)=x(t+1)+normrnd(0,sigmaEta); end; xOpt(1)=z(1); eOpt(1)=sigmaEta; for t=1:(N-1) eOpt(t+1)=sqrt((sigmaEta^2)*(eOpt(t)^2+sigmaPsi^2)/(sigmaEta^2+eOpt(t)^2+sigmaPsi^2)); K(t+1)=(eOpt(t+1))^2/sigmaEta^2; xOpt(t+1)=(xOpt(t)+a*s*t)*(1-K(t+1))+K(t+1)*z(t+1); end; hold all; plot(k,xOpt); plot(k,z); plot(k,x,'--','Color',[.1 .0 .0],'LineWidth',1); title('Filtering results'); xlabel('Time (sec)'); ylabel('Coordinates (m)'); legend('Kalman filter','Sensor readings','Real values'); Результат фильтрации представлен на графике (Рис.\ref{ris:img1}). Как мы видим, отфильтрованные данные имеют близкие значения к реальной координате \(x_k\).

    \label{ris:img1}Результаты фильтрации

    References

    1. G. Bishop G. Welch. An Introduction to the Kalman Filter. (2001). Link

    2. Hydaverdyan David. Filter Kalman. (2016). Link