From 8b518db8a4b21c449b4b47c4637dbbc1d01fa0aa Mon Sep 17 00:00:00 2001 From: Lucas Antonio Pelike Date: Tue, 2 Jul 2024 15:04:05 -0300 Subject: [PATCH] Add script for simulating the experimental sigma method The script simulates the experimental method we're using for replicating the results got from MATLAB's sigma [1] command. Co-authored-by: Guilherme Ricioli [1] https://www.mathworks.com/help/control/ref/dynamicsystem.sigma.html --- exp_sigma_sim.m | 210 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 210 insertions(+) create mode 100644 exp_sigma_sim.m diff --git a/exp_sigma_sim.m b/exp_sigma_sim.m new file mode 100644 index 0000000..324c4b5 --- /dev/null +++ b/exp_sigma_sim.m @@ -0,0 +1,210 @@ +% Copyright (C) 2024 CNPEM (cnpem.br) +% Author: Lucas Pelike +% Modified by: Guilherme Ricioli + +clc; clear; +addpath 'machine/SIRIUS/' + +respmat_fpath = 'respmat.mat'; +sysid_res_fpath = 'sysid_res.mat'; + +%% Simulation parameters + +% PRBS +prbs_amplitude = 4000; +prbs_lfsr_len = 9; +prbs_step_duration = 4; +prbs_period = (2^prbs_lfsr_len - 1)*prbs_step_duration; +n_prbs_periods = 2; +assert(n_prbs_periods > 1); % 1st period is considered transient + +% Orbit +orbit_rms_noise = 300; % in nm + +% System identification type ('Sensitivity' or 'Open Loop') +sysid_type = 'Sensitivity'; + +%% Building FOFB model +fprintf('Building FOFB model...\n'); + +M = load(respmat_fpath).mat_d'; +A = load(sysid_res_fpath).sys; + +excluded_corr = [1 80 81 160]; + +Ts = A{2}.Ts; + +tic +for i=1:size(A,1) + if ismember(i,excluded_corr) + A{i} = tf(0,1,'Ts',Ts); + else + A{i} = idtf(A{i}/dcgain(idtf(A{i})),'Ts',Ts); + end +end + +fofb_type.bpm_sector = 'M1M2C2C3'; +fofb_type.corr_sector = 'M1M2C2C3'; +fofb_type.bpm_remove_idx = []; +fofb_type.corr_remove_idx = excluded_corr; +[bpm_idx,corr_idx] = fofb_idx(fofb_type); + +M = M(bpm_idx,corr_idx); +Mc = pinv(M); +K = (1/Ts)*[0.12*ones(size(Mc,1)/2,1); 0.166*ones(size(Mc,1)/2,1)]; +A = A(corr_idx); +[P,G,C] = ofbmdl(M,Mc,K,A); + +fprintf('Elapsed time: %.2f s\n',toc); + +%% Building PRBS input cube +fprintf('Building PRBS input cube...\n'); + +prbs_frest = frest.PRBS('Amplitude',prbs_amplitude,... + 'Ts',Ts,... + 'Order',prbs_lfsr_len,... + 'NumPeriods',n_prbs_periods,... + 'UseWindow','off'); +prbs_ts = generateTimeseries(prbs_frest); +prbs = repelem(prbs_ts.Data,prbs_step_duration); +prbs = prbs - mean(prbs); + +T=0:Ts:(length(prbs) - 1)*Ts; + +% Defining U, V, u and y +[U,S,V] = svd(M); + +n_modes = min(size(U,1),size(V,1)); +n = size(U,1); +y = zeros(length(T),n,n_modes); +if strcmp(sysid_type,'Sensitivity') + sys = P('yd',{'d','n'}); + m = n; + u = zeros(length(T),m,n_modes); + for i=1:n_modes + u(:,:,i) = U(:,i)'.*prbs; + end +elseif strcmp(sysid_type,'Open Loop') + sys = G; + m = size(V,1); + u = zeros(length(T),m,n_modes); + for i=1:n_modes + u(:,:,i) = V(:,i)'.*prbs; + end +end + +fprintf('Elapsed time: %.2f s\n',toc); + +%% Simulating system +fprintf('Simulating system...\n'); + +freqs = rfftfreq(prbs_period, Ts); + +Yu = ones(length(freqs),m,n_modes); +Yy = ones(length(freqs),n,n_modes); + +snr = ones(m,n_modes); + +% Iterates for each excited mode +for k=1:n_modes + fprintf('Mode: %d\n',k); + + if strcmp(sysid_type,'Sensitivity') + noise = orbit_rms_noise.*randn(size(y,1),size(y,2)); + y(:,:,k) = lsim(sys,[u(:,:,k) noise],T); + elseif strcmp(sysid_type,'Open Loop') + y(:,:,k) = lsim(sys,u(:,:,k),T); + noise = orbit_rms_noise.*randn(size(y,1),1); + y(:,i,k) = y(:,i,k) + noise; + end + + % Iterates in each input/output + for i=1:n + % Building Yu arrays + if(i<=m) + u_avg = reshape(u(:,i,k),prbs_period,n_prbs_periods); + % 1st PRBS period is considered transient + u_avg = mean(u_avg(:,2:end),2); + Yu(:,i,k) = rfft(u_avg); + end + + % Building Yy arrays + y_avg = reshape(y(:,i,k),prbs_period,n_prbs_periods); + % 1st PRBS period is considered transient + y_avg = mean(y_avg(:,2:end),2); + Yy(:,i,k) = rfft(y_avg); + + % Calculating SNR + % if strcmp(sysid_type,'Open Loop') + % snr(i,k) = 20*log10(rms(y(:,i,k))/rms(noise)); + % elseif strcmp(sysid_type,'Sensitivity') + % snr(i,k) = 20*log10(rms(y(:,i,k))/rms(noise(:,i))); + % end + end +end + +fprintf('Elapsed time: %.2f s\n',toc); + +% Clear unused variables to free up space +clear U V u y prbs T sys; + +%% Calculating Response Matrices and their SVDs +fprintf('Calculating Response Matrices and their SVDs...\n'); + +respmat = zeros(n,m,length(freqs)); +exp_sigma = zeros(n,m,length(freqs)); + +for f=1:length(freqs) + respmat(:,:,f) = squeeze(Yy(f,:,:))*pinv(squeeze(Yu(f,:,:))); + [~,exp_sigma(:,:,f),~] = svd(squeeze(respmat(:,:,f))); +end + +fprintf('Elapsed time: %.2f s\n',toc); + +%% Plotting results +fprintf('Plotting results...\n'); + +if strcmp(sysid_type,'Sensitivity') + sys = P('yd','d'); +elseif strcmp(sysid_type,'Open Loop') + sys = G; +end + +figure; +for k=1:n_modes + % We removed DC from PRBS and orbit signals, so start from frequency + % index 2 + semilogx(freqs(2:end), ... + 20*log10(squeeze(exp_sigma(k,k,2:end))),'Color','#D95319'); + hold on; +end +legend({'Simulated'}) +h = sigmaplot(sys); +plotoptions = sigmaoptions; +plotoptions.FreqUnits = 'Hz'; +plotoptions.Grid = 'on'; +setoptions(h,plotoptions); + +fprintf('Elapsed time: %.2f s\n',toc); + +%% Savings results +fprintf('Savings results...\n'); + +save(['exp_sigma_sim-', sysid_type], 'freqs', 'respmat', 'exp_sigma'); + +fprintf('Elapsed time: %.2f s\n',toc); +fprintf('done!\n'); + +%% Function definitions +function f = rfftfreq(L, Ts) + Fs = 1/Ts; + n = idivide(int32(L),int32(2),'ceil'); + f = (Fs/L)*(0:double(n) - 1); +end + +function Y = rfft(X) + L = length(X); + n = idivide(int32(L),int32(2),'ceil'); + Y = fft(X)/L; + Y = Y(1:n); +end