%==============================================
% dead.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
%==============================================
dt = 0.03;                                    % Sampling interval 
N = 100;                                       %

%==============================================
% Initialize System
%==============================================
u = [0.01, 0.05];                             % [step, turn]
y0  = [0 0 0]';                               % initial state 
yout(:,1) = y0;                               % initial state condition
truep(:,1) = y0;
tout(:,1) = 0;                                %
z(:,1) = y0;                                  %
pe = randn(2,N+1);                            %
w = 0.01*randn(3,N+1);                        %
pl(:,1) = [0 0]';                             %

for j=2:(N+1),                                %
  yout(:,j) = feval('increm',dt,yout(:,j-1),u,pe(:,j)); %
  truep(:,j) = feval('increm',dt,truep(:,j-1),u,[0,0]); %
  tout(:,j) = j*dt;                           %
  z(:,j) = yout(:,j) + w(:,j);                %
  pl(:,j) = feval('pt',yout(:,j));            %
end                                           %

%==============================================
% Init variables for the Extended Kalman Filter
%==============================================
Pk_prev = ones(3);                            %
x_hat = [1 1 0]';                             %
R = 100*eye(3);                               % measurement noise
Q = eye(3);                                   % process noise
F = eye(3);                                   % function
dg = [1 0 0; 0 1 0; 0 0 1;];                  %
g = zeros(3,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) = 1;                                % Compute dF/dx(xk_1)
   F(1,3) = 1;                                % Compute dF/dx(xk_1)
   F(2,1) = 1;                                % Compute dF/dx(xk_1)
   F(2,2) = 1;                                % Compute dF/dx(xk_1)
   F(2,3) = 1;                                % Compute dF/dx(xk_1)
   F(3,1) = 1;                                % Compute dF/dx(xk_1)
   F(3,2) = 1;                                % Compute dF/dx(xk_1)
   F(3,3) = 1;                                % Compute dF/dx(xk_1)
   %===========================================
   % Compute dg/dx(xk_1)                      %
   %===========================================
   g = [x1 x2 0]';                            % Compute g(xk_1)     
   Pi = F*Pk_prev*F' + Q;                     % Compute P(k,k_1)
   %===========================================
   x1 = x1;                                   % Compute x_hat(k_1)
   x2 = x2;                                   % 
   x_hat = [x1 x2, 0]';                          %
   %===========================================
   Kk = Pi*dg'/(dg*Pi*dg' + R);               % Compute the Kalman gain matrix
   %===========================================  
   Pk_prev = (eye(3) - Kk*dg)*Pi;             % Update the error covariance matrix   
   %===========================================  
   x_hat = x_hat + Kk*(z(k) - g);                %
   %===========================================  
   xhistory(k+1,:)=x_hat';                    % Register for the history
end;                                          %


%==============================================
% Plot results
%==============================================
figure(1);                                    %
plot(tout,yout(1,:)-z(1,:),'g'), hold on      % 
plot(tout,yout(2,:)-z(2,:),'b');              %

figure(2);                                    %
clf; 
axis([-2,2,-2,2]);
plot(yout(1,:),yout(2,:),'r'), hold on        % plot output
plot(yout(1,:),yout(2,:),'r.');               %

plot(z(1,:),z(2,:),'b')                       % plot output + error
plot(z(1,:),z(2,:),'b+')                      %
   
plot(truep(1,:),truep(2,:),'g')               %
plot(truep(1,:),truep(2,:),'g.')              %
   
