% 15.7.02
% Tracking input parameter from first component of FitzHugh-Nagumo-Dynamics
% print -dpsc ex5.ps    
% save res/ex5.mat

global dT

orient tall

% Dimensions: (dq for param. vector, dx augmented state, dy observation)
dq=1; dx=dq+2; dy=1; 

fct='ex5fitzhughfct'; % this is the model function F(x) used in filtering
obsfct='ex5obsfct';   % this is the observation function G(x)

ll=800; % number of data samples
dT=0.2; % sampling time step (global variable)

% Simulating data: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x0=zeros(2,ll); x0(:,1)=[0; 0]; % true trajectory
dt=.1*dT; nn=fix(dT/dt); % the integration time step is smaller than dT

% External input, estimated as parameter p later on:
z=[1:ll]/250*2*pi; z=-.4-1.01*abs(sin(z/2));

% RuKu integrator of 4th order:

for n=1:ll-1;
  xx=x0(:,n);
  for i=1:nn
    k1=dt*ex5fitzhughint(xx,z(n));
    k2=dt*ex5fitzhughint(xx+k1/2,z(n));
    k3=dt*ex5fitzhughint(xx+k2/2,z(n));
    k4=dt*ex5fitzhughint(xx+k3,z(n));
    xx=xx+k1/6+k2/3+k3/3+k4/6;
  end;
  x0(:,n+1)=xx;
end;

x=[z; x0]; % augmented state vector (notation a bit different to paper)

R=.2^2*var(ex5obsfct(x))*eye(dy,dy); % observation noise covariance matrix
randn('state',0); y=feval(obsfct,x)+sqrtm(R)*randn(dy,ll); % noisy data

% Initial conditions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xhat=zeros(dx,ll); 
xhat(:,2)=x(:,2); % first guess of x_1 set to observation 

Q=.015; % process noise covariance matrix

Pxx=zeros(dx,dx,ll); 
Pxx(:,:,1)=blkdiag(Q,R,R);

errors=zeros(dx,ll); % not so important
Ks=zeros(dx,dy,ll);  % Kalman gains

% Main loop for recursive estimation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=2:ll 

[xhat(:,k),Pxx(:,:,k),Ks(:,:,k)]=ex5ut(xhat(:,k-1),Pxx(:,:,k-1),y(:,k),fct,obsfct,dq,dx,dy,R);

Pxx(1,1,k)=Q;

errors(:,k)=sqrt(diag(Pxx(:,:,k)));

end; % k

% Results %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

chisq=mean((x(1,:)-xhat(1,:)).^2+(x(2,:)-xhat(2,:)).^2+(x(3,:)-xhat(3,:)).^2)
est=xhat(1:dq,ll)'
error=errors(1:dq,ll)'
%meanest=mean(xhat(1:dq,:)')
%meanerror=mean(errors(1:dq,:)')

figure(1)

subplot(2,1,1)
plot(y,'bd','MarkerEdgeColor','blue', 'MarkerFaceColor','blue','MarkerSize',3);
hold on; 
plot(x(dq+1,:),'black','LineWidth',2);
%plot(xhat(dq+1,:),'r','LineWidth',2); 
xlabel(texlabel('t'));
ylabel(texlabel('x_1, y'));
hold off;
axis tight
title('(a)')
drawnow

subplot(2,1,2)
plot(x(dq+2,:),'black','LineWidth',2);
hold on
plot(xhat(dq+2,:),'r','LineWidth',2); 
plot(x(1,:),'black','LineWidth',2); 
for i=1:dq; plot(xhat(i,:),'m','LineWidth',2); end;
for i=1:dq; plot(xhat(i,:)+errors(i,:),'m'); end;
for i=1:dq; plot(xhat(i,:)-errors(i,:),'m'); end;
xlabel(texlabel('t'));
ylabel(texlabel('z, estimated z, x_2, estimated x_2'));
hold off
axis tight
title('(b)')


