%---------------------------------------------------
% Estima de una constante usando Kalman Filter
%---------------------------------------------------
% xtrue: verdadero valor
% z = xtrue + v      mediciones ruidosas
% v: ruido de medicion - covarianza R
% w: ruido de proceso  - covarianza Q
%---------------------------------------------------
% Generación de datos
%---------------------------------------------------
w=0.32*randn(100,1);
randn('state',25687)  
v=0.01*randn(100,1);
xtrue=-0.37727;
z=xtrue+v;
%----------------------------------------------------
% Condiciones Iniciales
%----------------------------------------------------
Q=1e-5;
R=0.01;
Pm_0=1;
xm_0=0;
%----------------------------------------------------
% Filtro de Kalman
%----------------------------------------------------
xm(1)=xm_0;
Pm(1)=Pm_0;
P(1)=Pm_0;
x(1)=xm_0;
K(1)=Pm(1)/(Pm(1)+R);
for k=2:100
   xm(k)=x(k-1);
   Pm(k)=P(k-1)+Q;
   K(k)=Pm(k)/(Pm(k)+R);
   x(k)=xm(k)+K(k)*(z(k)-xm(k));
   P(k)=(1-K(k))*Pm(k);
end
%-----------------------------------------------------
% Resultados de simulacion
%-----------------------------------------------------
plot([1:100]',z,'b+',[1:100]',xtrue*ones(100,1),'r-',[1:100]',x','k')
xlabel('Index k')



   

