--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/test/utils/test_loglikehood_ssm_td.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,335 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Test loglikehood_ssm_td
+% 
+% Test with an harmonic hoscillator and simplex parameters search
+% 
+% L Ferraioli 11-10-2010
+%
+% $Id: test_loglikehood_ssm_td.m,v 1.3 2010/11/26 13:52:32 luigi Exp $
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% set parameters
+
+m = 2; % kg
+k = 0.1; % kg s^-2
+damp = 0.05; % kg s^-1
+
+fAmp = 3;
+NAmp = 1;
+fs = 1;
+
+cutbefore = 100*fs;
+cutafter = 500*fs;
+
+f = logspace(-5+log10(fs),log10(fs/2),300);
+
+%% Load model
+
+sys = ssm(plist('built-in', 'HARMONIC_OSC_1D', 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+pl = plist('inputs', 'COMMAND.Force', 'HARMONIC_OSC_1D.outputs', 'Position', 'f', f);
+h = bode(sys,pl);
+
+iplot(h)
+
+%% input force and readout noise
+
+% input force
+fc = ao(plist('waveform','sine wave','A',fAmp,'f',1e-1,'phi',0,'toff',cutbefore/fs,'nsecs',6e2,'fs',fs));
+in = zeropad(fc,plist('Position','post','N',cutafter));
+
+
+rn = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', in.nsecs));
+rn = rn.*NAmp;
+
+
+
+%% Simulate the system
+
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force','noise.readout'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in,rn]);
+out = simulate(sys,plsym);
+
+iplot(out,rn)
+
+
+
+
+%% Do chisquare parameter search
+
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'m', 'k', 'vbeta'};
+
+
+x0 = [1 1 1];
+x0 = [2.5 0.08 0.1];
+
+
+% loglk =
+% loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)
+
+xp2 = fminsearch(@(x)utils.math.chisquare_ssm_td(x,in,out,parnames,mod,inNames,outNames,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));
+
+
+
+%%
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+outfit2 = simulate(sys,plsym);
+
+iplot(out,outfit2)
+iplot(out-outfit2)
+iplot(out-outfit2-rn)
+
+% xp =
+% 
+%           2.22352087931512         0.588010809495998         0.195689637008003
+
+
+
+%% Do parameters search
+
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+inNoiseNames = {'noise.readout'};
+inNoise = rn;
+outNoiseNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'M', 'K', 'VBETA'};
+
+
+% x0 = xp2;
+x0 = [2.5 0.08 0.1];
+
+% loglk =
+% loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)
+
+
+
+xp = fminsearch(@(x)utils.math.loglikehood_ssm_td(x,in,out,parnames,mod,inNames,outNames,rn,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));
+
+
+
+%%
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+outfit = simulate(sys,plsym);
+
+iplot(out,outfit)
+iplot(out-outfit)
+iplot(out-outfit-rn)
+
+
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% test covariace estimation
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'m', 'k', 'vbeta'};
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+modeout = simulate(sys,plsym);
+
+% get noise
+noise = out - modeout;
+nn = noise.y;
+% get covariance matrix
+cmat = utils.math.xCovmat(nn);
+cmatseq = cmat(1,:);
+
+% get covariance sequance
+[cseq,lags] = xcov(nn,'unbiased');
+cseq = ifftshift(cseq);
+lags=ifftshift(lags);
+cseqr = cseq(1:numel(cmatseq));
+lagsr = lags(1:numel(cmatseq));
+lagsr = lagsr.';
+
+% compare results
+figure
+plot(lagsr,cmatseq.','k')
+hold on
+plot(lagsr,cseqr,'r')
+
+figure
+plot(lagsr,cmatseq.'-cseqr,'k')
+
+[R,p] = chol(cmat);
+d = eig(cmat);
+% md = min(d);
+% cmat2 = cmat + abs(md).*eye(size(cmat));
+% [R,p] = chol(cmat2);
+
+ccmat = fliplr(flipud(cmat));
+[RR,pp] = chol(ccmat);
+
+%% correlation matrix
+
+[corr,sig] = utils.math.cov2corr(cmat);
+
+% test positive definiteness
+[R,p] = chol(corr);
+
+% get a valid correlation
+newcorr = validcorr(corr);
+
+%% 
+
+% get covariance sequance
+[cseq,lags] = xcov(nn,'unbiased');
+Num = numel(nn);
+for ii=1:Num
+  
+  ccmat(ii,:) = cseq(Num-ii+1:2*Num-ii);
+  
+end
+
+[R,p] = chol(ccmat);
+
+
+%% test the likelihood
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'m', 'k', 'vbeta'};
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+modeout1 = simulate(sys,plsym);
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[1 1 1]));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+modeout2 = simulate(sys,plsym);
+
+% xp = [1.1        0.6         0.01];
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+modeout3 = simulate(sys,plsym);
+
+% xp2 = [2.22352087931512         0.588010809495998         0.195689637008003];
+
+sys = copy(mod,1);
+sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
+sys.keepParameters();
+sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+  'AOS',[in]);
+modeout4 = simulate(sys,plsym);
+
+% get noise
+noise1 = out - modeout1; % true values
+noise2 = out - modeout2; % starting guess
+noise3 = out - modeout3; % fit values
+noise4 = out - modeout4; % chi2 fit values
+
+
+lik1 = utils.math.loglikehood_td(noise1,'cutbefore',cutbefore,'cutafter',cutafter)
+lik2 = utils.math.loglikehood_td(noise2,'cutbefore',cutbefore,'cutafter',cutafter)
+lik3 = utils.math.loglikehood_td(noise3,'cutbefore',cutbefore,'cutafter',cutafter)
+lik4 = utils.math.loglikehood_td(noise4,'cutbefore',cutbefore,'cutafter',cutafter)
+
+%% test a uniform grid
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'m', 'k', 'vbeta'};
+
+mm = [1.8:0.02:2.2];
+kc = [0.1:0.02:1];
+dd = [0.01:0.02:1];
+
+idx = 1;
+pars = struct;
+
+Ntot = numel(mm)*numel(kc)*numel(dd);
+
+lik = zeros(Ntot,1);
+%%
+
+for ii=1:numel(mm)
+  for jj=1:numel(kc)
+    for kk=1:numel(dd)
+      
+      fprintf('step %s of %s\n',num2str(idx),num2str(Ntot));
+      pars(idx).m = mm(ii);
+      pars(idx).k = kc(jj);
+      pars(idx).damp = dd(kk);
+      
+      sys = copy(mod,1);
+      sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[mm(ii) kc(jj) dd(kk)]));
+      sys.keepParameters();
+      sys.modifyTimeStep(plist('newtimestep',1/fs));
+
+      plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
+        'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
+        'AOS',[in]);
+      modeout = simulate(sys,plsym);
+      
+      noise = out - modeout;
+      
+      lik(idx) = utils.math.loglikehood_td(noise,'cutbefore',cutbefore,'cutafter',cutafter);
+      
+      idx = idx +1;
+    end
+  end
+end
+