function [state_vec,t_vec,h] = EKF2(model,param)

x = param.x0'; % Initial condition 
P = param.P0;  % Initial error covariance
t_meas = param.t_meas;
R = param.R;
Q = param.Q;
C = param.C;
y_meas = param.y_meas;

kBC=3;
kCB=2;
Vdot=1;
VR=10;
ca0 = 1;

h = t_meas(2) - t_meas(1);

j = 1;

% to save estimates
state_vec = zeros(numel(x),length(t_meas));
t_vec = zeros(length(t_meas),1);
t = 0;

%keyboard

while t(end) <= t_meas(end)

 % Measurement update / Correction
K = P*C'*inv(C*P*C'+R);
x = x + K*(y_meas(j,:)'-C*x);             % x[k|k]
P = (eye(4)-K*C)*P;                       % P[k|k]  
 
state_vec(:,j) = x;
t_vec(j) = t(end);
j = j+1;
 
tspan = [t(end),t(end)+h];

% Compute A at time k|k
% A = [-Vdot/VR-x(4) , 0 , 0 , -x(1); x(4) , -Vdot/VR-2*kBC*x(2) , kCB , x(1);...
%       0 , 2*kBC*x(2) , -Vdot/VR-kCB , 0; 0 , 0 , 0 , 0];
% % Discretize A
% Ad = expm(A*h);
  
 % Time update / Prediction
[t,x] = feval(@ode45,model,tspan,x);
A = [-Vdot/VR-x(4) , 0 , 0 , -x(1); x(4) , -Vdot/VR-2*kBC*x(2) , kCB , x(1);...
      0 , 2*kBC*x(2) , -Vdot/VR-kCB , 0; 0 , 0 , 0 , 0];
% Discretize A
Ad = expm(A*h);
P = Ad*P*Ad' + Q;

x = x(end,1:4)';

%keyboard

end






