%---------------------------------------------------
% Estima de parametros usando Kalman Filter
%---------------------------------------------------
% Sistema real:  y(n)- 0.9 y(n-1) = 1.0 u(n-1) + e(n) 
% Modelo:        y(n)+ a y(n-1) = b u(n-1) + e(n)      
%---------------------------------------------------
% Generación de datos
%---------------------------------------------------
ee=0.001*randn(1000,1);
ee=ee-mean(ee);
t=[1:1000]';
u=square(2*pi*t/10);
y=dlsim(1,[1 -0.9],u)+dlsim([1 0],[1 -0.9],ee);
%----------------------------------------------------
% Condiciones Iniciales
%----------------------------------------------------
Q=1e-1*eye(2);
R=1;
xm(:,1)=[-0.5;1.5];
x(:,1)=[-0.5;1.5];
Pm=eye(2);
P=Pm;
%----------------------------------------------------
% Filtro de Kalman
%----------------------------------------------------
K(:,1)=Pm*[-y(1) u(1)]'/([-y(1) u(1)]*Pm*[-y(1) u(1)]'+R);
for k=2:1000
   xm(:,k)=x(:,k-1);
   Pm=P+Q;
   K(:,k)=Pm*[-y(k-1) u(k-1)]'/([-y(k-1) u(k-1)]*Pm*[-y(k-1) u(k-1)]'+R);
   x(:,k)=xm(:,k)+K(:,k)*(y(k)-[-y(k-1) u(k-1)]*xm(:,k));
   P=(eye(2)-K(:,k)*[-y(k-1) u(k-1)])*Pm;
end
%-----------------------------------------------------
% Resultados de simulacion
%-----------------------------------------------------
subplot(211)
plot(t,x(1,:))
xlabel('Index k')
ylabel('Estima de "a"')
subplot(212)
plot(t,x(2,:))
xlabel('Index k')
ylabel('Estima de "b"')





   

