function TripPend_GaussNewton_fit_v3
% This function uses a Gauss-Newton Method to fit the physical parameters
% of the triple models to measured longitudinal and pitch data. Each
% Gauss-Newton iteration uses a linesearch algorithm to optimize the step
% size for the search direction for that iteration. This function must be
% put in the same directory where the model is compiled, i.e. where the
% ssmake... function lives. It uses a built in version of ssmake... and
% calls the same functions.
%
% What you must input as the user:
% 1) Model parameter file: e.g. 'hstsopt_metal', in the 'Fitting routine
% parameters and files' section
% 2) The top mass transfer fuction svn data path. This also is in the
% 'Fitting routine parameters and files' section.
% 3) The measured longitudinal and pitch mode frequencies in the 'Measured
% Data' section. These mode frequencies should be determined from the
% transfer function data for consistency.
% 4) The indices of the desired parameters to float in the fit. These are
% listed in the 'param_index' vector in the 'Parameter Initializations'
% section.
%
% The data used to make the fit is just the measured frequencies of the 6
% longitudinal and pitch modes. Future versions of the code can include in
% principle any kind of data.
%
% Along with the measured data, the code also asks for the estimated
% standard deviation of the measurements, e.g. the error bars for the
% resonance frequencies, etc. This data is important because it is used to
% calculate the minimum error bars on the adjusted model parameters. The
% parameter error bars are calculated using the Fisher Information Matrix,
% which by definition predicts only lower limits on these error bars, so
% the actual error bars could be larger. The estimated standard deviation
% here is just the frequency resolution of the measured data, assuming
% linear frequency spacing.
%
% The output model is a state space variable called pen_mod. The adjusted
% parameters are collected in a vector called param. The list of which
% element of the vector param is which model param is in the
% 'Parameter Initializations' section of the code below. All the relevant
% output data is saved to a filename of your choice in a folder called 'Fit
% Files'.
%
% A summary of the adjusted model parameters is outputted to the command
% line. A series of before and after plots of model transfer functions
% against the measured transfer functions are plotted. A final plot with
% the evolution of the model error is plotted as well.
%
% The code interfaces with the model through the function
% 'ssmake3MBf_modelfitting.m'. The starting values
% for all the parameters come from the '...opt...' file. The success of
% this fitting code depends on the parameters in this file being as close
% as possible to the as-built values. You also have to provide the string
% of the name of the parameter file in the 'Fitting routine parameters and
% files' section. E.g. 'hstsopt_metal'.
%
% Algorithm stopping parameters
% Search.iter: sets the maximum number of Gauss-Newton iterations
% Search.gtol: sets the minimum error gradient magnitude
% Search.iter_LS_max: maximum number of line search iterations
% Search.c1 and Search.c2: Wolfe condition parameters for linesearch
% Search.alpha_max0: maximum step size for linesearch algorithm
%
% Choosing the parameters to fit is done with the vector 'param_index'.
% A list of the number for each parameter is given in the code.
% Edit the values of param_index in the 'Parameter Initialization' section
% to choose the desired parameters. In this version, the parameters
% available to adjust are the d's, rotational inertias, wire lengths,
% wire longitudinal spacings, wire radii, masses, and vertical spring stiffnesses:
% pend.d0; %1 param(1)
% pend.d1; %2 param(2)
% pend.d2; %3 param(3)
% pend.d3; %4 param(4)
% pend.d4; %5 param(5)
% pend.I1x; %6 param(6)
% pend.I2x; %7 param(7)
% pend.I3x; %8 param(8)
% pend.I1y; %9 param(9)
% pend.I2y; %10 param(10)
% pend.I3y; %11 param(11)
% pend.I1z; %12 param(12)
% pend.I2z; %13 param(13)
% pend.I3z; %14 param(14)
% pend.l1; %15 param(15)
% pend.l2; %16 param(16)
% pend.l3; %17 param(17)
% pend.si; %18 param(18)
% pend.sl; %19 param(19)
% pend.r1; %20 param(20)
% pend.r2; %21 param(21)
% pend.r3; %22 param(22)
% pend.m1; %23 param(23)
% pend.m2; %24 param(24)
% pend.m3; %25 param(25)
% pend.kc1; %26 param(26)
% pend.kc2; %27 param(27)
%
% Brett Shapiro
% Version 1
% Original version.
% 27 September 2012
%
% Brett Shapiro
% Version 2
% Fixed typos in the comments above. Fixed errors in the parameter change
% report printed to the command window. The +- percent errors on the fit
% parameters were calculated incorrectly.
%
% Brett Shapiro
% Version 3
% General cleanup of code. Added more mode frequencies. Now includes all of
% them except the vertical and roll bounce modes.
% 26 September 2013
clc
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fitting routine parameters and files
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Path where transfer function data is stored
svnDir.sus = '/ligo/svncommon/SusSVN/sus/trunk/';
toolsDir = [svnDir.sus 'Common/MatlabTools/'];
modelDir = [svnDir.sus 'Common/MatlabTools/TripleModel_Production/'];
addpath(toolsDir,modelDir)
% M1toM1data_filename = 'HSTS/L1/MC2/SAGM1/Results/2013-06-17_1055564583_L1SUSMC2_M1_damp_OFF_MATTF';
% M1toM1data_filename = 'HSTS/L1/PRM/SAGM1/Results/2013-06-04_1054434416_L1SUSPRM_M1_damp_OFF_MATTF';
M1toM1data_filename = 'HSTS/H1/MC2/SAGM1/Results/2013-05-25_1053525085_H1SUSMC2_M1_damp_OFF_MATTF';
save_filename = 'L1MC2_L_GuassNewtonFit_25Sept2013'; % save the results of this run to this filename
% model parameter file name
modparamfilename = 'hstsopt_metal_withspringfits';
% model compilation code filename
modcompcodename = 'ssmake3MBf_modelfitting';
% Model compile string
modcompstring = ['mbtrip = ',modcompcodename,'(fitparam,''',modparamfilename,''');'];
% stopping conditions: The fit will run until either Search.iter or Search.gtol is satisfied first.
Search.iter = 50; % Maximum number of iterations in this run.
Search.g_tol = 0.001; % min tolerance on error gradient to keep running the routine. The routine stops when the magnitude of the gradient drops below this value
% line search parameters for Gauss-Newton algorithm
Search.c1 = 0;%0.0001;
Search.c2 = 1;%0.9; % Search.c1 and Searcg.c2 are the Wolfe condition constants
Search.iter_LS_max = 100; % Limit on number of iterations of the line search
Search.alpha_max0 = 50; % initial step length
% Model indices
[~,in,out,~] = generate_Triple_Model_Production(0.1:0.001:10,'hstsopt_metal'); % the inputs here are just dummies, only want the model input and output indices
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Measured Data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Measured mode frequencies
% measured long-pitch
meas.modes.free.LP.modefreqs = 2*pi*sort([0.67;...
1.52;...
2.76;...
1.02;...
3.10;...
3.63]);
% measured vert modes
meas.modes.free.V.modefreqs = 2*pi*sort([0.89;...
2.70]);
% measured top to top tf vert zero
% meas.modes.toplocked.V.modefreqs = 2*pi*1.40;
% measured yaw modes
meas.modes.free.Y.modefreqs = 2*pi*sort([1.1;...
2.08;
3.46]);
% measured trans-roll
meas.modes.free.TR.modefreqs = 2*pi*sort([0.67;...
1.52;...
2.93;...
1.51;...
2.27]);
% measured long mode when cavity is locked with bot and mid using high gain at mid
meas.modes.cavmidbot.LP.modefreqs = 2*pi*2.21094;
% meas.modes.all.modefreqs = [meas.modes.free.LP.modefreqs;meas.modes.free.V.modefreqs;meas.modes.toplocked.V.modefreqs;meas.modes.free.Y.modefreqs;meas.modes.free.TR.modefreqs]; % all measured modes
meas.modes.all.modefreqs = [meas.modes.free.LP.modefreqs;meas.modes.free.V.modefreqs;meas.modes.free.Y.modefreqs;meas.modes.free.TR.modefreqs;meas.modes.cavmidbot.LP.modefreqs]; % all measured modes
% meas.modes.all.modefreqs = [meas.modes.cavmidbot.LP.modefreqs]; % all measured modes
% The measured gain of the triple top mass electronics
egain = 1;%34.1086;
assignin('base','egain',egain)
% measured Transfer Function data
M1toM1TFdata = load([svnDir.sus,M1toM1data_filename]);
% M1 longitudinal to M1 longitudinal high frequency magnitude to help M1 mass estimation
meas.mag.M1toM1.L2L = [];
% low_freq = 7;
% high_freq = 9;
% low_freq_ind = indfind(M1toM1TFdata.meas.freq,low_freq);
% high_freq_ind = indfind(M1toM1TFdata.meas.freq,high_freq);
% meas.mag.M1toM1.L2L = sum(abs(M1toM1TFdata.meas.eul2eul(M1toM1TFdata.meas.out.osem.L-6,M1toM1TFdata.meas.in.L).f(low_freq_ind:high_freq_ind)))/length(M1toM1TFdata.meas.freq(low_freq_ind:high_freq_ind));
% frequency resolution to predict errors
meas.modes.free.freq_resolution = 2*pi*(M1toM1TFdata.meas.freq(3)-M1toM1TFdata.meas.freq(2)); % Assuming linear frequency spacing. Used to estimate the mode frequency measurement error, which then predicts the fit error.
meas.modes.toplocked.freq_resolution = 2*pi*(M1toM1TFdata.meas.freq(3)-M1toM1TFdata.meas.freq(2)); % Assuming linear frequency spacing. Used to estimate the mode frequency measurement error, which then predicts the fit error.
% magnitude percent error
meas.mag.err = [];
% meas.mag.err = 0.25;
% variance of measurement errors (order must match order in err_vec_calc)
meas.modes.all.err = meas.modes.free.freq_resolution./meas.modes.all.modefreqs;
meas.mag.all.err = meas.mag.err;
meas.err = [meas.modes.all.err;meas.mag.all.err];
Cmeas_var = meas.err*meas.err';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parameter Initializations %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%% Edit this vector using the list above to choose the desired parameter space for this fit
param_index = [2 6 7 9:11 23:25]; % selecting the parameters specific to the top mass
% param_index = 23; % selecting the parameters specific to the top mass
% Loading Original Model Parameters
eval(['pend = ',modparamfilename,';'])
% Defining all parameters of interest. The numbers are entered into
% parameter_index to choose the parameter space for the fit.
f_d0 = pend.d0; %1 param(1)
f_d1 = pend.d1; %2 param(2)
f_d2 = pend.d2; %3 param(3)
f_d3 = pend.d3; %4 param(4)
f_d4 = pend.d4; %5 param(5)
f_I1x = pend.I1x; %6 param(6)
f_I2x = pend.I2x; %7 param(7)
f_I3x = pend.I3x; %8 param(8)
f_I1y = pend.I1y; %9 param(9)
f_I2y = pend.I2y; %10 param(10)
f_I3y = pend.I3y; %11 param(11)
f_I1z = pend.I1z; %12 param(12)
f_I2z = pend.I2z; %13 param(13)
f_I3z = pend.I3z; %14 param(14)
f_l1 = pend.l1; %15 param(15)
f_l2 = pend.l2; %16 param(16)
f_l3 = pend.l3; %17 param(17)
f_si = pend.si; %18 param(18)
f_sl = pend.sl; %19 param(19)
f_r1 = pend.r1; %20 param(20)
f_r2 = pend.r2; %21 param(21)
f_r3 = pend.r3; %22 param(22)
f_m1 = pend.m1; %23 param(23)
f_m2 = pend.m2; %24 param(24)
f_m3 = pend.m3; %25 param(25)
f_kc1 = pend.kc1; %26 param(26)
f_kc2 = pend.kc2; %27 param(27)
param = [f_d0;f_d1;f_d2;f_d3;f_d4;f_I1x;f_I2x;f_I3x;f_I1y;f_I2y;f_I3y;f_I1z;f_I2z;f_I3z;f_l1;f_l2;f_l3;f_si;f_sl;f_r1;f_r2;f_r3;f_m1;f_m2;f_m3;f_kc1;f_kc2]; % parameters vector used by fitting code
fitparam0 = param; % fitparam is the parameter vector used by ssmake3MBf_modelfitting to recompile the model. fitparam0 is the before-the-fit version of this vector.
mbtrip = genmod(fitparam0,modcompstring);
err_vec1 = err_vec_calc(mbtrip,meas); %the error of the original model
err1 = err_vec1'*err_vec1;
param_index_length = length(param_index);
param_length = length(param);
d_param = (1e-9)*[ones(1,param_length-3) 0.1 0.1 0.1]; %step size used to calculate the error gradient vector. Must be small enough to represent the gradient at a given point, but large enough yield a junk gradient. The lower limit is d_param/param > eps =(approx) 1e-15.
grad_param = zeros(param_length,1); %error gradient vector
grad_param_k1 = grad_param; % linesearch error gradient
param_check = param; % used during linesearch to check if the error decreased
% There should always be more data points than variable model parameters.
% This is a necessary condition to finding a unique fit.
if param_index_length > length(err_vec1)
warning(['There are more parameters than data points, the solution will not be unique! # of parameters = ',num2str(param_index_length),', # of data points = ',num2str(length(err_vec1)),'.']) %#ok<WNTAG>
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Loop Initializations %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
err = zeros(1,Search.iter); % error vector
err(1) = err1; % initial error
disp(' '),disp(['1000*err(1) = ',num2str(1000*err1)]) % display initial error
ii = 1; % iterations of loop
pass = 0; % loop flag
% finding the intial error gradient
for jj = 1:param_index_length
param_test = param(param_index(jj)) + d_param(param_index(jj));
% Testing single parameter change
fitparam = param_conv(param,param_test,param_index(jj)); % assignin('base','fitparam',fitparam)
pen_mod_test = genmod(fitparam,modcompstring);
err_vec_test = err_vec_calc(pen_mod_test,meas);
err_test = err_vec_test'*err_vec_test;
grad_param(param_index(jj)) = (err_test - err(1))/d_param(param_index(jj));
end
gradmag = sqrt(grad_param'*grad_param);
disp([' |gradient| = ',num2str(gradmag)]),drawnow
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Gauss-Newton Algortithm Loop %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while ii < Search.iter && gradmag > Search.g_tol && pass < 2;
ii = ii+1;
% calculating the Jacobian matrix for this point in the parameter space
fitparam_vec = param_conv(param,0,0); % assignin('base','fitparam',fitparam)
pen_mod_vec = genmod(fitparam_vec,modcompstring);
err_vec = err_vec_calc(pen_mod_vec,meas);
Jacobian = zeros(length(err_vec),param_index_length);
for jj = 1:param_index_length
param_test = param(param_index(jj)) + d_param(param_index(jj));
% Testing single parameter change
fitparam_vec_test = param_conv(param,param_test,param_index(jj)); % assignin('base','fitparam',fitparam)
pen_mod_vec_test = genmod(fitparam_vec_test,modcompstring);
err_vec_test = err_vec_calc(pen_mod_vec_test,meas);
Jacobian(:,jj) = (err_vec_test - err_vec)/d_param(param_index(jj));
end
sval = svd(Jacobian);
if max(sval)/min(sval) > 1e5 % 1e5 is an arbitrary user defined value
% This means that the parameter space has lost strict convexity due
% to insuffient data points that are coupled to the chosen model
% parameters. A nonunique solution is possible. Examing the singular
% value decomposition of the Jacobian matrix will quantify how
% important the measurements are to the model parameters.
warning(['Jacobian poorly scaled, max/min singular value ratio at ',num2str(max(sval)/min(sval)),'. Good ratios are close to 1.']) %#ok<WNTAG>
end
% Gauss-Newton algorithm
% Step 1: find search direction
dk = -2*pinv(Jacobian)*err_vec;
% Step 2: line search
alpha_min = 0; % lower limit of alpha bracket
alpha_max = Search.alpha_max0; % upper limit of alpha bracket
alpha = (alpha_min+alpha_max)/2;
iter_LS = 0; % Count of the number of iterations of the line search
pass = 0; % Termination flag, pass == 1 means Wolfe conditions satisfied
while pass == 0
% Limit the number of iterations in this loop.
iter_LS = iter_LS + 1;
if iter_LS > Search.iter_LS_max
disp(['alpha_min = ',num2str(alpha_min)])
disp(['alpha_max = ',num2str(alpha_max)])
disp(['alpha = ',num2str(alpha)])
disp(['err_k1 = ',num2str(err_k1)])
disp(['sqrt(grad_param''*grad_param) = ',num2str(sqrt(grad_param'*grad_param))])
% error(['Maximum iterations of ',num2str(Search.iter_LS_max),' reached.'])
pass = 2;
warning(['Maximum line search iterations of ',num2str(Search.iter_LS_max),' reached.']) %#ok<WNTAG>
break
end
% Bisection iteration
alpha = (alpha_min+alpha_max)/2; % testing the middle point in the bracket
param_check(param_index) = param(param_index) + alpha*dk; % potential new position
% function and slope values at new point: [fk1 gk1] = f(xk1);
fitparam = param_conv(param_check,0,0);
pen_mod = genmod(fitparam,modcompstring);
err_veck1 = err_vec_calc(pen_mod,meas);
err_k1 = err_veck1'*err_veck1;
% handling cases where error goes complex do to instabilites in model from overly large steps
infinite_loop_flag = 0;
while abs(imag(err_k1)) > 0
alpha = alpha/2;
param_check(param_index) = param(param_index) + alpha*dk; % potential new position
fitparam = param_conv(param_check,0,0);
pen_mod = genmod(fitparam,modcompstring);
err_veck1 = err_vec_calc(pen_mod,meas);
err_k1 = err_veck1'*err_veck1;
infinite_loop_flag = infinite_loop_flag + 1;
if infinite_loop_flag > 100 % stopping run and spitting out useful information
disp(['err_k1 = ',num2str(err_k1)])
disp(['alpha_min = ',num2str(alpha_min)])
disp(['alpha_max = ',num2str(alpha_max)])
disp(['alpha = ',num2str(alpha)])
disp(['iter_LS = ',num2str(iter_LS)])
disp(['fitparam = ',num2str(fitparam)])
disp(['param = ',num2str(param')])
disp(['param_check = ',num2str(param_check')])
disp(['dk = ',num2str(dk')])
assignin('base','pen_mod',pen_mod)
error('Infinite loop during complex error check')
end
end
for jj = 1:param_index_length
param_test = param_check(param_index(jj)) + d_param(param_index(jj));
% Testing single parameter change
fitparam = param_conv(param_check,param_test,param_index(jj));
pen_mod_test = genmod(fitparam,modcompstring);
err_vec_test = err_vec_calc(pen_mod_test,meas);
err_test = err_vec_test'*err_vec_test;
grad_param_k1(param_index(jj)) = (err_test - err_k1)/d_param(param_index(jj));
end
if err_k1 <= err(ii-1) + Search.c1*alpha*dk'*grad_param(param_index) && err_k1 <= err(ii-1) % Armijo rule: makes sure function is reduced
curve_left = dk'*grad_param_k1(param_index);
curve_right = Search.c2*dk'*grad_param(param_index);
if abs(curve_left) <= abs(curve_right) % Curvature condition: ensures significant reduction in slope
pass = 1; % end loop, Wolfe conditions have been met
else
if curve_left > curve_right % Curvature failure: the left side of the inequality is greater, so we stepped too far, so set a new upper limit with this point.
alpha_max = alpha;
else % Curvature failure: the right side of the inequality is greater, so did not step far enough, so set a new lower limit with this point.
alpha_min = alpha;
end
end
else
alpha_max = alpha; % Armijo failure: sets alpha as the new upperlimit of the bracket
end
end
% Step 3: Replace old data with new
param(param_index) = param_check(param_index);
err(ii) = err_k1;
grad_param = grad_param_k1;
gradmag = sqrt(grad_param'*grad_param);
% Display Progress
disp(['1000*err(',num2str(ii),') = ',num2str(1000*err(ii))]), drawnow
disp([' |gradient| = ',num2str(gradmag),', alpha = ',num2str(alpha),', iter_LS = ',num2str(iter_LS)]), drawnow
% Display reason for terminating optimiztion
if ii == Search.iter
disp(' '),disp(['Maximum number of ',num2str(Search.iter),' iterations reached'])
end
if gradmag <= Search.g_tol
disp(' '),disp(['Gradient magnitude reached desired tolerance of ',num2str(Search.g_tol)])
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculating final values and predicting the fit error %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculating the Jacobian matrix for the terminal point in the paramter space
fitparam = param_conv(param,0,0); % assignin('base','fitparam',fitparam)
pen_mod = genmod(fitparam,modcompstring);
err_vec = err_vec_calc(pen_mod,meas);
Jacobian = zeros(length(err_vec),param_index_length);
for jj = 1:param_index_length
param_test = param(param_index(jj)) + d_param(param_index(jj));
% Testing single parameter change
fitparam_vec_test = param_conv(param,param_test,param_index(jj)); % assignin('base','fitparam',fitparam)
pen_mod_vec_test = genmod(fitparam_vec_test,modcompstring);
err_vec_test = err_vec_calc(pen_mod_vec_test,meas);
Jacobian(:,jj) = (err_vec_test - err_vec)/d_param(param_index(jj));
end
% Estimated fit errors. These come from the fisher information matrix.
% The fisher information matrix gives only a LOWER LIMIT on the fit error!!!!
% It is not clear to me (Brett) how much below the true error this tends to be.
pinvJ = pinv(Jacobian);
param_err_raw = sqrt(diag(pinvJ*Cmeas_var*pinvJ'));
param_err = ones(param_length,1); count = 0;
for jj = 1:param_length % padding the unfit parameters with zeros
if length(find(param_index-jj)) < param_index_length
count = count + 1;
param_err(jj) = param_err_raw(count);
else
param_err(jj) = 0;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Results %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% saving useful values
save(['Fit_Files/',save_filename],'pend','d_param','err','fitparam0','fitparam','param','param_index','param_index_length','pen_mod','Search','param_length','grad_param','Jacobian','pinvJ','Cmeas_var','param_err','meas','M1toM1TFdata')
assignin('base','pend',pend) % Original model structure
assignin('base','d_param',d_param) % finite difference parameter increment
assignin('base','err',err) % err values at each Gauss-Newton iteration
assignin('base','fitparam0',fitparam0) % original model parameter list used by the model compilation code
assignin('base','fitparam',fitparam) % new model parameter list used by the model compilation code
assignin('base','param',param) % new model parameter list used by the fitting routine
assignin('base','param_index',param_index) % index of the chosen parameters for this fit
assignin('base','param_index_length',param_index_length) % number of parameters chosen for this fit
assignin('base','pen_mod',pen_mod) % new model from this fit
assignin('base','Search',Search) % Gauss-Newton search parameters
assignin('base','param_length',param_length) % length of the full parameter vector (param_index choses which of these to fit)
assignin('base','grad_param',grad_param) % error gradient with respect to the fitting model parameters
assignin('base','Jacobian',Jacobian) % Jacobian matrix linking changes in model parameters to changes in the error residuals
assignin('base','pinvJ',pinvJ) % Moore-Penrose pseudoinverse of the Jacobian matrix
assignin('base','Cmeas_var',Cmeas_var) % Variance of measurements
assignin('base','param_err',param_err) % RMS of parameter error after running the fit
assignin('base','meas',meas) % measured data used in this fit
assignin('base','M1toM1TFdata',M1toM1TFdata) % M1 to M1 measured transfer function data
% if ii < Search.iter && pass < 2% if Search.gtol triggers end before max iteration limit
% disp(['sqrt(grad_param''*grad_param) = ',num2str(sqrt(grad_param'*grad_param)),', Search.gtol = ',num2str(Search.gtol)])
% end
% Plotting fit against measurements
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.L,in.m1.drive.L)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.T,in.m1.drive.T)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.V,in.m1.drive.V)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.Y,in.m1.drive.Y)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.P,in.m1.drive.P)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.R,in.m1.drive.R)
% model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.P,in.m1.drive.L)
model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.R,in.m1.drive.T)
% model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.L,in.m1.drive.P)
% model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.R,in.m1.drive.V)
% model_comparison(mbtrip,pen_mod,M1toM1TFdata,out.m1.disp.V,in.m1.drive.R)
% Plotting Error Evolution
figure,maxfig
semilogy(1:ii,1000*err(1:ii),'Linewidth',3),grid on
set(gca,'FontSize',25)
title(['1000*Err(1) = ',num2str(1000*err(1)),', 1000*Err(',num2str(ii),') = ',num2str(1000*err(ii)),'.'])
ylabel('1000 \times Error Magnitude'),xlabel('Iteration Number')
% Displaying the change in parameters from the model parameter file '...opt...'
disp(' '),disp(' ')
disp('Change in parameters from original model with the estimated minimum errors')
disp('---------------------------------------')
disp(['d0: ',num2str(1000*(param(1)-f_d0)),' +- ',num2str(1000*param_err(1)),' mm'])
disp(['d1: ',num2str(1000*(param(2)-f_d1)),' +- ',num2str(1000*param_err(2)),' mm'])
disp(['d2: ',num2str(1000*(param(3)-f_d2)),' +- ',num2str(1000*param_err(3)),' mm'])
disp(['d3: ',num2str(1000*(param(4)-f_d3)),' +- ',num2str(1000*param_err(4)),' mm'])
disp(['d4: ',num2str(1000*(param(5)-f_d4)),' +- ',num2str(1000*param_err(5)),' mm'])
disp(['I1x: ',num2str(100*(param(6)/f_I1x - 1)),' +- ',num2str(100*param_err(6)/f_I1x),' %'])
disp(['I2x: ',num2str(100*(param(7)/f_I2x - 1)),' +- ',num2str(100*param_err(7)/f_I2x),' %'])
disp(['I3x: ',num2str(100*(param(8)/f_I3x - 1)),' +- ',num2str(100*param_err(8)/f_I3x),' %'])
disp(['I1y: ',num2str(100*(param(9)/f_I1y - 1)),' +- ',num2str(100*param_err(9)/f_I1y),' %'])
disp(['I2y: ',num2str(100*(param(10)/f_I2y - 1)),' +- ',num2str(100*param_err(10)/f_I2y),' %'])
disp(['I3y: ',num2str(100*(param(11)/f_I3y - 1)),' +- ',num2str(100*param_err(11)/f_I3y),' %'])
disp(['I1z: ',num2str(100*(param(12)/f_I1z - 1)),' +- ',num2str(100*param_err(12)/f_I1z),' %'])
disp(['I2z: ',num2str(100*(param(13)/f_I2z - 1)),' +- ',num2str(100*param_err(13)/f_I2z),' %'])
disp(['I3z: ',num2str(100*(param(14)/f_I3z - 1)),' +- ',num2str(100*param_err(14)/f_I3z),' %'])
disp(['l1: ',num2str(1000*(param(15)-f_l1)),' +- ',num2str(1000*param_err(15)),' mm'])
disp(['l2: ',num2str(1000*(param(16)-f_l2)),' +- ',num2str(1000*param_err(16)),' mm'])
disp(['l3: ',num2str(1000*(param(17)-f_l3)),' +- ',num2str(1000*param_err(17)),' mm'])
disp(['si: ',num2str(1000*(param(18)-f_si)),' +- ',num2str(1000*param_err(18)),' mm'])
disp(['sl: ',num2str(1000*(param(19)-f_sl)),' +- ',num2str(1000*param_err(19)),' mm'])
disp(['r1: ',num2str(1000*(param(20)-f_r1)),' +- ',num2str(1000*param_err(20)),' mm'])
disp(['r2: ',num2str(1000*(param(21)-f_r2)),' +- ',num2str(1000*param_err(21)),' mm'])
disp(['r3: ',num2str(1000*(param(22)-f_r3)),' +- ',num2str(1000*param_err(22)),' mm'])
disp(['m1: ',num2str(1000*(param(23)-f_m1)),' +- ',num2str(1000*param_err(23)),' g'])
disp(['m2: ',num2str(1000*(param(24)-f_m2)),' +- ',num2str(1000*param_err(24)),' g'])
disp(['m3: ',num2str(1000*(param(25)-f_m3)),' +- ',num2str(1000*param_err(25)),' g'])
disp(['kc1: ',num2str(100*(param(26)/f_kc1 - 1)),' +- ',num2str(100*param_err(26)/f_kc1),' %'])
disp(['kc2: ',num2str(100*(param(27)/f_kc2 - 1)),' +- ',num2str(100*param_err(27)/f_kc2),' %'])
disp('---------------------------------------'),disp(' ')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Functions %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fitparam = param_conv(param,param_test,jj)
% Converts fitting parameter vector to a vector useful for the model compiling
% code
% testing a parameter for gradient calculation
if jj > 0
param(jj) = param_test;
end
% converting param to fitparam for
fitparam = param;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function pen_mod = genmod(fitparam,modcompstring) %#ok<INUSL>
% compiling model with values in fitparam
% modfit = 1; %#ok<NASGU> % tells the model to compile with these parameters
natural_damp = 0.01;
eval(modcompstring)
pen_mod = mbtrip;
pen_mod.a(19:36,19:36) = -natural_damp*eye(18);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function model_comparison(mbtrip,pen_mod,meas_data,model_index_out,model_index_in)
% comparing model to top mass transfer functions to view results
f = meas_data.meas.freq;
w = 2*pi*f;
xlim = [0.1 10]; % x axis limits
in_names = {'Gnd L','Gnd T','Gnd V','Gnd Y','Gnd P','Gnd R',...
'TOP L','TOP T','TOP V','TOP Y','TOP P','TOP R',...
'MID L','MID T','MID V','MID Y','MID P','MID R',...
'BOT L','BOT T','BOT V','BOT Y','BOT P','BOT R'};
out_names = {'TOP L','TOP T','TOP V','TOP Y','TOP P','TOP R',...
'MID L','MID T','MID V','MID Y','MID P','MID R',...
'BOT L','BOT T','BOT V','BOT Y','BOT P','BOT R'};
in_units = {'m','m','m','rad','rad','rad',...
'N','N','N','Nm','Nm','Nm',...
'N','N','N','Nm','Nm','Nm',...
'N','N','N','Nm','Nm','Nm'};
out_units = {'m','m','m','rad','rad','rad',...
'm','m','m','rad','rad','rad',...
'm','m','m','rad','rad','rad'};
model2meas_in_index = [zeros(6,1);1;2;3;6;5;4];
model2meas_out_index = [1;2;3;6;5;4];
[mod_mag,mod_ph] = bode(mbtrip(model_index_out,model_index_in),w);
mod_mag = abs(squeeze(mod_mag));
mod_ph = squeeze(mod_ph);
[mod_mag_fit,mod_ph_fit] = bode(pen_mod(model_index_out,model_index_in),w);
mod_mag_fit = abs(squeeze(mod_mag_fit));
mod_ph_fit = squeeze(mod_ph_fit);
meas_mag = abs(meas_data.meas.eul2eul(model2meas_out_index(model_index_out),model2meas_in_index(model_index_in)).f); % /((8.7431e-07)*1e6)
meas_ph = angle(meas_data.meas.eul2eul(model_index_out,model_index_in-6).f)*180/pi;
figure,maxfig
subplot(211)
loglog(f,meas_mag,'k',f,mod_mag,'b',f,mod_mag_fit,'r','LineWidth',3),grid on
set(gca,'XLim',xlim,'FontSize',25,'YTick',logspace(-10,10,11))%,'YLim',ylims)
title(['Triple Model Fit TF Results: ',in_names{model_index_in},' to ',out_names{model_index_out}])
ylabel([out_names{model_index_out},' Response (',out_units{model_index_out},'/',in_units{model_index_in},')'])
subplot(212)
semilogx(f,meas_ph,'k',f,mod_ph,'b',f,mod_ph_fit,'r','LineWidth',3),grid on
set(gca,'XLim',xlim,'FontSize',25,'YLim',[-185 5],'YTick',linspace(-180,0,5))
xlabel('Hz'),ylabel('Phase (degrees)')
legend('Measurement','Original Model','Fit Model',3)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function err = err_vec_calc(mbtrip,meas)
% This function calculates the percent error residuals between the model
% and the measurements. The residuals are output as a column vector. This
% function must be modified to account for any changes in which type of input data
% is used for the fit.
phi = zeros(22,1); % model predicted values
err = zeros(17,1); % percent error residuals between model predicted values and measured values
% phi = zeros(4,1); % model predicted values
% err = zeros(1,1); % percent error residuals between model predicted values and measured values
% Calculating the Long-Pitch model error
lpmodel = model_cut_triple(mbtrip,'lp');
phi(1:6) = sqrt(sort(eig(-lpmodel.a(7:12,1:6)))); % model modes for free suspension
err(1:6) = (meas.modes.free.LP.modefreqs-phi(1:6))./meas.modes.free.LP.modefreqs;
% Calculating the Vertical model error
vmodel = model_cut_triple(mbtrip,'v');
phi(7:9) = sqrt(sort(eig(-vmodel.a(4:6,1:3)))); % model modes for free suspension
err(7:8) = (meas.modes.free.V.modefreqs-phi(7:8))./meas.modes.free.V.modefreqs;
% % Calculating the top to top TF vertical zero
% v23model = model_cut_triple(mbtrip,'v23');
% phi(10:11) = sqrt(sort(eig(-v23model.a(3:4,1:2)))); % model modes for free suspension
% err(9) = (meas.modes.toplocked.V.modefreqs-phi(10))./meas.modes.toplocked.V.modefreqs;
% Calculating the Yaw model error
ymodel = model_cut_triple(mbtrip,'y');
phi(10:12) = sqrt(sort(eig(-ymodel.a(4:6,1:3)))); % model modes for free suspension
err(9:11) = (meas.modes.free.Y.modefreqs-phi(10:12))./meas.modes.free.Y.modefreqs;
% Calculating the Trans-Roll model error
trmodel = model_cut_triple(mbtrip,'tr');
phi(13:18) = sqrt(sort(eig(-trmodel.a(7:12,1:6)))); % model modes for free suspension
err(12:16) = (meas.modes.free.TR.modefreqs-phi(13:17))./meas.modes.free.TR.modefreqs;
% Calculating the top mass long mode when the cavity is locked at the mid
% stage with high gain
l1p123 = model_cut_triple(mbtrip,'l1p123');
phi(19:22) = sqrt(sort(eig(-l1p123.a(5:8,1:4)))); % model modes for free suspension
err(17) = (meas.modes.cavmidbot.LP.modefreqs-phi(20))./meas.modes.cavmidbot.LP.modefreqs;
% l1p123 = model_cut_triple(mbtrip,'l1p123');
% phi(1:4) = sqrt(sort(eig(-l1p123.a(5:8,1:4)))); % model modes for free suspension
% err(1) = (meas.modes.cavmidbot.LP.modefreqs-phi(2))./meas.modes.cavmidbot.LP.modefreqs;
% % M1 to M1 high freq mag
% freqlength = 100;
% freq = 2*pi*linspace(7,9,freqlength);
% phi(23) = sum(abs(squeeze(freqresp(mbtrip(1,7),freq))))/freqlength;
% err(18) = (meas.mag.M1toM1.L2L-phi(23))/meas.mag.M1toM1.L2L;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mbtriple,pend]=ssmake3MBf_modelfitting(fitparam,varargin) %#ok<DEFNU>
% ssmake3MBf - the full-matrix version of the triple 'make' file to end (hopefully) all triple make files.
%
% Generates 4 systems of state-space matrices representing a triple pendulum.
%
% This file loads file triple.m which should define structure pend with fields for
% all required parameters.
%
% Depending on the selected options, this file loads one of symbexport3.m,
% symbexport3db.m, symbexport3lat.m, or symbexport3dblat.m. These contain
% mechanically generated Matlab code for state-space matrix elements
% exported from the corresponding Mathematica model by Mark Barton.
%
% If pend.db is defined and non-zero, top mass has dual blades at both levels
% as proposed for the AdvLIGO beamsplitter, otherwise, quad blades at top
% mass a la GEO.
%
% If pend.kx1 and pend.kx2 are defined as per-side stiffnesses, blades can flex
% laterally, otherwise just vertically.
%
% If pend.ribbon is defined and non-zero, bottom stage is taken to have
% ribbons of thickness pend.t3 and width pend.W3.
%
% If pend.stage2 is defined and non-zero, d0-d4 are interpreted as raw
% values, i.e., as actual wire breakoff vertical positions, and fudges are
% applied to include the effect of wire flexure. The fudges used give essentially
% perfect agreement with the output of Stage 2 of the equivalent
% Mathematica model if pend.ribbon is not set, and very good agreement
% otherwise. If pend.stage2 is undefined or 0, d0-d4 are taken as effective values,
% and flexure correction is assumed to be inapplicable (as for infinitely
% flexible wire) or to have been applied manually.
%
% Ultimately based on the mathematical model of Calum Iain Torrie et al. 1998
% with contributions from K.A. Strain 6/99.
% 8/16/06: initial version based on ssmake3pv2nMB2.m for GEO-style triple with
% four blades, with lots of improvements cut'n'pasted from ssm4pv2eMB4.m.
% Transverse/roll SS object renamed from rtn to trn to avoid naming
% conflict with Real-Time Workshop. All B matrix signs hand-checked.
% 3/4/07: corrected formula for flex1/2/3 (was '4' instead of 'nw1'/'nw2'/'nw3')
% 6/21/2011: renamed ssmake3MBf.m, reworked to use full matrices from symbexport3full.m etc.
% 9/27/2012 Brett Shapiro: Added section between lines 57 and 83 to interface with the
% model fitting file TripPend_GaussNewton_fit_...m. Added the suffix
% '_modelfitting' to the name of this file to distinguish it from the
% non-model fitting version. Added the input, 'fitparam' to the function,
% which is the vector of values modified from the 'pend' structure.
% Commented out some disp comments that output to command line.
% TripPend_GaussNewton_fit calls this function many times and will flood
% the command line like crazy if they are left in.
% load in the pendulum parameters
% global pend
if isempty(varargin)
triplep;
else
pend = eval([varargin{1} ';']);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% for use with model fitting code TripPend_GuassNewton_...m %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pend.d0 = fitparam(1);
pend.d1 = fitparam(2);
pend.d2 = fitparam(3);
pend.d3 = fitparam(4);
pend.d4 = fitparam(5);
pend.I1x = fitparam(6);
pend.I2x = fitparam(7);
pend.I3x = fitparam(8);
pend.I1y = fitparam(9);
pend.I2y = fitparam(10);
pend.I3y = fitparam(11);
pend.I1z = fitparam(12);
pend.I2z = fitparam(13);
pend.I3z = fitparam(14);
pend.l1 = fitparam(15);
pend.l2 = fitparam(16);
pend.l3 = fitparam(17);
pend.si = fitparam(18);
pend.sl = fitparam(19);
pend.r1 = fitparam(20);
pend.r2 = fitparam(21);
pend.r3 = fitparam(22);
pend.m1 = fitparam(23);
pend.m2 = fitparam(24);
pend.m3 = fitparam(25);
pend.kc1 = fitparam(26);
pend.kc2 = fitparam(27);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% for use with model fitting code TripPend_GuassNewton_...m %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set any undefined flags to default values
if not(isfield(pend,'stage2'))
pend.stage2=0; % define, default to false (wire flexure correction neglected or implicit)
end
if not(isfield(pend,'ribbon') )
pend.ribbon=0; % define, default to false (round fibres)
end
if not(isfield(pend,'db') )
pend.db=0; % define, default to false (GEO-style quad blades, not AdvLIGO-style double)
end
if not(isfield(pend,'g') )
pend.g=9.81;
end
% general redefintion of variables to simplify notation
% and cope with different pendulum designs
g = pend.g;
Y1 = pend.Y1;
Y2 = pend.Y2;
Y3 = pend.Y3;
% Allow for blade stiffnesses to be specified directly rather than via ufcs.
if isfield(pend, 'kc1')
kc1 = pend.kc1;
pend.ufc1 = sqrt(2*kc1/pend.m1)/2/pi;
else
kc1 = 1/2 * (2*pi*pend.ufc1)^2*pend.m1;
pend.kc1 = kc1;
end
if isfield(pend, 'kc2')
kc2 = pend.kc2;
pend.ufc2 = sqrt(2*kc2/pend.m2)/2/pi;
else
kc2 = 1/2 * (2*pi*pend.ufc2)^2*pend.m2;
pend.kc2 = kc2;
end
l1 =pend.l1;
l2 =pend.l2;
l3 =pend.l3;
% wire stiffnesses (per side, as opposed to per wire for the Mathematica)
kw1 =pend.Y1*pi*pend.r1^2/l1*pend.nw1/2;
kw2 =pend.Y2*pi*pend.r2^2/l2*pend.nw2/2;
if not(isfield(pend,'kw3')) % vertical stiffness of fibers
if pend.ribbon
kw3 = pend.Y3*pend.W3*pend.t3/l3*pend.nw3/2;
else
kw3 = pend.Y3*pi*pend.r3^2/l3*pend.nw3/2;
end
else
kw3 = pend.kw3;
end
%********************************************************************
% allows choice of 2 wires to set separation to zero
s0=pend.su; %#ok<NASGU>
s1=pend.su; %#ok<NASGU>
s2=pend.si; %#ok<NASGU>
s3=pend.si; %#ok<NASGU>
s4=pend.sl; %#ok<NASGU>
s5=pend.sl; %#ok<NASGU>
d0=pend.d0;
d1=pend.d1;
d2=pend.d2;
d3=pend.d3;
d4=pend.d4;
%********************************************************************
m1 = pend.m1;
m2 = pend.m2;
m3 = pend.m3;
I1x = pend.I1x; %#ok<NASGU>
I2x = pend.I2x; %#ok<NASGU>
I3x = pend.I3x; %#ok<NASGU>
I1y = pend.I1y; %#ok<NASGU>
I2y = pend.I2y; %#ok<NASGU>
I3y = pend.I3y; %#ok<NASGU>
I1z = pend.I1z; %#ok<NASGU>
I2z = pend.I2z; %#ok<NASGU>
I3z = pend.I3z; %#ok<NASGU>
% for backward compatibility, default off-axis MOI terms to zero
if isfield(pend, 'I1xy')
else
pend.I1xy = 0;
end
if isfield(pend, 'I1yz')
else
pend.I1yz = 0;
end
if isfield(pend, 'I1zx')
else
pend.I1zx = 0;
end
if isfield(pend, 'I2xy')
else
pend.I2xy = 0;
end
if isfield(pend, 'I2yz')
else
pend.I2yz = 0;
end
if isfield(pend, 'I2zx')
else
pend.I2zx = 0;
end
if isfield(pend, 'I3xy')
else
pend.I3xy = 0;
end
if isfield(pend, 'I3yz')
else
pend.I3yz = 0;
end
if isfield(pend, 'I3zx')
else
pend.I3zx = 0;
end
I1xy = pend.I1xy; %#ok<NASGU>
I1yz = pend.I1yz; %#ok<NASGU>
I1zx = pend.I1zx; %#ok<NASGU>
I2xy = pend.I2xy; %#ok<NASGU>
I2yz = pend.I2yz; %#ok<NASGU>
I2zx = pend.I2zx; %#ok<NASGU>
I3xy = pend.I3xy; %#ok<NASGU>
I3yz = pend.I3yz; %#ok<NASGU>
I3zx = pend.I3zx; %#ok<NASGU>
m13 = m1+m2+m3;
m23 = m2+m3;
n0 = pend.n0;
n1 = pend.n1;
n2 = pend.n2;
n3 = pend.n3;
n4 = pend.n4;
n5 = pend.n5;
%***********************************************************************************
% cosine and sine of the angle the wire makes with the vertical (z)
si1=(n1-n0)/l1; %sin(omega1)
si2=(n3-n2)/l2; %sin(omega2)
si3=(n5-n4)/l3; %sin(omega3)
c1=(l1^2-(n1-n0)^2)^0.5/l1; %cos(omega1)
c2=(l2^2-(n3-n2)^2)^0.5/l2; %cos(omega2)
c3=(l3^2-(n5-n4)^2)^0.5/l3; %cos(omega3)
k1 = kw1; %#ok<NASGU>
k2 = kw2; %#ok<NASGU>
k3 = kw3; %#ok<NASGU>
su=pend.su; %#ok<NASGU>
si=pend.si; %#ok<NASGU>
sl=pend.sl; %#ok<NASGU>
% Total Mass Loads
pend.m13 = m13;
pend.m23 = m23;
% Vertical distances between centres of mass
pend.tl1 = sqrt(pend.l1^2 - (pend.n0-pend.n1)^2) + pend.d0;
pend.tl2 = sqrt(pend.l2^2 - (pend.n2-pend.n3)^2) + pend.d1 + pend.d2;
pend.tl3 = sqrt(pend.l3^2 - (pend.n4-pend.n5)^2) + pend.d3 + pend.d4;
% Distance to the centre of mass from suspension point
pend.l_suspoint_to_centreofoptic = pend.tl1+pend.tl2+pend.tl3;
% Distance to the bottom of the test mass from suspension point
pend.l_suspoint_to_bottomofoptic = pend.tl1+pend.tl2+pend.tl3+pend.tr;
% Stiffness of fibres/ribbons
% pi = 4*atan(1);
M11 = (1/4)*pi*pend.r1^4;
M21 = (1/4)*pi*pend.r2^4;
if pend.ribbon
M31 = (1/4)*pend.W3*pend.t3^3/12;
M32 = (1/4)*pend.W3^3*pend.t3/12;
else
M31 = (1/4)*pi*pend.r3^4;
M32 = (1/4)*pi*pend.r3^4;
end
pend.flex1 = sqrt(pend.nw1*M11*Y1/m13/g)*c1^(3/2);
pend.flex2 = sqrt(pend.nw2*M21*Y2/m23/g)*c2^(3/2);
pend.flex3 = sqrt(pend.nw3*M31*Y3/m3/g)*c3^(3/2); % for long/pitch, yaw and vertical
pend.flex3tr = sqrt(pend.nw3*M32*Y3/m3/g)*c3^(3/2); % for trans/roll, with M32 instead of M31
flex1 = pend.flex1;
flex2 = pend.flex2;
flex3 = pend.flex3; % for long/pitch, yaw and vertical; see below for trans/roll
if pend.stage2
% apply fudges to approximate Mathematica Stage2 results
d0 = d0 + flex1; %#ok<NASGU>
d1 = d1 + flex2; %#ok<NASGU>
d2 = d2 + flex2; %#ok<NASGU>
d3 = d3 + flex3; %#ok<NASGU>
d4 = d4 + flex3; %#ok<NASGU>
l1 = l1 - 2*flex1/c1; %#ok<NASGU>
l2 = l2 - 2*flex2/c2; %#ok<NASGU>
l3 = l3 - 2*flex3/c3; %#ok<NASGU>
n0 = n0 + si1*flex1/c1; %#ok<NASGU>
n1 = n1 - si1*flex1/c1; %#ok<NASGU>
n2 = n2 + si2*flex2/c2; %#ok<NASGU>
n3 = n3 - si2*flex2/c2; %#ok<NASGU>
n4 = n4 + si3*flex3/c3; %#ok<NASGU>
n5 = n5 - si3*flex3/c3; %#ok<NASGU>
% disp('Using Stage 2 fudges');
else
disp('Not using Stage 2 fudges');
end
bd = pend.bd;
latelements=0;
if isfield(pend,'kx1')
if isfield(pend,'kx2')
% lateral compliances have been defined - use appropriate matrix elements
disp('Using matrix elements with blade lateral compliance');
latelements = 1;
kx1 = pend.kx1; %#ok<NASGU>
kx2 = pend.kx2; %#ok<NASGU>
if pend.db
symbexport3dblatfull
disp('Using matrix elements with double blades at top mass');
else
disp('Using matrix elements with quad blades at top mass');
symbexport3latfull
end
else
end
end
if not(latelements)
% disp('Using matrix elements with no blade lateral compliance');
if pend.db
disp('Using matrix elements with double blades at top mass');
symbexport3dbfull
else
% disp('Using matrix elements with quad blades at top mass');
symbexport3full
end
end
mbtripleA = [...
zeros(18) eye(18)
(-km)\(xm-cqxm'/qm*cqxm) -bd*eye(18)
];
bm = (-km)\(cxsm-cqxm'/qm*cqsm); % ground displacement inputs: x,y,z,yaw,pitch,roll
bm2 = km\eye(18); % coordinate force inputs: pitchn,xn,pitch1,x1,...
mbtripleB = [...
zeros(18,24)
bm bm2
];
mbtripleC = [...
eye(18) zeros(18,18)
];
mbtripleD = [...
zeros(18,24)
]; %#ok<NBRAK>
mbtriple = ss(mbtripleA,mbtripleB,mbtripleC,mbtripleD);