// P44FilKF.sce
// Estimation of state variable with KF  
// --------------------------------------------                       
[u,t,n]=file();                         // find working directory
chdir(dirname(n(2)));                   // set working directory
clear("u","t","n")                      // clear auxiliary data
exec("ScIntro.sce",-1),mode(0)          // intro to sesion                 

nd=100;                  // number of data

// State-space model
Fil.Cy.rw=.1*eye(2);     // state covariance
Fil.Cy.rv=.01;           // output covariance
Fil.Cy.rx=1000*eye(2);   // state estimate covariance
Fil.Cy.M=[.2 .1;         // state-space
          .2 .4];        //   model
Fil.Cy.N=[1; -1];        //   parameters
Fil.Cy.F=[1; -1];        //      "
Fil.Cy.A=[1 0];          //      "
Fil.Cy.B=1;              //      "
Fil.Cy.G=2;              //   parameters

// declarations
xt=zeros(2,nd);     // state declaration (initial state)
x=zeros(2,nd);      // initial state estimate
ut=zeros(1,nd);     // input
yt=zeros(1,nd);     // output
yp=zeros(1,nd);     // output prediction
rw=Fil.Cy.rw; rv=Fil.Cy.rv; rx=Fil.Cy.rx;    // KF covariances
M=Fil.Cy.M; N=Fil.Cy.N; F=Fil.Cy.F;          // state model parameters
A=Fil.Cy.A; B=Fil.Cy.B; G=Fil.Cy.G;          // output model parameters

// TIME LOOP
for t=2:nd
  // simulation
  xt(:,t) = M*xt(:,t-1)+N*ut(t)+F+rw*rand(2,1,'norm');
  yt(t)   = A*xt(:,t) + B*ut(t)+G+rv*rand(1,1,'norm');
  // estimation
  [x(:,t),rx,yp(t)]=...
    Kalman(x(:,t-1),yt(t),ut(t),M,N,F,A,B,G,rw,rv,rx);
end
Fil.Cy.ut=ut;
Fil.Cy.yt=yt;
Fil.Cy.xt=x;

// RESULTS
s=1:nd;
subplot(311),plot(s,xt(1,s),s,x(1,s))
set(gca(),"data_bounds",[1,nd,0 2]);
title('First state and its estimate')
subplot(312),plot(s,xt(2,s),s,x(2,s))
set(gca(),"data_bounds",[1,nd,-2 1]);
title('Second state and its estimate')
subplot(313),plot(s,yp(s),s,yt(s))
set(gca(),"data_bounds",[1,nd,2 4]);
title('Output and its prediction')
set(gcf(),"position",[500 50 600 500])
// Note: xf is the result of state estimation (not x)
//  at time t, xf(t) is the result. x(t+1) aims at the next time

save _data/dataT44.dat Fil
