%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% This code implements a kalman filter that estimates the R and C parameters of a Windkessel circulatory system model.
% The model is just a parallel R and C circuit. The voltage across the R and C elements is the arterial pressure Pa, while the 
% current Q entering the parallel network network is the cardiac output.
%
% From the differential equation relating Q to Pa, I derived a difference equation.  The difference equation is a first order 
% autoregressive model of the following form: y(n) = a*y(n-1) + b*q(n).  The parameters a and b are related to R,C,Ts (Ts is
% the sampling time).
%
% When you run this file three plots will be shown:  
%
% Figure 1: is a plot of y(n) and yhat(n). y(n) is constructed form the actual values of parameters a and b. yhat(n) is 
% a constructed form the estimates of a and b prodcued by the kalman filter.
%  
% Figure 2: is a plot of the actual values of a and the estimated values of a.
%
% Figure 3: is a plot of the actual values of b and the estimated values of b.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
close all

% Ts is the Sampling interval, and N is the total number of samples
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ts = 30e-3;
N  = 1000;

% Here I set the dynamic course that the parameters a and b will follow.
% To change the course alter the coeffecients of the terms "ones(1,N/4)"
% keep values of a and b below 1.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a = [ 0.9*ones(1,N/4) 0.3*ones(1,N/4) 0.7*ones(1,N/4) 0.9*ones(1,N/4) ];
b = [ 0.5*ones(1,N/4) 0.3*ones(1,N/4) 0.1*ones(1,N/4) 0.8*ones(1,N/4) ];
 
% x(n) is the periodic cardiac output driving the system.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q = sin( (2*pi/0.86)*[0:N-1]*Ts );

% Generate the output y(n)
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:N
    
    if ( i == 1 )
        
        y(i) = 0;
        
    else
    
        y(i) = a(i)*y(i-1) + b(i)*q(i);
      
    end
        
end


%The kalman filter observes x(n) and y(n) and will generate dynamic estimates of the parameters a and b.

%The kalman filter takes the following form:

% x(n+1) = x(n) + noise_state (where x is a 2x1 vector of the parameters a and b)
% y = C*x + noise_observe (where C is a 1x2 vector [y(i-1) x(i)] that implements the autoregressive equation)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



% Here you can add some noise to the observations y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
noisePower = 0.03;
y = y + noisePower*randn(size(y));


% Initialize the state estimate and covariance matrices.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     x_predict(:,1)  = [0 ; 0];                     %State vector initialized to zero;
     lambda_state_prediction_error = [20 0; 0 20];  %Covariance Matrix of state prediction initialized as diagonal matrix with large trace.
     lambda_state_update_noise   = [2 0; 0 2];      %Covariance Matrix of noise corrupting state.
     lambda_observation_noise  = 5;                 %Noise corrupting observations

 for i = 1:N    
        
      if ( i == 1 )
     
          C = [ 0 0 ];
          
      else
          
          C = [ y(i-1) q(i) ];
          
      end
      
  %Correct prediction given observation y(n) 
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
      % Kalman Gain
      %%%%%%%%%%%%%
      G = lambda_state_prediction_error*C'*inv(C*lambda_state_prediction_error*C' + lambda_observation_noise);
      
      % Correct State prediction
      %%%%%%%%%%%%%%%%%%%%%%%%%%
      x_now(:,i) = x_predict(:,i) + G*(y(i) - C*x_predict(:,i));
      
      % Correct Variance in error of state prediction
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      lambda_state_prediction_error = lambda_state_prediction_error - G*C*lambda_state_prediction_error;
      

  %Make the next prediction
  %%%%%%%%%%%%%%%%%%%%%%%%%%%
       x_predict(:,i+1) = x_now(:,i);
       lambda_state_prediction_error = lambda_state_prediction_error + lambda_state_update_noise;
       
       
 end
 
 
 % This uses estimates of the course of the parameters a and b to compute a yhat.  The variable x_now 
 % produced by the kalman filter has estimates at each time-point of the parameters a and b.
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 for i = 1:N
     
    if ( i == 1 )
        
        y_hat(i) = 0;
        
    else
   
        y_hat(i) = x_now(1,i)*y_hat(i-1) + x_now(2,i)*q(i-1);
    
    end
        
end
 
 % Plot y and yhat
 %%%%%%%%%%%%%%%%%
 figure(1)
 plot( (1:N)*Ts, y ,'b',(1:N)*Ts, y_hat,'r')
 title('Arterial Pressure');
 xlabel('Time');
 ylabel('Magnitude');
  
 % Plot a and ahat
 %%%%%%%%%%%%%%%%%
 figure(2)
 plot( (1:N)*Ts,  x_now(1,:),'r-' )
 hold on
 plot( (1:N)*Ts, a, 'b' );
 title('Estimate of parameter a');
 xlabel('Time');
 ylabel('Magnitude');
 legend('Kalman Estimate','True Value')
 axis([Ts, N*Ts, -2, 2])

 % Plot b and bhat
 %%%%%%%%%%%%%%%%%
 figure(3)
 plot( (1:N)*Ts , x_now(2,:) ,'r-' )
 hold on
 plot( (1:N)*Ts , b , 'b' );
  title('Estimate of parameter b');
 xlabel('Time');
 ylabel('Magnitude');
 legend('Kalman Estimate', 'True Value');
 axis([Ts, N*Ts, -2, 2])
  
   
