+
−%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
−% 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');
+
−
+
−