%==============================================
% ekfvdp.m
% 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

%==============================================
% Initialize important parameters
%==============================================
T = 0.1;                                      % Sampling interval 
N = 300;                                      %

%==============================================
% Initialize System
%==============================================
%sim('vandpol',20);  % generates tout and yout = [x y theta]
y0  = [1 1]';                                 % initial state 
yout(:,1) = y0;                               % initial state condition
tout(:,1) = 1;                                %
v(:,1) = y0;                                  %
nv = 0.1*randn(1,N+1);                        %
for j=2:(N+1),                                %
  yout(:,j) = feval('bob_vdpol',T,yout(:,j-1)); %
  tout(:,j) = j*T;                            %
  v(:,j) = yout(:,j) + nv(:,j);             %
end                                           %
%v = yout(:,1);                               % Extracts information from yout
%v = v + randn(size(tout));                   % Corrupt measurements with noise

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

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

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