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