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