--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/+utils/@math/mhsample.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,283 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Metropolis algorithm
+%
+% M Nofrarias 15-06-09
+%
+% $Id: mhsample.m,v 1.19 2011/11/16 08:52:50 nikos Exp $
+% prior
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+function [smpl smplr] = mhsample(model,in,out,nse,cov,number,limit,param,Tc,xi,xo,search,jumps,parplot,dbg_info,inNames,outNames,fpars,anneal,SNR0,DeltaL,inModel,outModel)
+  
+  import utils.const.*
+  
+  % compute mean and range
+  mn = mean(limit);
+  range = diff(limit);
+  % initialize
+  acc = [];
+  nacc = 1;
+  nrej = 1;
+  loop = 0;
+  oldacc = 0;
+  oldrej = 0;
+  chgsc = 0;
+  nparam = length(param);
+  delta = 1;
+  SNR = 0;
+  heat = xi;
+  
+  % plist to pass in to the sampler for use in bode
+  spl = plist('outputs', outNames, ...
+    'inputs', inNames, ...
+    'reorganize', false,...
+    'numeric output',true);
+
+  % check if using priors
+  nparams = size(xo);
+  if isempty(fpars)
+      fpars = zeros(nparams(2),3);
+  end
+
+  
+  
+  % switch depending on model class (initial likelihood)
+  switch class(model)
+    case 'matrix'
+      [loglk1 SNR] = utils.math.loglikelihood_matrix(xo,in,out,nse,model,param,inModel,outModel);
+    case 'smodel'
+      loglk1 = utils.math.loglikelihood(xo,in,out,nse,model,param);
+    case 'ssm'
+      Amats = model.amats;
+      Bmats = model.bmats;
+      Cmats = model.cmats;
+      Dmats = model.dmats;
+      loglk1 = utils.math.loglikelihood_ssm(xo,in,out,nse,model,param,inNames,outNames, spl, Amats, Bmats, Cmats, Dmats);
+    otherwise
+      error('### Model must be either from the ''smodel'' or the ''ssm'' class. Please check the inputs')
+  end
+  
+  % accept the first sample if no search active
+  if ~search
+    smpl(1,:) = xo;
+    nacc = 1;
+  else
+    smpl = [];
+    nacc = 0;
+  end
+  nrej = 0;
+  
+  utils.helper.msg(msg.IMPORTANT, 'Starting Monte Carlo sampling', mfilename('class'), mfilename);
+  
+  % compute prior for the initial sample of the chain
+  logprior1 = logPriors(xo,fpars);
+  % if initial guess is far away, this contidion sets our logprior
+  % to zero and takes account only the loglikelihood for the 
+  % computation of logratio. From this initial point it starts
+  % searching for the small area of acceptance.
+  if logprior1 == -Inf;
+     logprior1 = 0; 
+  end
+  
+  T = Tc(1);
+  hjump = 0;
+  % main loop
+  while(nacc<number)
+    %    jumping criterion during search phase
+    %    - 2-sigma  by default
+    %    - 10-sigma  jumps at mod(10) samples
+    %    - 100-sigma jumps at mod(25) samples
+    %    - 1000-sigma jumps at mod(100) samples
+
+    
+    if search
+      if nacc <= Tc(1)
+        if(mod(nacc,10) == 0 && mod(nacc,25) ~= 0 && mod(nacc,100) ~= 0 && hjump  ~= 1)
+          hjump = 1;
+          xn = mvnrnd(xo,jumps(2)^2*cov);
+        elseif(mod(nacc,20) == 0 && mod(nacc,100) ~= 0 && hjump  ~= 1)
+          hjump = 1;
+          xn = mvnrnd(xo,jumps(3)^2*cov);
+        elseif(mod(nacc,50) == 0 && hjump  ~= 1)
+          hjump = 1;
+          xn = mvnrnd(xo,jumps(4)^2*cov);
+          %           xn = xo + range/2+rand(size(xo));
+        else
+          hjump = 0;
+          xn = mvnrnd(xo,jumps(1)^2*cov);
+        end
+      else
+        xn = mvnrnd(xo,cov);  % changed that too
+      end
+    else
+      xn = mvnrnd(xo,cov);
+    end
+    
+    
+    % compute prior probability 
+    logprior2 = logPriors(xn,fpars);
+    
+    %check if out of limits
+    if( any(xn < limit(1,:)) || any(xn > limit(2,:)))
+      logprior2 =  inf;
+      loglk2 = inf;
+      betta = inf;
+      %heat = inf;
+      
+    % This condition save us computation time.
+    elseif logprior2 == -Inf 
+        loglk2 = inf;
+        betta = inf; 
+    else
+      % switch depending on model class (new likelihood)
+      switch class(model)
+        case 'matrix'
+          [loglk2 SNR] = utils.math.loglikelihood_matrix(xn,in,out,nse,model,param,inModel,outModel);
+        case 'smodel'
+          loglk2 = utils.math.loglikelihood(xn,in,out,nse,model,param);
+        case 'ssm'
+          [loglk2 SNR] = utils.math.loglikelihood_ssm(xn,in,out,nse,model,param,inNames,outNames, spl, Amats, Bmats, Cmats, Dmats);
+        otherwise
+          error('### Model must be either from the ''smodel'' or the ''ssm'' class. Please check the inputs')
+      end
+      
+      
+      % compute annealing
+      if ~isempty(Tc)
+        if nacc <= Tc(1)
+            
+          % compute heat factor
+          switch anneal
+              case 'simul'
+                delta = 1;
+              case 'thermo'
+                if (0 <= SNR(1) && SNR(1) <= SNR0)
+                delta = 1;
+                elseif (SNR(1) > SNR0)
+                delta = (SNR(1)/SNR0)^2;              
+                end
+              case 'simple'
+                if (nacc > 10 && mod(nacc,10) == 0)
+                deltalogp = std(smpl(nacc-10:nacc,1));          
+                    if deltalogp <= DeltaL(1) 
+                    T = T + DeltaL(3);
+                    elseif deltalogp >= DeltaL(2)
+                    T = T - DeltaL(4);
+                    end 
+                    delta = Tc(1)/T;
+                end
+                heat = xi*delta;
+          end
+            
+          betta = 1/2 * 10^(-heat*(1-Tc(1)/Tc(2)));
+        elseif Tc(1) < nacc  && nacc <= Tc(2)
+          betta = 1/2 * 10^(-heat*(1-nacc/Tc(2)));
+        else
+          betta = 1/2;
+        end
+      else
+        betta = 1/2;
+      end
+      
+    end % here we are 
+    
+    % likelihood ratio
+    logr = betta*(logprior2 + loglk2 - loglk1 - logprior1) ;    
+    % decide if sample is accepted or not
+    if logr < 0
+      xo = xn;
+      nacc = nacc+1;
+      sumsamples = nacc + nrej;
+      smpl(nacc,1) = loglk2;
+      smpl(nacc,2:size(xn,2)+1) = xn;
+      smplr(sumsamples,1:size(xn,2)) = xn;
+      smplr(sumsamples,size(xn,2)+1) = 1;
+      smplr(sumsamples,size(xn,2)+2) = loglk2;
+      loglk1 =loglk2;
+      logprior1=logprior2;
+      if dbg_info
+       %utils.helper.msg(msg.IMPORTANT, sprintf('acc.\t loglik: %d -> %d priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
+       utils.helper.msg(msg.IMPORTANT, sprintf('acccount: %d SNR: %d %d Heat: %d',nacc,SNR(1),SNR(2),heat));
+      end
+    elseif rand(1) > (1 - exp(-logr))
+      xo = xn;
+      nacc = nacc+1;
+      sumsamples = nacc + nrej;
+      smpl(nacc,1) = loglk2;
+      smpl(nacc,2:size(xn,2)+1) = xn;
+      smplr(sumsamples,1:size(xn,2)) = xn;
+      smplr(sumsamples,size(xn,2)+1) = 1;
+      smplr(sumsamples,size(xn,2)+2) = loglk2;
+      loglk1 =loglk2;
+      logprior1=logprior2;
+      if dbg_info
+        %utils.helper.msg(msg.IMPORTANT, sprintf('acc.\t loglik: %d -> %d priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
+        utils.helper.msg(msg.IMPORTANT, sprintf('acccount: %d SNR: %d %d Heat: %d',nacc,SNR(1),SNR(2),heat));
+      end
+    else
+      nrej = nrej+1;
+      sumsamples = nacc + nrej;      
+      smplr(sumsamples,1:size(xn,2)) = xn;
+      smplr(sumsamples,size(xn,2)+1) = 0;
+      smplr(sumsamples,size(xn,2)+2) = loglk2;
+      if dbg_info
+        %utils.helper.msg(msg.IMPORTANT, sprintf('rej.\t loglik: %d -> %d  priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
+        utils.helper.msg(msg.IMPORTANT, sprintf('rejcount: %d SNR: %d %d Heat: %d',nrej,SNR(1),SNR(2),heat));
+      end
+      
+    end
+    
+    % display and save
+    if(mod(nacc,100) == 0 && nacc ~= oldacc)
+      updacc = nacc-oldacc;
+      updrej = nrej-oldrej;
+      ratio(nacc/10,:) = updacc/(updacc+updrej);
+      utils.helper.msg(msg.IMPORTANT, sprintf('accepted: %d   rejected : %d   acc. rate: %4.2f',nacc,updrej,ratio(end)), mfilename('class'), mfilename);
+      for i = 1:numel(param)
+        fprintf('###  Parameters: %s = %d \n',param{i},xn(i))        
+      end
+      oldacc = nacc;
+      oldrej = nrej;
+      save('chain.txt','smpl','-ASCII')
+      save('acceptance.txt','ratio','-ASCII')
+    end
+    
+    % plot 
+    if ((mod(sumsamples,100) == 0) && (nacc ~= 0) && ~isempty(parplot))
+      for i = 1:numel(parplot)
+        figure(parplot(i))
+        plot(smpl(:,parplot(i)))
+      end
+      figure(i+1)
+      plot(smplr(:,2))
+    end
+    
+  end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%      compute logpriors
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function logprior = logPriors(parVect,fitparams)
+
+D = size(parVect);
+
+for ii=1:D(2)
+   prior(ii) = normpdf(parVect(ii),fitparams(ii,1),fitparams(ii,2))/fitparams(ii,3);
+   % checking if priors are used for this run 
+   if isnan(prior(ii))
+        prior(ii) = 1;
+    end
+end
+prior = log(prior);
+logprior = sum(prior);
+
+% assuming that priors are independent, then
+% p(x|8n)=p(x|81)p(x|82)p(x|83)...p(x|8n)=p1xp2xp3...xpn
+% and log(p(x|8n)) = logp1+logp2+...+logpn
+
+end
+
+