0
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 2 % Test loglikehood_ssm_td
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 3 %
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 4 % Test with an harmonic hoscillator and simplex parameters search
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 5 %
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 6 % L Ferraioli 11-10-2010
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 7 %
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 8 % $Id: test_ao_mcmc_td.m,v 1.1 2010/11/26 13:52:21 luigi Exp $
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 9 %
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 10 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 11 %% set parameters
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 12
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 13 m = 2; % kg
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 14 k = 0.1; % kg s^-2
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 15 damp = 0.05; % kg s^-1
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 16
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 17 fAmp = 3;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 18 NAmp = 1;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 19 fs = 1;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 20
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 21 cutbefore = 100*fs;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 22 cutafter = 500*fs;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 23
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 24 f = logspace(-5+log10(fs),log10(fs/2),300);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 25
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 26 %% Load model
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 27
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 28 sys = ssm(plist('built-in', 'HARMONIC_OSC_1D', 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 29 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 30 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 31 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 32
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 33 pl = plist('inputs', 'COMMAND.Force', 'HARMONIC_OSC_1D.outputs', 'Position', 'f', f);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 34 h = bode(sys,pl);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 35
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 36 iplot(h)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 37
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 38 %% input force and readout noise
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 39
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 40 % input force
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 41 fc = ao(plist('waveform','sine wave','A',fAmp,'f',1e-1,'phi',0,'toff',cutbefore/fs,'nsecs',6e2,'fs',fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 42 in = zeropad(fc,plist('Position','post','N',cutafter));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 43
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 44
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 45 rn = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', in.nsecs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 46 rn = rn.*NAmp;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 47
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 48
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 49
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 50 %% Simulate the system
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 51
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 52
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 53 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force','noise.readout'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 54 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 55 'AOS',[in,rn]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 56 out = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 57
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 58 iplot(out)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 59
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 60
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 61 %% test parameters search with simplex only
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 62
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 63
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 64 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 65 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 66 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 67 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 68 inNoise = rn;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 69 parnames = {'M', 'K', 'VBETA'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 70
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 71
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 72 % x0 = xp2;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 73 x0 = [2.5 0.08 0.1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 74
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 75 % loglk =
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 76 % loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 77
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 78 xp = fminsearch(@(x)utils.math.loglikehood_ssm_td(x,in,out,parnames,mod,inNames,outNames,inNoise,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 79
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 80
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 81 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 82 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 83 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 84 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 85
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 86 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 87 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 88 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 89 outfit = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 90
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 91 iplot(out,outfit)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 92 iplot(out-outfit)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 93 iplot(out-outfit-rn)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 94
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 95 %% Optimal covariance (taken from smodel)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 96
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 97 smod = smodel('1./(M.*((2.*i.*pi.*f).^2)+VBETA.*(2.*i.*pi.*f)+K)');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 98 smod.setParams({'M','VBETA','K'},{m,k,damp});
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 99 %smod.setParams({'M','VBETA','K'},{2.5, 0.08, 0.1});
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 100 smod.setXvar('f');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 101 smod.setXvals(f);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 102 smod.setYunits('kg^-1 s^-2');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 103
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 104 % get derivatives
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 105 dsmod_M = diff(smod,plist('var','M'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 106 dsmod_K = diff(smod,plist('var','K'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 107 dsmod_VBETA = diff(smod,plist('var','VBETA'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 108
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 109 % eval first order term
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 110 eM = fftfilt(in,dsmod_M);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 111 eK = fftfilt(in,dsmod_K);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 112 eVBETA = fftfilt(in,dsmod_VBETA);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 113
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 114 % information matrix
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 115 bm = [eM.y eK.y eVBETA.y];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 116 Im = bm'*bm;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 117
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 118 % covariance matrix
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 119 Cm = inv(Im);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 120
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 121 %% test mcmc_td
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 122
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 123 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 124 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 125 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 126 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 127 inNoise = rn;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 128 outNoiseNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 129 parnames = {'M', 'K', 'VBETA'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 130
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 131 % c = (1/sqrt(9))^2;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 132 c=0.8;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 133
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 134 ranges = {[1 3],[0 2],[0 1]};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 135
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 136 x0 = [2.5 0.08 0.1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 137
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 138
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 139
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 140 pl = plist('N',6000,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 141 'Input',in,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 142 'cov',c*Cm,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 143 'range',ranges,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 144 'parnames',parnames,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 145 'noise',inNoise,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 146 'model',mod,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 147 'inNames',inNames,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 148 'outNames',outNames,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 149 'search',true,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 150 'Tc',[2000 4000],...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 151 'heat',4,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 152 'jumps',[2e0 1e2 1e3 1e4],...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 153 'x0',x0,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 154 'simplex',true,...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 155 'plot',[1 2 3],...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 156 'debug',true);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 157
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 158 % metropolis(measurement,noise,model,plist)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 159 b = mcmc_td(out,pl);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 160 % save(b,'stoc6_mcmc_all_9p_23061300_ssm.mat');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 161
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 162