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
|