// P44FilPar.sce
// State estimation (model with unknown parameter)
[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;                       // length of data
a=.6;                         // unknown model parameter
sM=[a .5                      // simulation parametrs
   .1 .8];
sN=[.5; .4];
sA=[1 0];
rv=.01; rw=.01*eye(2,2);      // noise variances

ut=rand(1,nd,'norm');
yt=zeros(1,nd);
xx=zeros(2,1);                 // initial state
xt=xx;

// SIMULATION
for t=1:nd
  xx     =sM*xx+sN*ut(:,t)+sqrt(rw)*rand(2,1,'norm');
  yt(:,t)=sA*xx+sqrt(rv)*rand(1,1,'norm');
  xt(:,t)=xx;
end
xt(3,:)=a;                    // true parameter

Rv=.01; Rw=.01;               // model covariance matrices
Rx=1e6*eye(3,3);              // covariance matrix of state estimate
N=[.5; .4; 0];                // parameters of model extended by unknown
A=[1 0 0];                    //     parmeter
G=0;
B=0;
x=zeros(3,1);
xe=zeros(3,nd);

// ESTIMATION
for t=1:nd
  M=[x(3) .5 0                // parameters of thr linearized model
     .1   .8 0
     0     0 1];
  dM=[x(3) .5 x(1)
      .1   .8 0
      0     0 1];
  F=(M-dM)*x;
  
  // KALMAN FILTER
  [x,Rx,yp]=Kalman(x,yt(t),ut(:,t),dM,N,F,A,B,G,Rw,Rv,Rx);
  xe(:,t)=x;
end

// RESULTS
set(scf(1),'position',[100 50 1200 400])
// figure 1
subplot(131)
plot(xt(1,:),'b','linewidth',2)
plot(xe(1,:),'r','linewidth',2)
legend('state','estimate');
title('First state and its estimate','fontsize',5,'FontName','Times')
xlabel('time','fontsize',4,'FontName','Times')
ylabel('values','fontsize',4,'FontName','Times')

// figure 2
subplot(132)
plot(xt(2,:),'b','linewidth',2)
plot(xe(2,:),'r','linewidth',2)
legend('state','estimate');
title('Second state and its estimate','fontsize',5,'FontName','Times')
xlabel('time','fontsize',4,'FontName','Times')
ylabel('values','fontsize',4,'FontName','Times')

// figure 3
subplot(133)
plot(xt(3,:),'b','linewidth',2)
plot(xe(3,:),'r','linewidth',2)
legend('true parameter','estimate');
title('Estimate of the parameter','fontsize',5,'FontName','Times')
xlabel('time','fontsize',4,'FontName','Times')
ylabel('values','fontsize',4,'FontName','Times')
