% Darren Pais

% s represents the system and comprises all relevant system data,
% dynamically updated.

function [s] = kalman_filter(s)

% set defaults for absent fields:

%s.x is the state vector estimate intialized to IC's for the system
if ~isfield(s,'x'); s.x=nan*z; end

%s.P is the inital covariance matrix, updated continously
if ~isfield(s,'P'); s.P=nan; end

%z is the observation vector  ---> z=Hx + v
if ~isfield(s,'z'); error('Observation vector missing'); end

%u is the dynamic control vector initalized to zero 
if ~isfield(s,'u'); s.u=0; end

%enter time step dt
if ~isfield(s,'dt'); error('No time step entered'); end

%A is the state transition matrix xdot=A*x + B*u + w
if ~isfield(s,'A'); s.A=eye(length(x)); end

%B is the control vector transition matrix initialized to zero
if ~isfield(s,'B'); s.B=0; end

%Q is the state noise covariance matrix (related to w)
if ~isfield(s,'Q'); s.Q=zeros(length(s.x)); end

%R is the measurement noise covariance matrix (related to v)  [Observation
%covariance]
if ~isfield(s,'R'); error('Observation covariance missing'); end

%H is the linear relationship between observation vector and state vector
%and is usually 1
if ~isfield(s,'H'); s.H=eye(length(x)); end

%initialization for state vector, covariance vector P in case IC's unavailable


if isnan(s.P)
   s.P = inv(s.H)*s.R*inv(s.H'); 
   %s.P=-1*s.Q*inv(s.A+(s.A)'-s.H'*inv(s.R)*s.H); 
end
if isnan(s.x)
   s.x = inv(s.H)*s.z;
else


   
% This is the code which implements the discrete Kalman filter
   
   % Prediction for state vector and covariance:
   s.x = s.A*s.x + s.B*s.u;
   
   %determine eigenvalue matrix for computation
   phi=expm( (s.dt)*s.A ) ;
   Pnew = phi * s.P * phi'+s.Q;
   s.P=Pnew ;
   % Compute Kalman gain factor:
   K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);

   % Correction based on observation:
   s.x = s.x + K*(s.z-s.H*s.x);
   s.P = s.P - K*s.H*s.P;
   
   % Note that the desired result, which is an improved estimate
   % of the sytem state vector x and its covariance P. 
   
end

return