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

Пусть \(x_{t}\) - координата объекта, движущегося по прямой по следующему закону: \[{x_{t}=3t+2t^2}\]

Выразим координату через предыдущее положение объекта: \[x_{t+1}=3\left(t+1\right)+2\left(t+1\right)^2=3t+3+2t^2+4t+2=3t+2t^2+4t+5=x_{t}+4t+5\]

Поскольку в реальном мире на объект наблюдений всегда действуют посторонние силы, которые мы не можем измерить, реальная координата объекта будет отличаться от теоретической, поэтому, к правой части уравнения, мы добавим случайную величину \(\psi\)

Получаем: \[\label{iks} x_{t+1}=x_t+4t+5+\psi_t\]

На объекте установлен GPS-сенсор, который будет измерять координату. Поскольку у каждого измерительного прибора есть погрешность, он меряет координату с ошибкой \(\eta\), которая тоже является случайной величиной.

Получаем: \[\label{zet} z_t=x_t+\eta_t\]

Цель задания заключается в следующем: Зная неверные измеренные GPS-сенсором показания, найти хорошее приближение для истиной координаты объекта. Это приближение мы обозначим как \(x_t^{opt}\) \cite{2009}

Идея Фильтра Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате объекта. Для этого нам надо выбрать какое-то значение между \(x_t\)- нашим предсказанием и \(z_t\) - измеренным значением. Показание сенсора будет иметь вес \(K\). Тогда для предсказания остается \((1−K)\). \(K\) так же называется коэфицентом Калмана и зависит от \(t\)

\[\label{opt} x^{opt}_{t+1}=K_{t+1}*z_{t+1}+(1-K_{t+1})*(x_t^{opt}+4t+5)\]

Наша задача заключается в том, чтобы получившееся оптимальное значение \(x^{opt}_{t+1}\) максимально приблизить к реальному \(x_{t+1}\) В общем случае, чтобы найти точное значение коэффициента Калмана \(K_{t+1}\) , нужно просто минимизировать ошибку: \[e_{t+1}=x_{t+1}−x^{opt}_{t+1}\]

Развернув эту формулу с помощью уравнений (\ref{opt}), (\ref{zet}) и (\ref{iks}) получаем: \[e_{t+1}=(1−K_{t+1})*(e_t + \psi_t)–K_{t+1}*\eta_{t+1}\]

Для минимизации ошибки нужно минимизировать среднее значение от квадрата ошибки:

\[E(e^2_{t+1})→min\]

Для этого приравняем произодную к нулю:

\[−2*(1−K_{t+1})*(E(e^2_{t})+D(\psi_t))+2*K_{t+1}*D(\eta_t)=0\] где \(D(\psi_t)\) и \(D(\eta_t)\) - дисперсии соответствующих случайных величин

Отсюда получаем коэфицент Калмана, при котором \(E(e^2_{t+1})\) будет минимальным \cite{kalm02}: \[K_{t+1} = \frac{E(e^2_{t})+D(\psi_t)}{E(e^2_{t})+D(\psi_t)+D(\eta_t)}\]

Результаты

Для тестирования фильтра был выбран описаный выше пример: \( {x_{t}=3t+2t^2} \); \(x_{t+1}=x_t+4t+5+\psi_t\)

И реализован на языке R: clear all; % x(t+1) = x(t) + 4*t + 5 N=30; % Number of measurements sigmaPsi=1; sigmaEta=55; 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)+4*t+5+normrnd(0,sigmaPsi); z(t+1)=x(t+1)+normrnd(0,sigmaEta); end; % Kalman filter 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)+4*t+5)*(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('Coordinates'); xlabel('Time'); legend('Kalman filter', 'Sensor measuring', 'Real');

Ниже преведена таблица первых 10 значений координаты:

После запуска программы получаем следующий график на рис. \ref{ris:image}

По результатам представленным в таблице и графику видно что фильтр Калмана действительно хорошо фильтрует данные, и мы получаем близкие значения к реальной координате \(x_t\)