--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/test/test_ao_mcmc_td.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,162 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Test loglikehood_ssm_td
+% 
+% Test with an harmonic hoscillator and simplex parameters search
+% 
+% L Ferraioli 11-10-2010
+%
+% $Id: test_ao_mcmc_td.m,v 1.1 2010/11/26 13:52:21 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)
+
+
+%% test parameters search with simplex only
+
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+inNoise = rn;
+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,inNoise,'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)
+
+%% Optimal covariance (taken from smodel)
+
+smod = smodel('1./(M.*((2.*i.*pi.*f).^2)+VBETA.*(2.*i.*pi.*f)+K)');
+smod.setParams({'M','VBETA','K'},{m,k,damp});
+%smod.setParams({'M','VBETA','K'},{2.5, 0.08, 0.1});
+smod.setXvar('f');
+smod.setXvals(f);
+smod.setYunits('kg^-1 s^-2');
+
+% get derivatives
+dsmod_M = diff(smod,plist('var','M'));
+dsmod_K = diff(smod,plist('var','K'));
+dsmod_VBETA = diff(smod,plist('var','VBETA'));
+
+% eval first order term
+eM = fftfilt(in,dsmod_M);
+eK = fftfilt(in,dsmod_K);
+eVBETA = fftfilt(in,dsmod_VBETA);
+
+% information matrix
+bm = [eM.y eK.y eVBETA.y];
+Im = bm'*bm;
+
+% covariance matrix
+Cm = inv(Im);
+
+%% test mcmc_td
+
+mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
+  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
+inNames = {'COMMAND.Force'};
+outNames = {'HARMONIC_OSC_1D.position'};
+inNoise = rn;
+outNoiseNames = {'HARMONIC_OSC_1D.position'};
+parnames = {'M', 'K', 'VBETA'};
+
+% c = (1/sqrt(9))^2;
+c=0.8;
+
+ranges = {[1 3],[0 2],[0 1]};
+
+x0 = [2.5 0.08 0.1];
+
+
+
+pl = plist('N',6000,...
+  'Input',in,...
+  'cov',c*Cm,... 
+  'range',ranges,...
+  'parnames',parnames,...
+  'noise',inNoise,...
+  'model',mod,...
+  'inNames',inNames,...
+  'outNames',outNames,...
+  'search',true,...
+  'Tc',[2000 4000],...
+  'heat',4,...
+  'jumps',[2e0 1e2 1e3 1e4],...
+  'x0',x0,...
+  'simplex',true,...
+  'plot',[1 2 3],...
+  'debug',true);
+
+%  metropolis(measurement,noise,model,plist)
+b = mcmc_td(out,pl);
+% save(b,'stoc6_mcmc_all_9p_23061300_ssm.mat');
+
+