Симуляция работы фильтра Калмана Петров Евгений (21512)

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

Предположим, что входной сигнал подчиняется следующему уравнению: \[\begin{gathered} \\ x_{t} = \phi \cdot x_{t-1} + u \cdot t + \upsilon_{t} = 0,26 \cdot x_{t-1} + 0,8 \cdot t + \upsilon_{t} , \\ \end{gathered}\] где \(\upsilon_{t} \sim N(0,1)\) произвольная случайная величина. Уравнение наблюдение будет выглядеть следующим образом: \[\begin{gathered} \\ y_{t} = \gamma \cdot x_{t} + \epsilon_{t} = 0,72 \cdot x_{t} + \epsilon_{t} , \\ \end{gathered}\] где \(\epsilon_{t} \sim N(0,1)\) также случайная величина. Более того предполагается, что ошибки наблюдения и сигнала не коррелируемы ( т.е \(E(\epsilon_{t-i} \upsilon_{t-j} , \forall i,j)\) ). По начальной оценке сигнала и дисперсии предсказания, мы можем просимулировать работу данной модели и посчитать результат фильтрации. Сначала необходимо посчитать усиление Калмана и дисперсию предсказания и оценки . Усиление Калмана представляет собой следующее выражение: \[\begin{gathered} \\ K_{N} = \frac{\gamma \cdot S_{N}}{\gamma^2 \cdot S_{N} + \sigma^2_{\epsilon}} \\ \end{gathered}\] где \(S_{N} \) дисперсия ошибки в предсказании \(x_{N}\) в момент \(N-1\), и определяется следующим выражением: \[\begin{gathered} \\ S_{N} = \phi^2 \cdot p^e_{N-1} + \sigma^2_{\upsilon} \\ \end{gathered}\] Дисперсии оценки \(p^e_{N}\) и дисперсия предсказания \(p^p_{N+1}\) равняются: \[\begin{gathered} \\ p^e_{N} = (1 - \gamma \cdot K_{N}) \cdot S_{N} = (1 - 0,72 \cdot K_{N}) \cdot S_{N} \\\\ p^p_{N+1} = p^e_{N} \cdot \phi^2 + \sigma^2_{\upsilon} \\ \end{gathered}\] В начальный момент времени ( \(t = 1 \) ) дисперсия предсказания равна дисперсии сигнала \(p^p_{1} = 0.2 \), \(S_{1} = 1\). Тогда: \[\begin{gathered} \\ \sigma^2_{\upsilon} = 5 \qquad \sigma^2_{\epsilon} = 0,2 \\\\ K_{1} = \frac{0,72 \cdot 1}{0,72^2 \cdot 1 + 0,2} = 1,0022 \\\\ p^e_{1} = (1 - 0,72 \cdot 1,0022) \cdot 1 = 0,2783 \\\\ p^p_{2} = 0,2783 \cdot 0,26^2 + 5 = 5,01881 \\ \end{gathered}\] В момент времени \(t=2\): \[\begin{gathered} \\ K_{2} = \frac{0,72 \cdot 5,01881}{0,72^2 \cdot 5,01881 + 0,2} = 1,2897 \\\\ p^e_{2} = (1 - 0,72 \cdot 1,2897) \cdot 5,01881 = 0,3582 \\\\ p^p_{3} = 0,3582 \cdot 0,26^2 + 5 = 5,0242 \\ \end{gathered}\] В момент времени \(t=3\): \[\begin{gathered} \\ K_{3} = \frac{0,72 \cdot 5,0242}{0,72^2 \cdot 5,0242 + 0,2} = 1,2898 \\\\ p^e_{3} = (1 - 0,72 \cdot 1,2898) \cdot 5,0242 = 0,3582 \\\\ p^p_{4} = 0,3582 \cdot 0,26^2 + 5 = 5,0242 \\ \end{gathered}\] После нескольких шагов, Усиление Калмана, Дисперсия Предсказания и Дисперсия Оценки сводиться к следующим значениям: \[\begin{gathered} \\ \bar k_{mean} = 0,7913 \\ \bar p^p_{mean} = 1,747 \\ \bar p^e_{mean} = 0,2198 \\ \end{gathered}\]

