this is for holding javascript data
Stepan Salov edited untitled.tex
almost 8 years ago
Commit id: 545945b3a78cea216d7c70a52133cfeb66378d74
deletions | additions
diff --git a/untitled.tex b/untitled.tex
index ef2df91..12a1b18 100644
--- a/untitled.tex
+++ b/untitled.tex
...
z(t)=x(t)+N_{t}
\end{cases}
\end{equation}
\section*{Алгоритм Калмана}
\addcontentsline{toc}{section}{Алгоритм Калмана}
Идея Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате $x(t+1)$,мы должны выбрать золотую середину между показанием $z(t+1)$ неточного сенсора и $xOpt(t)+a\cdot t+2.1$ — нашим предсказанием того, что мы ожидали от него увидеть. Показанию сенсора мы дадим вес $K$, а на предсказанное значение останется вес $(1-K)$:
$$
xOpt(t+1)=K_{t+1}\cdot z(t+1)+(1-K_{t+1})\cdot (xOpt(t)+a\cdot t+2.1)
$$
где $K_{t+1}$ - коэффициент Калмана, зависящий от шага итерации. \\
Мы должны выбрать $K_{t+1}$ таким, чтобы получившееся оптимальное значение координаты $xOpt(t+1)$ было бы наиболее близкое к истиной координате $x(t+1)$.
В общем случае, чтобы найти точное значение коэффициента Калмана $K_{t+1}$ , нужно просто минимизировать ошибку:
$$
e(t+1)=x(t+1)-xOpt(t+1)
$$
Подставляем в выражения уравнения и упрощаем:
$$
e(t+1)=x(t+1)–K_{t+1}\cdot z(t+1)-(1-K_{t+1})\cdot (xOpt(t)+a\cdot t+2.1)=\\=x(t+1)-K_{t+1}\cdot (x(t+1)+N_{t+1})-(1-K_{t+1})\cdot (xOpt(t)+a\cdot t+2.1)=\\=x(t+1)\cdot (1-K_{t+1})-K_{t+1}\cdot (x(t+1)+N_{t+1})-(1-K_{t+1})\cdot (xOpt(t)+a\cdot t+2.1)=\\=(1-K_{t+1})\cdot (x(t+1)–xOpt(t)–a\cdot t–2.1)–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (x(t)+a\cdot t+2.1+E_{t}–xOpt(t)–a\cdot t–2.1)–K_{t+1}\cdot N_{t+1}=\\=(1-K_{t+1})\cdot (x(t)–xOpt(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}$.
Найдем минимальное значение для этого выражения (т.е. найдем производную):
$$
-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, потому что законы распределения не зависят от него.
Подставляем в выражение для среднеквадратичной ошибки $E(e^{2}(t+1))$минимизирующее ее значение коэффициента Калмана $K_{t+1}$ и получаем:
$$
E(e^{2}(t+1))=(1-\frac{E(e^{2}(t))+\delta^{2}_{E}}{E(e^{2}(t))+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot (E(e^{2}(t))+\delta^{2}_{E})+(\frac{E(e^{2}(t))+\delta^{2}_{E}}{E(e^{2}(t))+\delta^{2}_{E}+\delta^{2}_{N}})^{2}\cdot \delta^{2}_{N}
$$
Пусть: $E(e^{2}(t))=a; \delta^{2}_{E}=b; \delta^{2}_{N}=c;$
Тогда:
$$
E(e^{2}(t+1))=(1-\frac{a+b}{a+b+c})^{2}\cdot (a+b)+(\frac{a+b}{a+b+c})^{2}\cdot c=\frac{c^{2}\cdot (a+b)}{(a+b+c)^{2}}+\frac{c\cdot (a+b)^{2}}{(a+b+c)^{2}}=\\=\frac{c\cdot (a+b)\cdot (c+a+b)}{(a+b+c)^{2}}=\frac{c\cdot (a+b)}{a+b+c}=\frac{\delta^{2}_{N}\cdot (E(e^{2}(t))+\delta^{2}_{E})}{E(e^{2}(t))+\delta^{2}_{E}+\delta^{2}_{N}}
$$
Таким образом получаем:
$$
E(e^{2}(t+1))=\frac{\delta^{2}_{N}\cdot (E(e^{2}(t))+\delta^{2}_{E})}{E(e^{2}(t))+\delta^{2}_{E}+\delta^{2}_{N}} - среднее\ значение\ квадрата\ ошибки
$$
Таким образом, мы получили формулу для вычисления коэффициента Калмана.
\section*{MatLab}
\addcontentsline{toc}{section}{MatLab}
Теперь реализуем всё вышесказанное в MatLab.\\
Код:
\begin{verbatim}
clear all;
% Задано движение формулой:
% x(t) = 5 + 2t + 0.1*t^2
% Т.е. ускорение равно 0.2, начальная скорость равно 2
% x(t+1) = x(t) + a*t + 2.1
N=100; % Количество измерений
a=0.2; % Ускорение
sigmaPsi=1;
sigmaEta=50;
k=1:N;
x=k;
x(1)=5; % Начальная координата
z(1)=x(1)+normrnd(0,sigmaEta);
for t=1:(N-1)
x(t+1)=x(t)+a*t+2.1+normrnd(0,sigmaPsi);
z(t+1)=x(t+1)+normrnd(0,sigmaEta);
end;
%Применяем фильтр Калмана
xOpt(1)=z(1); % База итерации
eOpt(1)=sigmaEta; % Т.к. eOpt(e0^2) = eOpt(Eta0^2) = 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*t+2.1)*(1-K(t+1))+K(t+1)*z(t+1);
end;
hold all;
plot(k,xOpt,'LineWidth',2);
plot(k,z);
plot(k,x,'--','LineWidth',2);
ylabel('Координата (м)');
xlabel('Время (сек)');
legend('Фильтрация сенсора по Калману', 'Показания сенсора', 'Истинная координата');
\end{verbatim}
Запустим программу и получим график на котором будут располагаться 3 линии: линия истинного положения бронепоезда, линия показания сенсора и линия полученная после использования фильтра Калмана.\\
Результат выполнения программы:
\begin{figure}[h!]
\begin{center}
\includegraphics[width=0.77\columnwidth]{figures/Salov/123.png}
\end{center}
\end{figure}
Как мы видим из графика, фильтр Калмана очень хорошо фильтрует данные, и мы получаем близкие значения к реальной координате $x(t)$