%Simulação Filtro de Kalman para SLVTs a Tempo Discreto

% SLVT de 2a. ordem, forma acoplada, MIMO: 2 entradas e 2 saídas 

clear all; close all; clc

N=1000;  % número de amostras de avaliação temporal
k=0:N; k=k(:);  % base de tempo

%Elementos da Matriz A(k)
a1=0.7*cos(pi.*k./1000);
a2=0.7*sin(pi.*k./1000);
%A=[a1  -a2; -a2 a1];

B=eye(2); % B é matriz identidade de ordem 2

% Elementos da matriz C(k)
c1=1-0.988.^k; c2=1-c1;

% C=[1 c2;c1 1];

% Entradas
u1=2*cos(pi*k./50); % entrada u1
u2=0.5+k.*(0.97).^k; % entrada u2


% Implementação do SLVT 

y=zeros(2,N);  % saída do SLVT
x=zeros(2,N);  % matriz para armazenar os estados do SLVT (ideal)
xh=zeros(2,N); % matriz para armazenar os estados estimados pelo FK
xhi=zeros(2,N); % matriz para armazenar os estados intermediários
x(:,1)=[-19; 40]; % estado inicial do SLVT (arbitrário)
xh(:,1)=[10; -10]; % estado inicial ótimo do FK \hat{x}_1(1);

% Hipótese: os 2 elementos de \hat{x}_1(1) têm variância 2 e 3, com as
% médias indicadas acima
P=[2 0; 0 3];  % P_1(1)

% w(k) estacionário 
vw=15; % mesma variância para as 2 componentes
w=sqrt(vw)*randn(2,N);
Q=[vw 0;0 vw]; % para todo k

% v(k) estacionário
vv1=4; vv2=1; % variâncias das componentes
v1=sqrt(vv1)*randn(1,N);
v2=sqrt(vv2)*randn(1,N);
v=[v1;v2]; clear v1; clear v2;
R=[vv1 0; 0 vv2];

y(:,1)=[1 c2(1); c1(1) 1]*x(:,1)+v(:,1); % calcula a saída do SLVT em k=1

for k=2:N,   
    A=[a1(k-1) -a2(k-1); -a2(k-1) a1(k-1)]; % A(k-1)
    u=[u1(k-1);u2(k-1)];
    x(:,k)=A*x(:,k-1)+B*u; % atualiza estado do SLVT (ideal, sem ruído)
    % FK - Predição/Propagação
    xhi(:,k)=A*xh(:,k-1)+B*u; % atualiza estimativa intermediaria do estado
    P_i=A*P*A'+Q;  % autocov do erro da estimativa intermediária
    % FK - Assimilação
    C=[1 c2(k); c1(k) 1];  % matriz C(k)
    y(:,k)=C*x(:,k)+v(:,k); % "mede" a observação ruidosa do SLVT 
    G=P_i*C'*inv(R+C*P_i*C');  % Matriz G(k) de Ganhos de Kalman 
    xh(:,k)=xhi(:,k)+G*(y(:,k)-C*xhi(:,k)); % estimativa fina do estado
    P=(eye(2)-G*C)*P_i;  % atuliza P_k(k) 
end
    
figure
set(gcf,'position',[7  95  1428   901])
set(gca,'fontsize',16)

subplot(211)    
plot(0:N-1,x(1,:),0:N-1,xh(1,:),'linewidth',3);
ylabel('${x}_1(k)$','interpreter','latex','fontsize',18)
hlegen=legend('${x}_1(k)$','$\hat{x}_1(k)$');
set(hlegen,'interpreter','latex','fontsize',20)


subplot(212)    
plot(0:N-1,x(2,:),0:N-1,xh(2,:),'linewidth',3);
ylabel('${x}_2(k)$','interpreter','latex','fontsize',18)
hlegen=legend('${x}_2(k)$','$\hat{x}_2(k)$');
set(hlegen,'interpreter','latex','fontsize',20)







