// P44FilNL.sce
// State estimation (nonlinear model)
// -----------------------------------------
[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=200;                                  // length of data

ut=.1*(1-rand(1,nd,'norm'));            // control
x1=0; x2=-1;                            // initial state

xs=zeros(2,nd); xs(:,1)=[x1;x2];
yt=zeros(1,nd);

// SIMULATION
for t=2:nd
  x1=x1*x2+.1*x2-ut(t)+.01*rand(1,1,'norm');  // state
  x2=.5*x1+.8*x2+ut(t)+.01*rand(1,1,'norm');  //     equations
  yt(t)=x2;                             // output equation
  xs(:,t)=[x1;x2];                    // store the actual sate variable
end

xx=[0;0];                               // initial state estimate
rw=.1*eye(2,2); rv=.1; rx=1e3*eye(2,2); // model and initial covariances
xt=zeros(2,nd); rr=zeros(2,nd); rr(:,1)=diag(rx);
// ESTIMATION
for t=2:nd
  // parameters of the linearized model
  M=[xx(2) xx(1)+.1; .5 .8];
  N=[-1;1];
  F=[-.1*xx(1)+(.1-xx(2))*xx(2);0];
  A=[0 1];
  B=0; 
  G=0;
  // Kalman filter
  [xx,rx,yp]=Kalman(xx,yt(t),ut(t),M,N,F,A,B,G,rw,rv,rx);
  xt(:,t)=xx;                           // stor actual state estimate
  rr(:,t)=diag(rx);                     // stor variance of noise estimate 
end

// RESULTS
// figure 1
plot(xs')
plot(xt',':')
legend('state 1','state 2','estimate 1','estimate 2',4);
title('State and its estimate','fontsize',5,'FontName','Times')
xlabel('time','fontsize',4,'FontName','Times')
ylabel('values','fontsize',4,'FontName','Times')
set(gcf(),'position',[300 100 500 400])
