0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011 clear all
0012 close all
0013
0014
0015
0016
0017
0018
0019
0020 N = 5000;
0021
0022
0023 NoisePlane = 'z';
0024 PlantPlane = 'z';
0025
0026
0027
0028
0029
0030
0031
0032
0033
0034 RecipPlant = 1;
0035
0036
0037 switch PlantPlane
0038 case 'z', stde = 1;
0039 case 's', stde = 0.05;
0040 case 'w', stde = 1;
0041 end
0042
0043
0044 Ts = 1/5.5;
0045 fs = 1/Ts;
0046
0047
0048 ny = 3;
0049 nu = 2;
0050
0051
0052
0053
0054
0055
0056 switch NoisePlane
0057
0058 case 'z',
0059 c0 = zeros(3,3,3);
0060 c0(1,1,:) = fliplr([3.5666e-01 0.9830e-01 3.3444e-01]);
0061 c0(2,2,:) = fliplr([1 -1 0.9]);
0062 c0(3,3,:) = fliplr([9.6478e-01 -5.4142e-01 9.4233e-01]);
0063 c0(3,1,:) = [0.1 -0.15 0];
0064 c0(1,3,:) = c0(3,1,:);
0065 c0(2,3,:) = [0.2 0 0];
0066 c0(3,2,:) = [0.35 0.5 -0.25];
0067 d0 = [1 -0.2 0.85];
0068
0069 case 's',
0070 c0 = zeros(3,3,3);
0071
0072 fz = 1.5/2;
0073 deltaz = 0.1;
0074 TheZero = 2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz);
0075 c0(1,1,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0076
0077 fz = 1.2;
0078 deltaz = 0.05;
0079 TheZero = 2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz);
0080 c0(2,2,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0081
0082
0083 fz = 0.35;
0084 deltaz = 0.15;
0085 TheZero = 2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz);
0086 c0(3,3,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0087
0088 fp1 = 0.25;
0089 deltap1 = 0.2;
0090 ThePole1 = 2*pi*sqrt(-1)*fp1*(1 + sqrt(-1)*deltap1);
0091 fp2 = 1;
0092 deltap2 = 0.05;
0093 ThePole2 = 2*pi*sqrt(-1)*fp2*(1 + sqrt(-1)*deltap2);
0094 d0 = real(poly([ThePole1,conj(ThePole1),ThePole2,conj(ThePole2)]));
0095 d0 = real(poly([ThePole2,conj(ThePole2)]));
0096 d0 = fliplr(d0);
0097
0098 case 'w',
0099 c0 = zeros(3,3,3);
0100
0101 fz = 1.5/2;
0102 deltaz = 0.1;
0103 TheZero = (2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz)).^0.5;
0104 c0(1,1,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0105
0106 fz = 1.2;
0107 deltaz = 0.05;
0108 TheZero = (2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz)).^0.5;
0109 c0(2,2,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0110
0111
0112 fz = 0.35;
0113 deltaz = 0.15;
0114 TheZero = (2*pi*sqrt(-1)*fz*(1 + sqrt(-1)*deltaz)).^0.5;
0115 c0(3,3,:) = fliplr(real(poly([TheZero,conj(TheZero)])));
0116
0117 fp1 = 0.25;
0118 deltap1 = 0.2;
0119 ThePole1 = (2*pi*sqrt(-1)*fp1*(1 + sqrt(-1)*deltap1)).^0.5;
0120 fp2 = 1;
0121 deltap2 = 0.05;
0122 ThePole2 = (2*pi*sqrt(-1)*fp2*(1 + sqrt(-1)*deltap2)).^0.5;
0123 d0 = real(poly([ThePole1,conj(ThePole1),ThePole2,conj(ThePole2)]));
0124 d0 = real(poly([ThePole2,conj(ThePole2)]));
0125 d0 = fliplr(d0);
0126
0127 end
0128
0129
0130 switch NoisePlane
0131
0132 case 'z',
0133 c0(1,2,:) = [0.1 -0.05 0.07]*2;
0134 c0(2,1,:) = c0(1,2,:);
0135 c0(2,1,end) = c0(2,1,end)*0.4;
0136
0137 case {'s','w'}
0138 c0(1,2,:) = 0.1*c0(3,3,:);
0139 c0(2,1,:) = 0.1*c0(1,1,:);
0140
0141 end
0142
0143
0144 nc = size(c0, 3) - 1;
0145 nd = length(d0) - 1;
0146
0147
0148
0149
0150
0151
0152 nmax = 3;
0153 switch PlantPlane
0154
0155 case 'z',
0156 rand('state',0);
0157 b0 = rand(nmax, nmax, 5);
0158
0159 [b110, a0] = cheby1(4, 3, 0.5);
0160
0161 case {'s','w'}
0162 rand('state',0);
0163 b0 = rand(nmax, nmax, 4);
0164
0165 [b110, a0] = cheby2(4, 40, 2*2*pi,'s');
0166 b110 = fliplr(b110(2:end));
0167 b110(2) = 5e-4;
0168 if PlantPlane == 'w'
0169 ThePoles = roots(a0);
0170 ThePoles = ThePoles.^0.5;
0171 a0 = poly(ThePoles);
0172 end
0173 a0 = fliplr(a0);
0174 b0(1,1,:) = 0.1*b110;
0175 b0(1,2,:) = b0(1,1,:);
0176 b0(1,3,:) = b0(1,2,:);
0177 b0(2,1,:) = b0(2,1,:);
0178 b0(2,2,:) = fliplr([a0(1) 0 2e-3 1])/100;
0179 b0(3,3,:) = fliplr([2e-3 a0(1) 0 1]);
0180 b0(3,2,:) = fliplr([0 5e-4 a0(1) 1]);
0181
0182 end
0183
0184 na = length(a0) - 1;
0185 nb = size(b0,3) - 1;
0186
0187
0188 if RecipPlant == 1
0189 for ll = 1:nb+1
0190 b0(:,:,ll) = (b0(:,:,ll) + b0(:,:,ll).')/2;
0191 end
0192 end
0193
0194
0195
0196
0197
0198
0199 [Sel, Theta0, ModelVar, IterVar] = MIMO_ML_DefaultValues(na, nb, nu, ny, PlantPlane);
0200
0201
0202
0203 ModelVar.Struct = 'OE';
0204
0205
0206 ModelVar.RecipPlant = RecipPlant;
0207
0208 fmin = 0.1;
0209 fmax = 2;
0210 Select = [ceil(fmin/(fs/N))+1:1:floor(fmax/(fs/N))+1].';
0211 freq = (Select-1)*fs/N;
0212 SelectAll = [1:1:N+1].';
0213 freqAll = ((SelectAll-1)/(2*N)/Ts);
0214
0215
0216 switch PlantPlane
0217 case 's', sAll = sqrt(-1)*2*pi*freqAll;
0218 case 'w', sAll = (sqrt(-1)*2*pi*freqAll).^0.5;
0219 end
0220 switch NoisePlane
0221 case 's', sAll = sqrt(-1)*2*pi*freqAll;
0222 case 'w', sAll = (sqrt(-1)*2*pi*freqAll).^0.5;
0223 end
0224
0225
0226
0227
0228
0229
0230 Theta0.A = a0;
0231 for ii = 1:ny
0232 for jj = 1:nu
0233 Theta0.B(ii,jj,:) = b0(ii,jj,:);
0234 end
0235 end
0236 Theta0.B = Theta0.B(1:ny,1:nu,:);
0237 Theta0.D = d0;
0238 for ii = 1:ny
0239 for jj = 1:ny
0240 Theta0.C(jj,ii,:) = c0(jj,ii,:);
0241 end
0242 end
0243 Theta0.C = Theta0.C(1:ny,1:ny,:);
0244
0245
0246 nig = 0;
0247 ModelVar.nig = nig;
0248 Sel.Ig = zeros(ny, nig+1);
0249 Theta0.Ig = zeros(ny, nig+1);
0250
0251
0252
0253
0254
0255
0256 switch ModelVar.PlantPlane
0257
0258 case 'z',
0259 u = randn(nu, N);
0260 y0 = zeros(ny, N);
0261 for jj=1:ny
0262 for ii=1:nu
0263 y0(jj,:) = y0(jj,:) + filter(squeeze(b0(jj,ii,:)), a0, u(ii,:));
0264 end
0265 end
0266
0267 case {'s','w'}
0268 u = randn(nu, 2*N);
0269 U = fft(u,[],2);
0270 Y0 = zeros(ny, length(SelectAll));
0271 for jj=1:ny
0272 for ii=1:nu
0273 Y0(jj,:) = Y0(jj,:) + ((polyval(fliplr(squeeze(b0(jj,ii,:)).'),sAll)./polyval(fliplr(a0),sAll)).').*U(ii,SelectAll);
0274 end
0275 end
0276 y0 = 2*real(ifft([zeros(ny,1),Y0(:,2:end-1),zeros(ny,N)],[],2));
0277 u = u(:,1:N);
0278 y0 = y0(:,1:N);
0279
0280 end
0281
0282
0283
0284
0285
0286
0287 Te = fliplr(hankel(ones(ny,1)));
0288
0289 switch NoisePlane
0290
0291 case 'z',
0292 e = stde*randn(ny, N);
0293 v = zeros(ny, N);
0294 e = Te*e;
0295 for jj=1:ny
0296 for ii=1:ny
0297 v(jj,:) = v(jj,:) + filter(squeeze(c0(jj,ii,:)),d0,e(ii,:));
0298 end
0299 end
0300
0301 case {'s', 'w'}
0302 e = stde*randn(ny, 2*N);
0303 e = Te*e;
0304 E = fft(e,[],2);
0305 V = zeros(ny, length(SelectAll));
0306 for jj=1:ny
0307 for ii=1:ny
0308 V(jj,:) = V(jj,:) + ((polyval(fliplr(squeeze(c0(jj,ii,:)).'),sAll)./polyval(fliplr(d0),sAll)).').*E(ii,SelectAll);
0309 end
0310 end
0311 v = 2*real(ifft([zeros(ny,1),V(:,2:end-1),zeros(ny,N)],[],2));
0312 v = v(:,1:N);
0313
0314 end
0315
0316
0317
0318
0319
0320
0321
0322
0323 data.y = y0 + v;
0324 data.u = u;
0325 data.Ts = 1/fs;
0326
0327
0328 method.dof = 10;
0329 method.startfreq = fmin;
0330 method.stopfreq = fmax;
0331
0332 [CZ, Z, freq, G, CvecG, dof, CL] = ArbLocalPolyAnal(data, method);
0333
0334
0335
0336
0337
0338
0339 F = length(freq);
0340 data_fit.Y = Z.m_nt(1:ny,:);
0341 data_fit.U = Z.m_nt(ny+1:end,:);
0342 data_fit.freq = freq;
0343 data_fit.Ts = Ts;
0344 data_fit.CY = CZ.m_nt(1:ny,1:ny,:);
0345 data_fit.CU = zeros(nu,nu,F);
0346 data_fit.CYU = zeros(ny,nu,F);
0347
0348
0349
0350
0351
0352
0353
0354 data_fit.W = data_fit.CY;
0355 [ThetaWTLS, smax, smin, wscale] = MIMO_WTLS(data_fit, Sel, ModelVar);
0356 [ThetaIQML, CostIQML, smax, smin, wscale] = MIMO_IQML(data_fit, Sel, ThetaWTLS, ModelVar, IterVar);
0357
0358
0359 [ThetaML, CostML, smax, smin, wscale] = MIMO_ML(data_fit, Sel, ThetaIQML, ModelVar, IterVar);
0360
0361
0362
0363
0364
0365
0366 data_fit.dof = dof;
0367 data_fit.CY = CZ.n(1:ny,1:ny,:);
0368
0369
0370
0371 [CRbound, GML, ThetaML, CovThetan, Thetan, Seln, wscale, TheCond] = MIMO_ML_CRbound(data_fit, Sel, ThetaML, ModelVar);
0372
0373
0374
0375
0376
0377
0378
0379 switch PlantPlane
0380 case 'z', x.Plant = exp(-sqrt(-1)*2*pi*freq.'*Ts);
0381 case 's', x.Plant = sqrt(-1)*2*pi*freq.';
0382 case 'w', x.Plant = (sqrt(-1)*2*pi*freq.').^0.5;
0383 end
0384 switch NoisePlane
0385 case 'z', x.Noise = exp(-sqrt(-1)*2*pi*freq.'*Ts);
0386 case 's', x.Noise = sqrt(-1)*2*pi*freq.';
0387 case 'w', x.Noise = (sqrt(-1)*2*pi*freq.').^0.5;
0388 end
0389
0390
0391 PolyTrans0 = MIMO_ML_CalcPolyTrans(Theta0, x);
0392
0393
0394 H0 = zeros(ny, ny, F);
0395 D0 = polyval(fliplr(Theta0.D), x.Noise.');
0396 for ii = 1:ny
0397 for jj = 1:ny
0398 H0(ii, jj, :) = polyval(fliplr(squeeze(Theta0.C(ii, jj, :)).'), x.Noise.');
0399 end
0400 end
0401 dummy(1,1,:) = D0;
0402 H0 = H0./repmat(dummy, ny, ny);
0403
0404
0405 CovE = stde^2*Te*Te.';
0406 S0 = zeros(ny, ny, F);
0407 for kk = 1:F
0408 S0(:,:,kk) = H0(:,:,kk) * CovE * H0(:,:,kk)';
0409
0410 S0(:,:,kk) = S0(:,:,kk) - diag(sqrt(-1)*imag(diag(S0(:,:,kk))));
0411 end
0412
0413
0414
0415
0416
0417
0418
0419
0420 [CovPoles0, Poles0] = CovRoots(Theta0.A, eye(na+1), Sel.A, PlantPlane, Ts);
0421
0422
0423
0424
0425
0426
0427
0428 CYn = CZ.n(1:ny,1:ny,:);
0429 FigNum = 1;
0430 figure(FigNum)
0431 clf
0432 mm = 0;
0433 for jj = 1:ny
0434 for ii = 1:ny
0435 mm = mm+1;
0436 subplot(ny, ny, mm)
0437 plot(freq, db(squeeze(S0(jj,ii,:)))/2, 'k', freq, db(squeeze(CYn(jj,ii,:)))/2, 'r');
0438 end
0439 end
0440 subplot(ny, ny, ceil(ny/2))
0441 title('True noise cov.: black; Nonparametric estimate: red');
0442 zoom on; shg
0443
0444
0445 FigNum = FigNum+1;
0446 figure(FigNum)
0447 mm = 0;
0448 for jj = 1:ny
0449 for ii = 1:nu
0450 mm = mm+1;
0451 subplot(ny, nu, mm)
0452 plot(freq, db(squeeze(GML(jj,ii,:))), 'r', freq, db(squeeze(PolyTrans0.G(jj,ii,:))), 'k', ...
0453 freq, db(squeeze(PolyTrans0.G(jj,ii,:)-GML(jj,ii,:))), 'k--', ...
0454 freq, db(squeeze(CRbound.G(jj,ii,:)))/2, 'r--');
0455 end
0456 end
0457 subplot(ny, nu, ceil(nu/2))
0458 title('G_0: black; G: red; |G-G_0|: black --; var(G): red --');
0459 zoom on; shg
0460
0461
0462 [Auto_Corr0, Lags0, Conf_Bound0, Fraction0] = WhitenessTestResiduals(G, CvecG, GML, CL, dof);
0463 if nu*ny == 1
0464 Fraction0m = squeeze(Fraction0);
0465 elseif ny == 1
0466 Fraction0m = squeeze(mean(Fraction0, 2));
0467 elseif nu == 1
0468 Fraction0m = squeeze(mean(Fraction0, 1));
0469 else
0470 Fraction0m = mean(mean(Fraction0));
0471 end
0472 disp(['mean fraction outside 50% and 95% confidence bounds: ',num2str(Fraction0m(1)),', and ',num2str(Fraction0m(2))]);
0473
0474 F=length(freq);
0475 ntheta = sum(Sel.A)+sum(sum(sum(Sel.B))+sum(sum(Sel.Ig))) - 1;
0476 Cost0 = dof/(dof-ny)*ny*(F-ntheta/2);
0477 stdCost0 = sqrt(3*(dof)^3*ny/(dof-ny)^2/(dof-ny-1)*(F-ntheta/2));
0478 disp(['Expected value true cost, std true cost, and actual value cost: ',num2str(Cost0),', ',num2str(stdCost0),', and ',num2str(CostML)]);
0479
0480
0481 disp('Estim. f0 [Hz], std(f0) [Hz], estim. - true [Hz]')
0482 [ThetaML.poles.freq, CRbound.poles.freq.^0.5, ThetaML.poles.freq-Poles0.freq]
0483
0484
0485 disp('Estim. damping, std(damping), estim. - true')
0486 [ThetaML.poles.damp, CRbound.poles.damp.^0.5, ThetaML.poles.damp-Poles0.damp]
0487