Simulation

% kf
% Sistema
% x = F_x*x + F_u*u + v
% y = H*x + w

% inicio das variáveis
X = 5;
x = 0;
u = 1;

F_x = 1;
F_u = 1;
H = 1;
P = 0;

V = 50;
W = 1;
v1 = sqrt(V);
w1 = sqrt(W);

% Laço
tempo = 0 : 1 : 100;
E_Real = zeros(1,size(tempo,2));
E_Corrigido = zeros(1,size(tempo,2));
M_Real = zeros(1,size(tempo,2));
Cov = zeros(1,size(tempo,2));

i = 1;
for t = tempo
  % Simulação
  v = v1 * randn;
  X = F_x*X + F_u*u + v;
  w = w1 * randn;
  y = H*X + w;

  % Predição
  x = F_x*x + F_u*u;
  P = F_x*P*F_x' + V;

  %Ganho de Kalman
  e = H*x;
  z = y - e;
  E = H*P*H';
  Z = W + E;
  K = P*H'*Z^-1;

  % Atualização
  x = x + K*z;
  P = P - K*H * H*P;

  % Informação
  E_Real(:,i) = X;
  E_Corrigido(:,i) = x;
  M_Real(:,i) = y;
  Cov(:,i) = diag(P);

  i = i + 1;

end

plot(tempo,E_Real,tempo,E_Corrigido,tempo,M_Real);
grid on;
title('WALL-E');
xlabel('TEMPO');
ylabel('POSIÇÃO');
legend('E\_Real','E\_Corrigido','M\_Real');
print t800.png;