Предполагаем,что начальная оценка сигнала и предсказанное значение равны нулю. В момент времени \( t = 1\) , после получения первного наблюдения \(y_{1} = -0,3\) первоначальныя оценка сигнала обновляется следующим образом: \[\begin{gathered} \\ \hat{x}^e_{1} = 0 + k_{1} \cdot (y_{1} - 0 ) = 1,0022 \cdot (-0,3 - 0) = -0,30066 \\ \end{gathered}\] На следующем шаге \[\begin{gathered} \\ \hat{x}^p_{2} = 1 + \phi \cdot \hat{x}^e_{1} = 1 + 0,26 \cdot -0,30066 = 0,9218 \\ \end{gathered}\] Предсказанное значение наблюдения \[\begin{gathered} \\ \hat{y}^p_{2} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 0,9218 = 0,6637 \\ \end{gathered}\] В момент времени \(t = 2\), первоначальной оценкой сигнала является предсказание выполненное во время \(t = 1\). Данная оценка обновляется после наблюдения \(y_{2} = 2,127\): \[\begin{gathered} \\ \hat{x}^e_{2} = \hat{x}^p_{2} + k_{2} \cdot (y_{2} - \hat{y}^p_{2} ) = 0,9218 + 1,2897 \cdot (-2,127 - 0,6637) = 2,8090 \\ \end{gathered}\] И \[\begin{gathered} \\ \hat{x}^p_{3} = 1 + \phi \cdot \hat{x}^e_{2} = 1 + 0,26 \cdot 2,8090 = 1,7303 \\\\ \hat{y}^p_{3} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 1,7303 = 1,2458 \\ \end{gathered}\]

Листинг программы на matlab

% x(t) = u*t + a*x(t-1) + v_t 
% y(t) = b*x(t-1) + e_t
%
% количество шагов
Quantity = 20;
% инициализируем массивы
k = 1:Quantity;
% координата
x = k;
% предсказанна¤ координата
x_predic = k;
% оценочна¤ координата
x_estim = k;
% наблюдаема¤ координата
y = k;
% предсказанное наблюдение
y_predic = k;
% дисперси¤ ошибки в предсказании X в
Sn = k;
% усиление калмана
KalmanGain = k;
% оценка дисперсии
EstimVar = k;
% предсказанна¤ дисперси¤
PredicVar = k;

%константы
% коэффииент координаты
a = 0.26;
% коэффициент наблюдени¤
b = 0.72;
% коэффициент при упрал¤ющей функции
ControlF = 0.8;
% дисперси¤ ошибки модели
disp_ErrorModel = 0.2;
% дисперис¤ ошибки сенсора
disp_ErrorSensor = 5;
% ошибка модели
ErrorModel = normrnd(0,disp_ErrorModel); 
% ошибка сенсора
ErrorSensor = normcdf(0,disp_ErrorSensor); 


% заполн¤ем координаты
x(1) = 0;
y(1) = b*x(1) +  ErrorSensor;
for time = 1 : (Quantity-1)
    ErrorModel = normrnd(0,disp_ErrorModel);
    ErrorSensor = normrnd(0,disp_ErrorSensor);
    x(time+1) = a*x(time) +  ControlF*time + ErrorModel;
    y(time+1) = b*x(time+1) + ErrorSensor;
end;

% в момент t = 1;
Sn(1) = 1;
PredicVar(1) = 1;
x_predic(1) = 0;
y_predic(1) = 0;
% симул¤ци¤ фильтра
for time = 1 : (Quantity-1)
    disp(time);
    KalmanGain(time) = (b*Sn(time))/((b*b*Sn(time)) + disp_ErrorModel);
    disp(KalmanGain(time));
    EstimVar(time) = (1 - b*KalmanGain(time))*Sn(time);
    disp(EstimVar(time));
    PredicVar(time+1) = EstimVar(time) * a * a + disp_ErrorSensor;
    disp(PredicVar(time));
    Sn(time+1) = a*a*EstimVar(time) + disp_ErrorSensor;    
end;
% заполн¤ем координаты
for time = 1 : (Quantity-1)
    disp(time);
    x_estim(time) = x_predic(time) + KalmanGain(time)*(y(time)-y_predic(time));
    disp( x_estim(time));
    x_predic(time+1) = (ControlF*time) + a*x_estim(time);
    disp( x_predic(time));
    y_predic(time+1) = b*x_predic(time+1);
end;

set(0,'DefaultAxesFontSize',14,'DefaultAxesFontName','Times New Roman');
set(0,'DefaultTextFontSize',14,'DefaultTextFontName','Times New Roman'); 
figure('Units', 'normalized', 'OuterPosition', [0 0 1 1]);
title('ќдномерный фильтр  алмана');
hold on;
plot(k,x,'r','Color',[.7 .1 .1],'LineWidth',2);
plot(k,y,'--','Color',[.1 .7 .1],'LineWidth',2);
plot(k,x_predic,'r','Color',[.1 .1 .7],'LineWidth',2);
plot(k,x_estim,':b','Color',[.7 .1 .7],'LineWidth',2);
legend('координа