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_loglikehood_ssm_td.m,v 1.3 2010/11/26 13:52:32 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,rn)
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
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 62
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 63 %% Do chisquare parameter search
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 64
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 65
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 66 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 67 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 68 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 69 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 70 parnames = {'m', 'k', 'vbeta'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 71
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 72
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 73 x0 = [1 1 1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 74 x0 = [2.5 0.08 0.1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 75
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 76
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 77 % loglk =
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 78 % loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 79
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 80 xp2 = fminsearch(@(x)utils.math.chisquare_ssm_td(x,in,out,parnames,mod,inNames,outNames,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 81
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 82
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 83
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 84 %%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 85
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 86 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 87 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 88 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 89 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 90
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 91 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 92 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 93 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 94 outfit2 = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 95
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 96 iplot(out,outfit2)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 97 iplot(out-outfit2)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 98 iplot(out-outfit2-rn)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 99
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 100 % xp =
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 101 %
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 102 % 2.22352087931512 0.588010809495998 0.195689637008003
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 103
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 104
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 105
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 106 %% Do parameters search
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 107
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 108
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 109 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 110 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 111 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 112 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 113 inNoiseNames = {'noise.readout'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 114 inNoise = rn;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 115 outNoiseNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 116 parnames = {'M', 'K', 'VBETA'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 117
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 118
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 119 % x0 = xp2;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 120 x0 = [2.5 0.08 0.1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 121
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 122 % loglk =
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 123 % loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 124
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 125
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 126
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 127 xp = fminsearch(@(x)utils.math.loglikehood_ssm_td(x,in,out,parnames,mod,inNames,outNames,rn,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 128
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 129
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 130
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 131 %%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 132
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 133 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 134 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 135 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 136 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 137
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 138 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 139 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 140 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 141 outfit = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 142
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 143 iplot(out,outfit)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 144 iplot(out-outfit)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 145 iplot(out-outfit-rn)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 146
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 147
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 148 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 149 %% test covariace estimation
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 150
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 151 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 152 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 153 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 154 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 155 parnames = {'m', 'k', 'vbeta'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 156
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 157 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 158 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 159 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 160 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 161
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 162 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 163 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 164 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 165 modeout = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 166
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 167 % get noise
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 168 noise = out - modeout;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 169 nn = noise.y;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 170 % get covariance matrix
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 171 cmat = utils.math.xCovmat(nn);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 172 cmatseq = cmat(1,:);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 173
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 174 % get covariance sequance
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 175 [cseq,lags] = xcov(nn,'unbiased');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 176 cseq = ifftshift(cseq);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 177 lags=ifftshift(lags);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 178 cseqr = cseq(1:numel(cmatseq));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 179 lagsr = lags(1:numel(cmatseq));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 180 lagsr = lagsr.';
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 181
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 182 % compare results
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 183 figure
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 184 plot(lagsr,cmatseq.','k')
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 185 hold on
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 186 plot(lagsr,cseqr,'r')
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 187
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 188 figure
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 189 plot(lagsr,cmatseq.'-cseqr,'k')
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 190
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 191 [R,p] = chol(cmat);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 192 d = eig(cmat);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 193 % md = min(d);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 194 % cmat2 = cmat + abs(md).*eye(size(cmat));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 195 % [R,p] = chol(cmat2);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 196
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 197 ccmat = fliplr(flipud(cmat));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 198 [RR,pp] = chol(ccmat);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 199
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 200 %% correlation matrix
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 201
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 202 [corr,sig] = utils.math.cov2corr(cmat);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 203
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 204 % test positive definiteness
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 205 [R,p] = chol(corr);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 206
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 207 % get a valid correlation
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 208 newcorr = validcorr(corr);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 209
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 210 %%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 211
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 212 % get covariance sequance
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 213 [cseq,lags] = xcov(nn,'unbiased');
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 214 Num = numel(nn);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 215 for ii=1:Num
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 216
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 217 ccmat(ii,:) = cseq(Num-ii+1:2*Num-ii);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 218
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 219 end
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 220
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 221 [R,p] = chol(ccmat);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 222
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 223
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 224 %% test the likelihood
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 225
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 226 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 227 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 228 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 229 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 230 parnames = {'m', 'k', 'vbeta'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 231
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 232 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 233 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 234 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 235 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 236
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 237 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 238 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 239 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 240 modeout1 = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 241
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 242 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 243 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[1 1 1]));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 244 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 245 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 246
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 247 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 248 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 249 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 250 modeout2 = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 251
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 252 % xp = [1.1 0.6 0.01];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 253
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 254 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 255 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 256 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 257 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 258
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 259 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 260 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 261 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 262 modeout3 = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 263
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 264 % xp2 = [2.22352087931512 0.588010809495998 0.195689637008003];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 265
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 266 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 267 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 268 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 269 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 270
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 271 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 272 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 273 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 274 modeout4 = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 275
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 276 % get noise
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 277 noise1 = out - modeout1; % true values
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 278 noise2 = out - modeout2; % starting guess
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 279 noise3 = out - modeout3; % fit values
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 280 noise4 = out - modeout4; % chi2 fit values
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 281
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 282
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 283 lik1 = utils.math.loglikehood_td(noise1,'cutbefore',cutbefore,'cutafter',cutafter)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 284 lik2 = utils.math.loglikehood_td(noise2,'cutbefore',cutbefore,'cutafter',cutafter)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 285 lik3 = utils.math.loglikehood_td(noise3,'cutbefore',cutbefore,'cutafter',cutafter)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 286 lik4 = utils.math.loglikehood_td(noise4,'cutbefore',cutbefore,'cutafter',cutafter)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 287
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 288 %% test a uniform grid
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 289
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 290 mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 291 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 292 inNames = {'COMMAND.Force'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 293 outNames = {'HARMONIC_OSC_1D.position'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 294 parnames = {'m', 'k', 'vbeta'};
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 295
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 296 mm = [1.8:0.02:2.2];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 297 kc = [0.1:0.02:1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 298 dd = [0.01:0.02:1];
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 299
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 300 idx = 1;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 301 pars = struct;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 302
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 303 Ntot = numel(mm)*numel(kc)*numel(dd);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 304
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 305 lik = zeros(Ntot,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 306 %%
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 307
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 308 for ii=1:numel(mm)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 309 for jj=1:numel(kc)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 310 for kk=1:numel(dd)
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 311
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 312 fprintf('step %s of %s\n',num2str(idx),num2str(Ntot));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 313 pars(idx).m = mm(ii);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 314 pars(idx).k = kc(jj);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 315 pars(idx).damp = dd(kk);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 316
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 317 sys = copy(mod,1);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 318 sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[mm(ii) kc(jj) dd(kk)]));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 319 sys.keepParameters();
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 320 sys.modifyTimeStep(plist('newtimestep',1/fs));
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 321
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 322 plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 323 'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 324 'AOS',[in]);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 325 modeout = simulate(sys,plsym);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 326
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 327 noise = out - modeout;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 328
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 329 lik(idx) = utils.math.loglikehood_td(noise,'cutbefore',cutbefore,'cutafter',cutafter);
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 330
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 331 idx = idx +1;
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 332 end
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 333 end
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 334 end
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
+ − 335