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

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

    Пусть \(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}\) (Charles K. Chui, Guan...)

    Идея Фильтра Калмана состоит в том, чтобы получить наилучшее приближение к истинной координате объекта. Для этого нам надо выбрать какое-то значение между \(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})\) будет минимальным (D. Khudaverdyan. Kalm...): \[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 значений координаты:

    Время Измеренная координата Отфильрованая координата Реальная координата
    1 -2.5302 -2.5302 0
    2 17.4349 11.9537 8.7903
    3 48.7136 32.8802 22.7827
    4 8.2697 39.4596 41.0884
    5 56.9524 59.7561 61.2239
    6 136.2727 95.0552 87.6071
    7 89.6219 116.1046 119.4813
    8 178.1935 155.3939 147.9606
    9 131.9830 185.6066 184.9835
    10 143.3234 218.1615 225.6489

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

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

    \label{ris:image} Графики изменения координаты от времени

    References

    1. . Charles K. Chui, Guanrong Chen. Kalman Filtering. (2009).

    2. . D. Khudaverdyan. Kalman filter. (2013).

    <