--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/@matrix/dispersion.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,556 @@
+% DISPERSION computes the dispersion function
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% DESCRIPTION: DISPERSION computes the dispersion function
+%
+% CALL:        bs = dipersion(in,pl)
+%
+% INPUTS:      in      - matrix objects with input signals to the system
+%              model   - symbolic models containing the transfer function model
+%
+%              pl      - parameter list
+%
+% OUTPUTS:     bs   - dispersion function AO
+%
+% <a href="matlab:utils.helper.displayMethodInfo('matrix', 'dispersion')">Parameters Description</a>
+%
+% VERSION:    $Id: dispersion.m,v 1.1 2011/06/22 09:54:19 miquel Exp $
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function varargout = dispersion(varargin)
+
+% Check if this is a call for parameters
+if utils.helper.isinfocall(varargin{:})
+  varargout{1} = getInfo(varargin{3});
+  return
+end
+
+import utils.const.*
+utils.helper.msg(msg.PROC3, 'running %s/%s', mfilename('class'), mfilename);
+
+% Method can not be used as a modifier
+if nargout == 0
+  error('### crb cannot be used as a modifier. Please give an output variable.');
+end
+
+% Collect input variable names
+in_names = cell(size(varargin));
+for ii = 1:nargin,in_names{ii} = inputname(ii);end
+
+% Collect all AOs smodels and plists
+[mtxs, mtxs_invars] = utils.helper.collect_objects(varargin(:), 'matrix', in_names);
+pl              = utils.helper.collect_objects(varargin(:), 'plist', in_names);
+
+% Combine plists
+pl = parse(pl, getDefaultPlist);
+
+% get params
+params = find(pl,'FitParams');
+numparams = find(pl,'paramsValues');
+mmdl = find(pl,'model');
+channel = find(pl,'channel');
+mtxns = find(pl,'noise');
+outModel = find(pl,'outModel');
+bmdl = find(pl,'built-in');
+f1 = find(pl,'f1');
+f2 = find(pl,'f2');
+pseudoinv = find(pl,'pinv');
+tol = find(pl,'tol');
+outNames = find(pl,'outNames');
+inNames = find(pl,'inNames');
+
+% Decide on a deep copy or a modify
+fin = copy(mtxs, nargout);
+n = copy(mtxns, nargout);
+mdl = copy(mmdl,1);
+
+% Get number of experiments
+nexp = numel(fin);
+
+% fft
+% fin = fft(in);
+
+% N should get before spliting, in order to convert correctly from psd to
+% fft
+N = length(fin(1).getObjectAtIndex(1).x);
+
+% Get rid of fft f =0, reduce frequency range if needed
+if ~isempty(f1) && ~isempty(f2)
+  fin = split(fin,plist('frequencies',[f1 f2]));
+end
+
+FMall = zeros(numel(params),numel(params));
+% loop over experiments
+for k = 1:nexp
+  
+  utils.helper.msg(msg.IMPORTANT, sprintf('Analysis of experiment #%d',k), mfilename('class'), mfilename);
+  
+%   if (((numel(n(1).objs)) == 2) && (numel(fin(1).objs) == 2))
+    
+    % use signal fft to get frequency vector. Take into account signal
+    % could be empty or set to zero
+    % 1st channel
+    if all(fin(k).getObjectAtIndex(1,1).y == 0) || isempty(fin(k).getObjectAtIndex(1,1).y)
+      i1 = ao(plist('type','fsdata','xvals',0,'yvals',0));
+    else
+      i1 = fin(k).getObjectAtIndex(1,1);
+      freqs = i1.x;
+    end
+    % 2nd channel
+    if all(fin(k).getObjectAtIndex(2,1).y == 0) || isempty(fin(k).getObjectAtIndex(2,1).y)
+      i2 = ao(plist('type','fsdata','xvals',0,'yvals',0));
+    else
+      i2 = fin(k).getObjectAtIndex(2,1);
+      freqs = i2.x;
+    end
+    
+    % Compute psd
+    n1  = lpsd(n(k).getObjectAtIndex(1,1));
+    n2  = lpsd(n(k).getObjectAtIndex(2,1));
+    n12 = lcpsd(n(k).getObjectAtIndex(1,1),n(k).getObjectAtIndex(2,1));
+    
+    % interpolate to given frequencies
+    % noise
+    S11 = interp(n1,plist('vertices',freqs));
+    S12 = interp(n12,plist('vertices',freqs));
+    S22 = interp(n2,plist('vertices',freqs));
+    S21 = conj(S12);
+    
+    % get some parameters used below
+    fs = S11.fs;
+    
+    
+%     if (isempty(outModel))
+      
+%       if ~isempty(bmdl)
+        % compute built-in smodels
+%         for i = 1:4
+%           if strcmp(bmdl{i},'0');
+%             h(i) = smodel('0');
+%             h(i).setXvar('f');
+%             h(i).setXvals(freqs);
+%             h(i).setParams(params,numparams);
+%           else
+%             h(i) = smodel(plist('built-in',bmdl{i},'f',freqs));
+%             % set all params to all models. It is not true but harmless
+%             for ii = 1:numel(params)
+%               vecparams(ii) = {numparams(ii)*ones(size(freqs))};
+%             end
+%             h(i).setParams(params,vecparams);
+%           end
+%         end
+%       elseif ~isempty(mdl) && all(strcmp(class(mdl),'matrix'))
+    
+        for i = 1:numel(mdl.objs)
+          % set Xvals
+          h(i) = mdl.getObjectAtIndex(i).setXvals(freqs);
+          % set alias
+          h(i).assignalias(mdl.objs(i),plist('xvals',freqs));
+          % set paramaters
+          h(i).setParams(params,numparams);
+        end
+        % differentiate and eval
+        for i = 1:length(params)
+          utils.helper.msg(msg.IMPORTANT, sprintf('computing symbolic differentiation with respect %s',params{i}), mfilename('class'), mfilename);
+          % differentiate symbolically
+          dH11 = diff(h(1),params{i});
+          dH12 = diff(h(3),params{i});  % taking into account matrix index convention h(2) > H(2,1)
+          dH21 = diff(h(2),params{i});
+          dH22 = diff(h(4),params{i});
+          % evaluate
+          d11(i) = eval(dH11,plist('output type','fsdata','output x',freqs));
+          d12(i) = eval(dH12,plist('output type','fsdata','output x',freqs));
+          d21(i) = eval(dH21,plist('output type','fsdata','output x',freqs));
+          d22(i) = eval(dH22,plist('output type','fsdata','output x',freqs));
+        end
+        
+%       elseif ~isempty(mdl) && all(strcmp(class(mdl),'ssm'))
+%         
+%         meval = copy(mdl,1);
+%         % set parameter values
+%         meval.doSetParameters(params, numparams);
+%         
+%         % make numeric
+%         %         meval.doSubsParameters(params, true);
+%         
+%         % get the differentiation step
+%         step = find(pl,'diffStep');
+%         if isempty(step)
+%           error('### Please input a step for the numerical differentiation')
+%         end
+%         
+%         % differentiate and eval
+%         for i = 1:length(params)
+%           utils.helper.msg(msg.IMPORTANT, sprintf('computing numerical differentiation with respect %s, Step:%4.2d ',params{i},step(i)), mfilename('class'), mfilename);
+%           % differentiate numerically
+%           dH = meval.parameterDiff(plist('names', params(i),'values',step(i)));
+%           % create plist with correct outNames (since parameterDiff change them)
+%           out1 = strrep(outNames{1},'.', sprintf('_DIFF_%s.',params{i})); % 2x2 case
+%           out2 =strrep(outNames{2},'.', sprintf('_DIFF_%s.',params{i}));
+%           spl = plist('set', 'for bode', ...
+%             'outputs', {out1,out2}, ...
+%             'inputs', inNames, ...
+%             'reorganize', true,...
+%             'f', freqs);
+%           % do bode
+%           d  = bode(dH, spl);
+%           % assign according matlab's matrix notation: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
+%           d11(i) = d(1);
+%           d21(i) = d(2);
+%           d12(i) = d(3);
+%           d22(i) = d(4);
+%         end
+%         
+%       else
+%         error('### please introduce models for the transfer functions')
+%       end
+      
+%     elseif (~isempty(outModel))
+%       
+%       %   if(~isempty(outModel))
+%       for lll=1:size(outModel,1)
+%         for kkk=1:size(outModel,2)
+%           outModel(lll,kkk) = split(outModel(lll,kkk),plist('frequencies',[f1 f2]));
+%         end
+%       end
+%       %end
+%       
+%       % Avoid numerical differentiation (faster for the magnetic case)
+%       Param{1} = [1 0;
+%         0 0;];
+%       Param{2} = [0 0;
+%         0 1;];
+%       
+%       for pp = 1:length(params)
+%         for ll = 1:size(outModel,1)
+%           for kk = 1:size(Param{pp},2)
+%             % index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
+%             tmp = 0;
+%             for innerIndex = 1:size(outModel,2)
+%               tmp = tmp + outModel(ll,innerIndex).y * Param{pp}(innerIndex,kk);
+%             end
+%             h{pp}(:,(kk-1)*size(outModel,1) + ll) = tmp;
+%           end
+%         end
+%         
+%       end
+%       
+%       for i = 1:length(params)
+%         d11(i).y =  h{i}(:,1);
+%         d21(i).y =  h{i}(:,2);
+%         d12(i).y =  h{i}(:,3);
+%         d22(i).y =  h{i}(:,4);
+%       end
+%       
+%     end
+    
+    % scaling of PSD
+    % PSD = 2/(N*fs) * FFT *conj(FFT)
+    C11 = N*fs/2.*S11.y;
+    C22 = N*fs/2.*S22.y;
+    C12 = N*fs/2.*S12.y;
+    C21 = N*fs/2.*S21.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 Fisher Matrix
+    for i =1:length(params)
+      for j =1:length(params)
+        
+        v1v1 = conj(d11(i).y.*i1.y + d12(i).y.*i2.y).*(d11(j).y.*i1.y + d12(j).y.*i2.y);
+        v2v2 = conj(d21(i).y.*i1.y + d22(i).y.*i2.y).*(d21(j).y.*i1.y + d22(j).y.*i2.y);
+        v1v2 = conj(d11(i).y.*i1.y + d12(i).y.*i2.y).*(d21(j).y.*i1.y + d22(j).y.*i2.y);
+        v2v1 = conj(d21(i).y.*i1.y + d22(i).y.*i2.y).*(d11(j).y.*i1.y + d12(j).y.*i2.y);
+        
+        FisMat(i,j) = sum(real(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1));
+      end
+    end
+    utils.helper.msg(msg.IMPORTANT, sprintf('rank(FisMat) = %d', rank(FisMat)));
+
+    for kk = 1:numel(freqs)
+      % create input signal with power at single freq. 
+      % depending on input channel
+      p = zeros(1,numel(freqs));
+      if channel == 1
+        p(kk) = sum(i1.y);
+        % create aos
+        i1single = ao(plist('Xvals',freqs,'Yvals',p));
+        i2single = ao(plist('Xvals',freqs,'Yvals',zeros(1,numel(freqs))));
+      elseif channel == 2
+        p(kk) = sum(i2.y);
+        % create aos
+        i1single = ao(plist('Xvals',freqs,'Yvals',zeros(1,numel(freqs))));
+        i2single = ao(plist('Xvals',freqs,'Yvals',p));
+      else
+        error('### wrong channel')
+      end
+      
+      % compute Fisher Matrix for single frequencies
+      for i =1:length(params)
+        for j =1:length(params)
+          
+          v1v1 = conj(d11(i).y.*i1single.y + d12(i).y.*i2single.y).*(d11(j).y.*i1single.y + d12(j).y.*i2single.y);
+          v2v2 = conj(d21(i).y.*i1single.y + d22(i).y.*i2single.y).*(d21(j).y.*i1single.y + d22(j).y.*i2single.y);
+          v1v2 = conj(d11(i).y.*i1single.y + d12(i).y.*i2single.y).*(d21(j).y.*i1single.y + d22(j).y.*i2single.y);
+          v2v1 = conj(d21(i).y.*i1single.y + d22(i).y.*i2single.y).*(d11(j).y.*i1single.y + d12(j).y.*i2single.y);
+          
+          FisMatsingle(i,j) = sum(real(InvS11.*v1v1 + InvS22.*v2v2 - InvS12.*v1v2 - InvS21.*v2v1));
+        end
+      end
+      d(kk) = trace(FisMat\FisMatsingle)/numel(i1.x);    % had to divide for num. freqs
+%         d(kk) = trace(pinv(FisMat)*FisMatsingle)/numel(i1.x);    % had to divide for num. freqs
+
+    end
+        
+%     % store Fisher Matrix for this run
+%     FM{k} = FisMat;
+%     % adding up
+%     FMall = FMall + FisMat;
+    
+%   elseif ((numel(n(1).objs) == 3) && (numel(in.objs) == 4) && ~isempty(outModel))
+%     % this is only valid for the magnetic model, where we have 4 inputs
+%     % (corresponding to the 4 conformator waveforms) and 3 outputs
+%     % (corresponding to IFO.x12, IFO.eta1 and IFO.phi1). And there is a
+%     % contribution of an outModel converting the conformator waveforms
+%     % into forces and torques.
+%     
+%     
+%     % For other cases not implemented yet.
+%     
+%     % use signal fft to get frequency vector. Take into account signal
+%     % could be empty or set to zero
+%     % 1st channel
+%     freqs = fin.getObjectAtIndex(1,1).x;
+%     
+%     for ii = 1:numel(n.objs)
+%       for jj = ii:numel(n.objs)
+%         % Compute psd
+%         if (ii==jj)
+%           spec(ii,jj)  = psd(n(k).getObjectAtIndex(ii), pl);
+%           S2(ii,jj) = interp(spec(ii,jj),plist('vertices',freqs,'method','linear'));
+%         else
+%           spec(ii,jj) = cpsd(n(k).getObjectAtIndex(ii),n(k).getObjectAtIndex(jj),pl);
+%           S2(ii,jj) = interp(spec(ii,jj),plist('vertices',freqs,'method','linear'));
+%           S2(jj,ii) = conj(S2(ii,jj));
+%         end
+%       end
+%     end
+%     
+%     S = matrix(S2,plist('shape',[numel(n.objs) numel(n.objs)]));
+%     
+%     % get some parameters used below
+%     fs = S.getObjectAtIndex(1,1).fs;
+%     
+%     
+%     if(~isempty(outModel))
+%       for lll=1:size(outModel,1)
+%         for kkk=1:size(outModel,2)
+%           outModel(lll,kkk) = split(outModel(lll,kkk),plist('frequencies',[f1 f2]));
+%         end
+%       end
+%     end
+%     
+%     % Avoid numerical differentiation (faster for the magnetic case)
+%     Param{1} = [  1 0 0 0;
+%       0 0 0 0;
+%       0 0 0 0;];
+%     Param{2} = [  0 1 0 0;
+%       0 0 0 0;
+%       0 0 0 0;];
+%     Param{3} = [  0 0 0 0;
+%       0 0 1 0;
+%       0 0 0 0;];
+%     Param{4} = [  0 0 0 0;
+%       0 0 0 0;
+%       0 0 0 1;];
+%     
+%     % scaling of PSD
+%     % PSD = 2/(N*fs) * FFT *conj(FFT)
+%     for j = 1: numel(S.objs)
+%       % spectra to variance
+%       C(:,j) = (N*fs/2)*S.objs(j).data.getY;
+%     end
+%     
+%     detm = (C(:,1).*C(:,5).*C(:,9) + ...
+%       C(:,2).*C(:,6).*C(:,7) + ...
+%       C(:,3).*C(:,4).*C(:,8) -...
+%       C(:,7).*C(:,5).*C(:,3) -...
+%       C(:,8).*C(:,6).*C(:,1) -...
+%       C(:,9).*C(:,4).*C(:,2));
+%     
+%     InvS11 = (C(:,5).*C(:,9) - C(:,8).*C(:,6))./detm;
+%     InvS12 = -(C(:,4).*C(:,9) - C(:,7).*C(:,6))./detm;
+%     InvS13 = (C(:,4).*C(:,8) - C(:,7).*C(:,5))./detm;
+%     InvS21 = -(C(:,2).*C(:,9) - C(:,8).*C(:,3))./detm;
+%     InvS22 = (C(:,1).*C(:,9) - C(:,7).*C(:,3))./detm;
+%     InvS23 = -(C(:,1).*C(:,8) - C(:,7).*C(:,2))./detm;
+%     InvS31 = (C(:,2).*C(:,6) - C(:,5).*C(:,3))./detm;
+%     InvS32 = -(C(:,1).*C(:,6) - C(:,4).*C(:,3))./detm;
+%     InvS33 = (C(:,1).*C(:,5) - C(:,4).*C(:,2))./detm;
+%     
+%     for pp = 1:length(params)
+%       for ll = 1:size(outModel,1)
+%         for kk = 1:size(Param{pp},2)
+%           % index convention: H(1,1)->h(1)  H(2,1)->h(2)  H(1,2)->h(3)  H(2,2)->h(4)
+%           tmp = 0;
+%           for innerIndex = 1:size(outModel,2)
+%             tmp = tmp + outModel(ll,innerIndex).y * Param{pp}(innerIndex,kk);
+%           end
+%           h{pp}(:,(kk-1)*size(outModel,1) + ll) = tmp;
+%         end
+%       end
+%       
+%     end
+%     
+%     for kk = 1:numel(in.objs)
+%       inV(:,kk) = fin.objs(kk).data.getY;
+%     end
+%     
+%     
+%     % compute Fisher Matrix
+%     for i =1:length(params)
+%       for j =1:length(params)
+%         
+%         for ll = 1:size(outModel,1)
+%           tmp = 0;
+%           for kk = 1:size(Param{1},2)
+%             tmp = tmp + h{i}(:,(kk-1)*size(outModel,1) + ll).*inV(:,kk);
+%           end
+%           v{i}(:,ll) = tmp;
+%         end
+%         
+%         
+%         for ll = 1:size(outModel,1)
+%           tmp = 0;
+%           for kk = 1:size(Param{1},2)
+%             tmp = tmp + h{j}(:,(kk-1)*size(outModel,1) + ll).*inV(:,kk);
+%           end
+%           v{j}(:,ll) = tmp;
+%         end
+%         
+%         v1v1 = conj(v{i}(:,1)).*v{j}(:,1);
+%         v1v2 = conj(v{i}(:,1)).*v{j}(:,2);
+%         v1v3 = conj(v{i}(:,1)).*v{j}(:,3);
+%         v2v1 = conj(v{i}(:,2)).*v{j}(:,1);
+%         v2v2 = conj(v{i}(:,2)).*v{j}(:,2);
+%         v2v3 = conj(v{i}(:,2)).*v{j}(:,3);
+%         v3v1 = conj(v{i}(:,3)).*v{j}(:,1);
+%         v3v2 = conj(v{i}(:,3)).*v{j}(:,2);
+%         v3v3 = conj(v{i}(:,3)).*v{j}(:,3);
+%         
+%         FisMat(i,j) = sum(real(InvS11.*v1v1 +...
+%           InvS12.*v1v2 +...
+%           InvS13.*v1v3 +...
+%           InvS21.*v2v1 +...
+%           InvS22.*v2v2 +...
+%           InvS23.*v2v3 +...
+%           InvS31.*v3v1 +...
+%           InvS32.*v3v2 +...
+%           InvS33.*v3v3));
+%       end
+%     end
+%     % store Fisher Matrix for this run
+%     FM{k} = FisMat;
+%     % adding up
+%     FMall = FMall + FisMat;
+%   else
+%     error('Implemented cases: 2 inputs / 2outputs (TN3045 analysis), and 4 inputs / 3 outpus (magnetic complete analysis model. Other cases have not been implemented yet. Sorry for the inconvenience)');
+%   end
+  
+  
+% end
+
+% inverse is the optimal covariance matrix
+% if pseudoinv && isempty(tol)
+%   cov = pinv(FMall);
+% elseif pseudoinv
+%   cov = pinv(FMall,tol);
+% else
+%   cov = FMall\eye(size(FMall));
+% end
+
+
+% create AO
+out = ao(plist('Xvals',freqs,'Yvals',d,'type','fsdata','fs',fs,'name',''));
+% spectrum in the procinfo
+if channel == 1
+out.setProcinfo(plist('S11',S11));
+elseif channel == 2
+out.setProcinfo(plist('S22',S22));
+end
+% Fisher Matrix in the procinfo
+out.setProcinfo(plist('FisMat',FisMat));
+
+varargout{1} = out;
+end
+
+end
+
+
+%--------------------------------------------------------------------------
+% Get Info Object
+%--------------------------------------------------------------------------
+function ii = getInfo(varargin)
+if nargin == 1 && strcmpi(varargin{1}, 'None')
+  sets = {};
+  pls  = [];
+else
+  sets = {'Default'};
+  pls  = getDefaultPlist;
+end
+% Build info object
+ii = minfo(mfilename, 'ao', 'ltpda', utils.const.categories.sigproc, '$Id: dispersion.m,v 1.1 2011/06/22 09:54:19 miquel Exp $', sets, pls);
+end
+
+%--------------------------------------------------------------------------
+% Get Default Plist
+%--------------------------------------------------------------------------
+function plout = getDefaultPlist()
+persistent pl;
+if exist('pl', 'var')==0 || isempty(pl)
+  pl = buildplist();
+end
+plout = pl;
+end
+
+function pl = buildplist()
+pl = plist.LPSD_PLIST;
+pset(pl,'Navs',1)
+
+p = plist({'f1', 'Initial frequency for the analysis'}, paramValue.EMPTY_DOUBLE);
+pl.append(p);
+
+p = plist({'f2', 'Final frequency for the analysis'}, paramValue.EMPTY_DOUBLE);
+pl.append(p);
+
+p = plist({'FitParamas', 'Parameters of the model'}, paramValue.EMPTY_STRING);
+pl.append(p);
+
+p = plist({'model','An array of matrix models'}, paramValue.EMPTY_STRING);
+pl.append(p);
+
+p = plist({'noise','An array of matrices with the cross-spectrum matrices'}, paramValue.EMPTY_STRING);
+pl.append(p);
+
+p = plist({'built-in','Symbolic models of the system as a string of built-in models'}, paramValue.EMPTY_STRING);
+pl.append(p);
+
+p = plist({'frequencies','Array of start/sop frequencies where the analysis is performed'}, paramValue.EMPTY_STRING);
+pl.append(p);
+
+p = plist({'pinv','Use the Penrose-Moore pseudoinverse'}, paramValue.TRUE_FALSE);
+pl.append(p);
+
+p = plist({'tol','Tolerance for the Penrose-Moore pseudoinverse'}, paramValue.EMPTY_DOUBLE);
+pl.append(p);
+
+p = plist({'diffStep','Numerical differentiation step for ssm models'}, paramValue.EMPTY_DOUBLE);
+pl.append(p);
+
+end