%%
%                            Example11_23.m
%
%
% Employ synthetic data and construct sampling distributions for the parameters
% C and K as illustrated in Example 11.22.  This code also constructs
% the density from 10,000 simulations, which is compared with the sampling
% distribution.  Because this requires repeated optimization, the code
% takes approximately 15 seconds to run.
%
% Required functions: spring_function_2params.m
%                     ksdensity.m
%

  clear all
%  close all

%%
% Specify true parameter and measurement variance values.
%

  K = 22.1;
  C = 1.3;
  sigma = .1;
  var = sigma^2;
  p = 2;
  
%%
% Contruct the time vector t and load synethetic spring data generated in Example 6.5
%

  factor = sqrt(K - C^2/4);
  denom = sqrt(4*K - C^2);
  
  t = 0:.01:5;
  error = sigma*randn(size(t));
  n = length(t);

  f = 2*exp(-C*t/2).*cos(factor*t);
  f_true = f;
  dfdc = exp(-C*t/2).*((C*t/denom).*sin(factor*t) - t.*cos(factor*t));
  dfdk = (-2*t/denom).*exp(-C*t/2).*sin(factor*t);
  S = [dfdc' dfdk'];
  V = inv(S'*S)*var;
  %V(1,1)
  %V(2,2)
  obs = f + error;
  obs(1) = 2;                                 % Enforce the initial condition
  
  load spring_data.txt
  obs = spring_data(:,2)'; 
  %spring_data = [t' obs'];
  %save('spring_data.txt','spring_data','-ascii')


%%
% Construct asympototic distribution and determine the optimal values of  C and K using
% the routine fminsearch.m for this data.  We additionally estimate the sensitivity
% matrix S, covariance matrix V, and compute the confidence intervals for C and K.
%

  C_interval = [1.2:.001:1.4];
  sample_distC = normpdf(C_interval,C,sqrt(V(1,1)));
  K_interval = [21:.01:23];
  sample_distK = normpdf(K_interval,K,sqrt(V(2,2))); 

  params = [C, K];
  modelfun = @(params) spring_function_2params(params,obs,t);
  [params_opt,fval] = fminsearch(modelfun,params);

  C_opt = params_opt(1)
  K_opt = params_opt(2)

  factor_opt = sqrt(K_opt - C_opt^2/4)
  denom_opt = sqrt(4*K_opt - C_opt^2)
  f = 2*exp(-C_opt*t/2).*cos(factor_opt*t);
  Res = obs - f;
  dfdc = exp(-C_opt*t/2).*((C_opt*t/denom_opt).*sin(factor_opt*t) - t.*cos(factor_opt*t));
  dfdk = (-2*t/denom_opt).*exp(-C_opt*t/2).*sin(factor_opt*t);
  S = [dfdc' dfdk'];
  var_est = (1/(n-p))*Res*Res'
  sigma_est = sqrt(var_est);
  V = inv(S'*S)*var_est

  alpha = 0.05;
  tcrit2 = tinv(1-alpha/2,n-1);
  conf_intC = [C_opt - tcrit2*sqrt(V(1,1)) C_opt + tcrit2*sqrt(V(1,1))]
  conf_intK = [K_opt - tcrit2*sqrt(V(2,2)) K_opt + tcrit2*sqrt(V(2,2))]


% Plot the model response, synthetic data, and residuals. One notes in the residual
% plot that the 2 sigma interval contains approximately 95% of the residuals.
%

  figure(1)
  plot(t,f,'k','linewidth',2)
  hold on
  plot(t,obs,'bx','linewidth',1)
  plot(t,0*f,'k','linewidth',2)
  hold off
  set(gca,'Fontsize',[20]);
  legend('Model','Synthetic Data','Location','NorthEast')
  xlabel('Time (s)')
  ylabel('Displacement')
  %print -depsc fig3_6a

  figure(2)
  plot(t,Res,'bx',t,0*f,'k','linewidth',2)
  hold on
  plot(t,2*sigma_est*ones(size(t)),'--r',t,-2*sigma_est*ones(size(t)),'--r','linewidth',3)
  hold off
  set(gca,'Fontsize',[20]);
  xlabel('Time (s)')
  ylabel('Residuals')
  %print -depsc fig3_6b

  
%%
% Construct the densities for c and k by repeated sampling.  The final KDE is performed
% using ksdensity. The variable count compiles the number of 95% confidence
% intervals that contain the true parameter value.
%
  C_count = 0;
  K_count = 0;
  for j = 1:10000
    error = sigma*randn(size(t));
    obs = f_true + error;
    Res = obs-f_true;
    modelfun = @(params) spring_function_2params(params,obs,t);
    [params_opt,fval] = fminsearch(modelfun,params);
    C_opt = params_opt(1);
    K_opt = params_opt(2);

    factor_opt = sqrt(K_opt - C_opt^2/4);
    denom_opt = sqrt(4*K_opt - C_opt^2);
    dfdc = exp(-C_opt*t/2).*((C_opt*t/denom_opt).*sin(factor_opt*t) - t.*cos(factor_opt*t));
    dfdk = (-2*t/denom_opt).*exp(-C_opt*t/2).*sin(factor_opt*t);
    S = [dfdc' dfdk'];
    var_est = (1/(n-p))*Res*Res';
    V = inv(S'*S)*var_est;

    C_lower = C_opt - tcrit2*sqrt(V(1,1));
    C_upper = C_opt + tcrit2*sqrt(V(1,1));
    if (C_lower <= C) & (C <= C_upper)
      C_count = C_count + 1;
    end
    Cvals(j) = C_opt;
    K_lower = K_opt - tcrit2*sqrt(V(2,2));
    K_upper = K_opt + tcrit2*sqrt(V(2,2));
    if (K_lower <= K) & (K <= K_upper)
      K_count = K_count + 1;
    end
    Kvals(j) = K_opt;
  end
  C_count
  K_count

  [density_C,Cmesh] = ksdensity(Cvals);
  [density_K,Kmesh] = ksdensity(Kvals);

%%
% Plot the sampling distribution and constructed density computed by generating data
% and optimizing 10,000 times. 
%
  
 
  figure(3)
  plot(Cmesh,density_C,'k--','linewidth',3)
  axis([1.23 1.37 0 28])
  hold on
  plot(C_interval,sample_distC,'b-','linewidth',3)
  hold off 
  set(gca,'Fontsize',[22]);
  xlabel('Optimal C')
  ylabel('PDF')
  legend('Constructed','Normal','Location','NorthEast')
%  print -depsc fig11_5a

  figure(4)
  plot(Kmesh,density_K,'k--','linewidth',3)
  axis([21.8 22.4 0 6])
  hold on
  plot(K_interval,sample_distK,'b-','linewidth',3)
  hold off
  set(gca,'Fontsize',[22]);
  xlabel('Optimal K')
  ylabel('PDF')
  legend('Constructed','Normal','Location','NorthEast')
%  print -depsc fig11_5b