--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/+utils/@math/loglikelihood_ssm.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,261 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Compute log-likelihood for SSM objects
+%
+% M Nofrarias 15-06-09
+%
+% $Id: loglikelihood_ssm.m,v 1.3 2011/11/16 08:52:49 nikos Exp $
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [loglk snr]= loglikelihood_ssm(xn,in,out,noise,model,params,inNames,outNames, spl, Amats, Bmats, Cmats, Dmats)
+  
+  loglk = 0;
+  snr = 0;
+  switch class(in)
+    case 'ao'
+      % parameters
+      fs = noise(1).fs;
+      N = length(noise(1).y);
+      
+      if (numel(in) == 1 && numel(out) == 1)
+        xn = double(xn);
+        
+        spl = plist('set', 'for bode', ...
+          'outputs', outNames, ...
+          'inputs', inNames, ...
+          'reorganize', false,...
+          'f', in(1).x);
+        
+        eval = copy(model,1);
+        
+        % set parameter values
+        eval.doSetParameters(params, xn);
+        
+        % make numeric
+        eval.doSubsParameters(params, true);
+        
+        % do bode
+        h11  = bode(eval, spl, 'internal');
+        
+        f = in.x;
+        % spectra to variance
+        C11 = (N*fs/2)*noise(1).y;
+        
+        % compute elements of inverse cross-spectrum matrix
+        InvS11 = 1./C11;
+        
+        % compute log-likelihood terms first, all at once does not cancel the
+        % imag part when multiplying x.*conj(x)
+        v1v1 = conj(out(1).y - h11.y.*in(1).y).*(out(1).y - h11.y.*in(1).y);
+        
+        tmplt = h11.*in(1).y;
+        
+        %computing SNR
+        snrexp = utils.math.stnr(tmplt,0,out(1).y,0,InvS11,0,0,0);
+    
+        snr = snr + 20*log10(snrexp);
+        
+        log1exp = sum(InvS11.*v1v1);
+        
+        loglk = loglk + log1exp;
+        
+      elseif (numel(in) == 2 && numel(out) == 2)
+        
+        f = in(1).x;
+        
+        xn = double(xn);
+        
+        spl = plist('set', 'for bode', ...
+          'outputs', outNames, ...
+          'inputs', inNames, ...
+          'reorganize', false,...
+          'f', in(1).x);
+        
+        eval = copy(model,1);
+        
+        % set parameter values
+        eval.doSetParameters(params, xn);
+        
+        % make numeric
+        eval.doSubsParameters(params);
+        
+        % do bode
+        h  = bode(eval, spl, 'internal');
+        h11 = h(1);
+        h12 = h(2);
+        h21 = h(3);
+        h22 = h(4);
+        
+        % spectra to variance
+        C11 = (N*fs/2)*noise(1).y;
+        C22 = (N*fs/2)*noise(2).y;
+        C12 = (N*fs/2)*noise(3).y;
+        C21 = (N*fs/2)*noise(4).y;
+        
+        % compute elements of inverse cross-spectrum matrix
+        InvS11 = (C22./(C11.*C22 - C12.*C21));
+        InvS22 = (C11./(C11.*C22 - C12.*C21));
+        InvS12 = (C21./(C11.*C22 - C12.*C21));
+        InvS21 = (C12./(C11.*C22 - C12.*C21));
+        
+        % compute log-likelihood terms first, all at once does not cancel the
+        % imag part when multiplying x.*conj(x)
+        v1v1 = conj(out(1).y - h11.y.*in(1).y - h12.y.*in(2).y).*(out(1).y - h11.y.*in(1).y - h12.y.*in(2).y);
+        v2v2 = conj(out(2).y - h21.y.*in(1).y - h22.y.*in(2).y).*(out(2).y - h21.y.*in(1).y - h22.y.*in(2).y);
+        v1v2 = conj(out(1).y - h11.y.*in(1).y - h12.y.*in(2).y).*(out(2).y - h21.y.*in(1).y - h22.y.*in(2).y);
+        v2v1 = conj(out(2).y - h21.y.*in(1).y - h22.y.*in(2).y).*(out(1).y - h11.y.*in(1).y - h12.y.*in(2).y);
+        
+        tmplt1 = h11.*in(1).y + h12.*in(2).y;
+        tmplt2 = h21.*in(1).y + h22.*in(2).y;
+        
+        %computing SNR
+        snrexp = utils.math.stnr(tmplt1,tmplt2,out(1).y,out(2).y,InvS11,InvS22,InvS12,InvS21);
+    
+        snr = snr + 20*log10(snrexp); 
+        
+        log1exp = sum(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1);
+        
+        loglk = loglk + log1exp;
+        
+      else
+        error('This method is only implemented for 1 input / 1 output model or for 2 inputs / 2 outputs models');
+      end
+      
+    case 'matrix'
+      % parameters
+      
+      fs = in(1).objs(1).fs;
+      
+      % num. experiments
+      nexp = numel(in);
+      
+      noutChannels = numel(out(1).objs);
+      
+      N = length(noise(1).objs(1).y);
+      
+      loglk = 0;
+      for nnn = 1:nexp
+        
+        if ((numel(in(1).objs) == 1) && numel(out(1).objs) == 1)
+          
+          freqs = in(nnn).objs(1).data.getX;
+          
+          xn = double(xn);
+          
+          spl = plist('set', 'for bode', ...
+            'outputs', outNames, ...
+            'inputs', inNames, ...
+            'reorganize', false,...
+            'f', freqs);
+          
+          
+          eval = copy(model(nnn),1);
+          
+          % set parameter values
+          eval.doSetParameters(params, xn);
+          
+          % make numeric
+          eval.doSubsParameters(params, true);
+          
+          % do bode
+          h(:,1)  = bode(eval, spl, 'internal');
+          
+          for j = 1:noutChannels^2
+            % spectra to variance
+            % (N*fs/2)* this multiplication is done now in mcmc
+            C(:,j) = noise(nnn).objs(j).data.getY;
+          end
+          
+          % compute elements of inverse cross-spectrum matrix
+          InvS11 = 1./C(:,1);
+          
+          
+          % compute log-likelihood terms first, all at once does not cancel the
+          % imag part when multiplying x.*conj(x)
+          in1 = in(nnn).objs(1).data.getY;
+          out1 = out(nnn).objs(1).data.getY;
+          
+          % matrix index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
+          v1v1 = conj(out1 - h.getObjectAtIndex(1,1).y.*in1).*(out1 - h.getObjectAtIndex(1,1).y.*in1);
+          
+          tmplt = h.getObjectAtIndex(1,1).y.*in1;
+          
+          %computing SNR
+          snrexp = utils.math.stnr(tmplt,0,out1,0,InvS11,0,0,0);
+    
+          snr = snr + 20*log10(snrexp); 
+        
+          log1exp = sum(InvS11.*v1v1);
+        
+          loglk = loglk + log1exp;
+          
+        elseif ((numel(in(1).objs) == 2) && numel(out(1).objs) == 2)
+          
+          freqs = in(nnn).objs(1).data.getX;
+          
+          xn = double(xn);
+          
+          spl.pset('f', freqs);
+          
+          eval = model(nnn);
+          eval.setA(Amats);
+          eval.setB(Bmats);
+          eval.setC(Cmats);
+          eval.setD(Dmats);
+          
+          % set parameter values
+          eval.doSetParameters(params, xn);
+          
+          % make numeric
+          eval.doSubsParameters(params, true);
+          
+          % do bode
+          [h1 h2 h3 h4]  = bode(eval, spl);
+          
+          for j = 1:noutChannels^2
+            % spectra to variance
+            % (N*fs/2)* this multiplication is done now in mcmc
+            C(:,j) = noise(nnn).objs(j).data.getY;
+          end
+          
+          % compute elements of inverse cross-spectrum matrix
+          detm = (C(:,1).*C(:,4) - C(:,2).*C(:,3));
+          InvS11 = C(:,4)./detm; %1 4
+          InvS22 = C(:,1)./detm; %4 1
+          InvS12 = C(:,2)./detm; %2 2
+          InvS21 = C(:,3)./detm; %3 3
+          
+          % compute log-likelihood terms first, all at once does not cancel the
+          % imag part when multiplying x.*conj(x)
+          in1 = in(nnn).objs(1).data.getY;
+          in2 = in(nnn).objs(2).data.getY;
+          out1 = out(nnn).objs(1).data.getY;
+          out2 = out(nnn).objs(2).data.getY;
+          
+          % matrix index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
+          v1v1 = conj(out1 - h1.*in1 - h3.*in2).*(out1 - h1.*in1 - h3.*in2);
+          v2v2 = conj(out2 - h2.*in1 - h4.*in2).*(out2 - h2.*in1 - h4.*in2);
+          v1v2 = conj(out1 - h1.*in1 - h3.*in2).*(out2 - h2.*in1 - h4.*in2);
+          v2v1 = conj(out2 - h2.*in1 - h4.*in2).*(out1 - h1.*in1 - h3.*in2);
+          
+          tmplt1 = h1.*in1 + h3.*in2;
+          tmplt2 = h2.*in1 + h4.*in2;
+          
+          %computing SNR
+          snrexp = utils.math.stnr(tmplt1,tmplt2,out1,out2,InvS11,InvS22,InvS12,InvS21);
+    
+          snr = snr + 20*log10(snrexp);
+          
+          log1exp = sum(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1);
+          
+          loglk = loglk + log1exp;
+        else
+          error('This method is only implemented for 1 input / 1 output model or for 2 inputs / 2 outputs models');
+        end
+        
+        
+        
+      end
+  end
+end
+