Contents
% 16.323 Project % Frequency domain shaped LQR control of a Fabry-Perot interferometer bound % between 2 double pendulum suspended mirrors. % % Brett Shapiro % 4 May 2010 % clear clc,close all
Initializations
Simulation Paramters
dt = 2^(-14); % simulink simulation sampling of 16384 Hz (actual sampling used by LIGO) t_end = 128; % simulink simulation time in seconds (long enough to capture sufficient oscillations of support table motion) % Double pendulum plant state space model A = [ 0 0 1 0 0 0 0 1 -73.83 16.04 -0.001 0 16.36 -16.36 0 -0.001 ]; B = [ 0 0 0 0 0 0 57.8 0.02476 0 0 0 0.02525 ]; Bu = B(:,[2 3]); C = [ 1 0 0 0 0 1 0 0 ]; D = zeros(2,3); P = ss(A,B,C,D); np = size(A,1); % dimension of plant state % Passive mirror state space: uncontrolled but with damping. Damping simply % modeled with added viscosity. Input is ground motion, output is mirror % motion P2 = ss(A+[zeros(2,4);zeros(2) -3*eye(2)],B(:,1),C(2,:),0); % Actuator saturation limits top_sat = 1.0e-3; % Newtons bottom_sat = 0.1e-3; % Newtons % Cavity_rms_limit = 1e-12; % Noise/Disturbance filtering curves seis_shape = zpk(-2*pi*1*ones(1,4),-2*pi*0.2*ones(1,6),(2e-7)*((2*pi)^2)*(0.2)^6); % ground motion shape, target experimental goal upcon_shape = zpk(0,-2*pi*5*[1 1],(1e-7)*(2*pi*5)^2); % best 'guess' of what the actuator Barkhausen noise looks like.
LQR Control
Frequency dependent cost functions
% cost function for state two (mirror position) zx2 = -2*pi*1; px2 = -2*pi*[0;40]; kx2 = 1e1; [numx2 denx2] = zp2tf(zx2,px2,kx2); bx2_0 = numx2(1); numx2=numx2(2:end);denx2=denx2(2:end); nx2 = length(px2); % dimension of the state 2 weighting filter Afwx2 = [-denx2' [eye(nx2-1);zeros(1,nx2-1)]]; Bfwx2 = [zeros(nx2,np/2-1) (numx2-denx2*bx2_0)' zeros(nx2,np/2)]; Cfwx2 = [1 zeros(1,nx2-1)]; Dfwx2 = [zeros(1,np/2-1) bx2_0 zeros(1,np/2)]; Wx2 = ss(Afwx2,Bfwx2,Cfwx2,Dfwx2); % Global control weighting parameters r = 1e-15; % broadband control effort weight (weights importance of state error verse control effort; relative control effort weighting done with cost filters) crf = 5; % crossover of control filter costs in Hz. Also approximate crossover of input loop gains. crg = 1.35; % crossover gap between control effort cost filters. % Top mass control cost function Rs1 = 80; % dB gain change of elliptical filter [zu1,pu1,ku1] = ellip(6,1,Rs1,2*pi*crf*crg,'high','s'); zu1 = [0;0;zu1]; pu1 = [pu1;-2*pi*0.1*[1;1;1]]; ku1 = 2000*sqrt(r)*ku1; [numu1 denu1] = zp2tf(zu1,pu1,ku1); bu1_0 = numu1(1); numu1=numu1(2:end);denu1=denu1(2:end); nu1 = length(pu1); % dimension of the top mass weighting filter Afwu1 = [-denu1' [eye(nu1-1);zeros(1,nu1-1)]]; Bfwu1 = (numu1-denu1*bu1_0)'; Cfwu1 = [1 zeros(1,nu1-1);zeros(1,nu1)]; Dfwu1 = [bu1_0;sqrt(r)]; Wu1 = ss(Afwu1,Bfwu1,Cfwu1,Dfwu1); % Bottom mass control cost function Rs2 = 60; % dB gain change of elliptical filter [zu2,pu2,ku2] = ellip(6,1,Rs2,2*pi*crf/crg,'low','s'); zu2 = [zu2]; pu2 = [pu2;-2*pi*0.1*[0.1;0.1;1;1]]; ku2 = 2000*sqrt(r)*ku2; %#ok<NBRAK> [numu2 denu2] = zp2tf(zu2,pu2,ku2); bu2_0 = numu2(1); numu2=numu2(2:end);denu2=denu2(2:end); nu2 = length(pu2); % dimension of the bottom mass weighting filter Afwu2 = [-denu2' [eye(nu2-1);zeros(1,nu2-1)]]; Bfwu2 = (numu2-denu2*bu2_0)'; Cfwu2 = [1 zeros(1,nu2-1);zeros(1,nu2)]; Dfwu2 = [bu2_0;sqrt(r)]; Wu2 = ss(Afwu2,Bfwu2,Cfwu2,Dfwu2); % Superplant with plant and weighting functions Ass = [ A zeros(np,nx2) zeros(np,nu1) zeros(np,nu2) Bfwx2 Afwx2 zeros(nx2,nu1) zeros(nx2,nu2) zeros(nu1,np) zeros(nu1,nx2) Afwu1 zeros(nu1,nu2) zeros(nu2,np) zeros(nu2,nx2) zeros(nu2,nu1) Afwu2 ]; Bss = [ Bu zeros(nx2,np/2) Bfwu1 zeros(nu1,np/2-1) zeros(nu2,np/2-1) Bfwu2 ]; Css = [zeros(1,np/2-1) 1 zeros(1,np/2+nx2+nu1+nu2)];Dss = zeros(1,np/2); Pss = ss(Ass,Bss,Css,Dss); % LQR Weights Jss = [Dfwx2'*Dfwx2 Dfwx2'*Cfwx2 zeros(np,nu1+nu2+np/2) ; Cfwx2'*Dfwx2 Cfwx2'*Cfwx2 zeros(nx2,nu1+nu2+np/2) ; zeros(nu1,np+nx2) Cfwu1'*Cfwu1 zeros(nu1,nu2) Cfwu1'*Dfwu1 zeros(nu1,1) ; zeros(nu2,np+nx2+nu1) Cfwu2'*Cfwu2 zeros(nu2,1) Cfwu2'*Dfwu2 ; zeros(1,np+nx2) Dfwu1'*Cfwu1 zeros(1,nu2) Dfwu1'*Dfwu1 0 ; zeros(1,np+nx2+nu1) Dfwu2'*Cfwu2 0 Dfwu2'*Dfwu2 ;]; % convenient compilation of all weights in one matrix nJ = size(Jss,1); Q = Jss(1:nJ-np/2,1:nJ-np/2); % weight on super plant states R = Jss(nJ-np/2+1:nJ,nJ-np/2+1:nJ); % Weight on super plant control N = Jss(1:nJ-np/2,nJ-np/2+1:nJ); % Super plant cross term weights % LQR feedback gain K = lqr(Ass,Bss,Q,R,N); % Generating estimator Ckn = [0 1 0 0]; % only have test mass position measurement % L = lqr(A',Ckn',diag([1 1 1 1]),2e-10)'; % lqr defined estimatorgain L = place(A',Ckn',2*pi*[pair(100,150) pair(100,120)])'; % butterworth-ish pattern Pest = ss(A-L*Ckn,[L Bu],eye(4),zeros(4,3)); % estimator state space % Generating overall control law Kx = K(:,1:4); Kw = K(:,5:end); % Components of K feedback gain. Kx is for the plantr states, Kw is for the augmented states. S = [zeros(nu1,nx2) Afwu1 zeros(nu1,nu2);zeros(nu2,nx2) zeros(nu2,nu1) Afwu2]; % A matrix for combined weighting function filter T = [Bfwu1 zeros(nu1,1);zeros(nu2,1) Bfwu2]; % B matrix for combined weighting function filter AA = [(A-L*Ckn-Bu*Kx) -Bu*Kw; zeros(nx2,np) Afwx2 zeros(nx2,nu1+nu2);-T*Kx (S-T*Kw)]; BB = [L;Bfwx2(:,np/2);zeros(nu1+nu2,1)]; CC = K; DD = [0;0]; SuperControl = ss(AA,BB,CC,DD); % SIMO compensator block. % Breaking SuperControl into the two feedback filters u1_filt = SuperControl(1,1); % top mass controller u2_filt = SuperControl(2,1); % bottom mass controller
Plotting
f = 0.01:0.005:1000; w = 2*pi*f; figure % loop gain plot bode(P(2,2)*u1_filt,w),hold on bode(P(2,3)*u2_filt,w) bode(P(2,2)*u1_filt+P(2,3)*u2_filt,w),grid on title('System Loop Gain with Frequency Weighting') legend('u_1','u_2','Sum') bform % format bode plot figure % cost function plot bode(Wu1(1,1)),hold on bode(Wu2(1,1)) bode(Wx2(1,2)) bode(ss([],[],[],sqrt(r))),grid on title('Cost Function Plot') legend('u_1','u_2','x_2','u broadband') bform % format bode plot
Standard LQR for comparison
Kstandard = lqr(A,Bu,diag([0 1 0 0]),diag(5*[r 10000*r])); SuperControl_standard = ss(A-L*Ckn-Bu*Kstandard,L,Kstandard,[0;0]); u1_filt_standard = SuperControl_standard(1,1); % top mass controller u2_filt_standard = SuperControl_standard(2,1); % bottom mass controller figure % loop gain plot bode(P(2,2)*u1_filt_standard,w),hold on bode(P(2,3)*u2_filt_standard,w) bode(P(2,2)*u1_filt_standard+P(2,3)*u2_filt_standard,w),grid on title('System Loop Gain with standard LQR') legend('u_1','u_2','Sum') bform % format bode plot Cavity_Simulation_File % Simulink file. On my computer it takes about 2 minutes to run.
Plotting Simulink simulation results: RUN THIS CODE ONLY AFTER RUNNING
SIMULINK
I chose not to upload the data in .mat files because they collectively are about 60 MB
err = err_sim; iter = length(err.signals.values); [ph1,fh] = periodogram(err.signals.values(1:iter-1),hanning(iter-1),iter-1,1/dt); %#ok<NASGU> err = err_sim1; iter = length(err.signals.values); [ph2,fh] = periodogram(err.signals.values(1:iter-1),hanning(iter-1),iter-1,1/dt); figure,loglog(fh,sqrt(ph2),fh,sqrt(ph1),[10 10000],[1e-18 1e-21],'LineWidth',3),grid on set(gca,'XLim',[0.1 8000],'YLim',[1e-25 1e-12],'FontSize',15) legend('Standard LQR','Frequency LQR','Requirement') xlabel('Frequency (Hz)') ylabel('Calibrated Cavity Error (m/\surdHz)') title('Calibrated Cavity Error with Frequency and Standard LQR Control') t = 0:dt:t_end; figure plot(t,err_sim.signals.values,t,err_sim1.signals.values,[0 t_end],Cavity_rms_limit*[1 1],'r',[0 t_end],-Cavity_rms_limit*[1 1],'r','LineWidth',3),grid on set(gca,'FontSize',15,'XLim',[0 t_end],'YLim',Cavity_rms_limit*1.1*[-1 1]) legend('Frequency LQR','Standard LQR','RMS Requirement') xlabel('Time (seconds)') ylabel('Displacement (N)') title('Cavity Error Signal') figure plot(t,u1.signals.values,t,u1b.signals.values,[0 t_end],top_sat*[1 1],'r',[0 t_end],-top_sat*[1 1],'r','LineWidth',3),grid on set(gca,'FontSize',15,'XLim',[0 t_end],'YLim',top_sat*1.1*[-1 1]) legend('Frequency LQR','Standard LQR','Saturation Limit') xlabel('Time (seconds)') ylabel('Force (N)') title('Top Mass Actuation') figure plot(t,u2.signals.values,t,u2b.signals.values,'LineWidth',3),grid on%,[0 t_end],bottom_sat*[1 1],'r',[0 t_end],-bottom_sat*[1 1],'r','LineWidth',3),grid on set(gca,'FontSize',15,'XLim',[0 t_end],'YLim',bottom_sat*[-1 1]/10) legend('Frequency LQR','Standard LQR') xlabel('Time (seconds)') ylabel('Force (N)') title('Bottom Mass Actuation')
