% This program simulates an Extended Kalman Filter estimation
% the states of the Van der Pol system.

% (c) 1999 Luis E. Navarro-Serment

clear, clf		% Cleans everything

yout = [ [1, 1.1, 1+1]
         [2, 2.1, 2+1] 
         [3, 3.1, 3+1] 
         [4, 4.1, 4+1] 
         [5, 5.1, 5+1] 
         [6, 6.1, 6+1] 
         [7, 7.1, 7+1] 
         [8, 8.1, 8+1] ];
      
      
tout = [1,2,3,4,5,6,7,8]'

%sim('vandpol',20);	% generates tout and yout = [x y theta]

% Extracts information from yout
v = yout(:,1); 

% Corrupt measurements with noise
v = v + 0.1*randn(size(tout));

% Initialize important parameters
T = 0.1;			% Sampling interval used in the simulation

% Initialize variables for the Extended Kalman Filter
Pk_prev = ones(2);
x_hat = [1 1]';
R = 100*eye(2);
Q = eye(2); 
F = eye(2);
F(2,1) = T;
dg = [1 0; 0 0];
g = zeros(2,1);

xhistory = x_hat';

% Init EKF process

for k=1:length(tout),
   
   x1 = x_hat(1);	x2 = x_hat(2);
   
   % Compute dF/dx(xk_1)
	F(1,2) = 1;
	F(2,2) = 1;
%	F(1,2) = -T*(1 + 2*x1*x2);        % -t(1 + 2xy)
%	F(2,2) = 1 + T - T*x1^2;          % 1 + t - tx^2
   
   % Compute dg/dx(xk_1)
   
   % Compute g(xk_1)
   g = [x1 0]';
   
   
	% Compute P(k,k_1)
	Pi = F*Pk_prev*F' + Q;
   
   % Compute x_hat(k_1)
   x1 = x1 + T*x2;                       % x + yt
   x2 = x2 + x2*(1-x1^2)*T - x1*T;       % y + (1-x^2)yt - xt
   
   x_hat = [x1 x2]';
   
   % Compute the Kalman gain matrix
   Kk = Pi*dg'/(dg*Pi*dg' + R);
   
   % Update the error covariance matrix
   Pk_prev = (eye(2) - Kk*dg)*Pi;
   
      % Update estimate
   x_hat = x_hat + Kk*([v(k) 0]'-g);

   % Register for the history
   xhistory(k+1,:)=x_hat';
end;

% Plot results
plot(tout,yout), hold on
plot(tout,xhistory(1:length(xhistory)-1,:),'r')
   
   
