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


x = param.x0'; % Initial condition 
P = param.P0;  % Initial error covariance

t_est = 0;

integration_step = param.delta_t;
h = integration_step;
integrat = 10^(-log10(integration_step));

j = 1;

% to save estimates
state_vec = zeros(length(t_meas),numel(x));
yest = zeros(numel(t_meas),size(y_meas,2));
t_vec = zeros(length(t_meas),1);

while t_est <= 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]  
 
 % Save the state values at t = t_meas
 if round(t_est*integrat)/integrat == round(t_meas(j)*100)/100
    yest(j,:) = C*x;
    state_vec(j,:) = x;
    t_vec(j) = t_est;
    j = j+1;
 end
  
 % Time update / Prediction
[A,x] = feval(model,x,h); % Integration step
P = A*P*A' + Q;  % P[k+1|k] 

t_est = round((t_est + h)*integrat)/integrat;

  
end



