Commit c6ef454b authored by Patrick Göttsch's avatar Patrick Göttsch
Browse files

Gitlab Runner Update from atc-aux-files/master:Update .gitlab-ci.yml

parent f9178a46
%% Analyzing the Stability of a Closed Loop with Scheduled State Feedback
% -------------------------------------------------------------------------
% script : exercise_Analysis_State_Feedback
% -------------------------------------------------------------------------
% Author : Christian Hoffmann
% Version : November, 4th 2013
% Copyright: CH, 2013
% -------------------------------------------------------------------------
%
% 1. Define plant
% 2. Setup and solve LMI problem
%
% -------------------------------------------------------------------------
%% Initialization
clc;clear all;close all;warning off;s = tf('s');
% Add subdirectories to path
addpath(genpath('.'))
thisdate = datestr(now, 'yyyy-mm-dd_HH-MM');
%% 1. Plant Definition
% General plant definition
% Scheduling signal
rho_bnd = [-pi pi ];
sigma_bnd = [-1 1 ];
tau = 3.75;
gamma = 0.5;
R = @(rho) [ cos(rho) sin(rho) ;...
-sin(rho) cos(rho) ];
A11 = [ 0.75 2.0 ;...
0 0.5 ];
A = @(rho) [ A11 R(rho) ;...
zeros(2,2) -eye(2)*tau ];
B = [ zeros(2,2) ;...
eye(2)*tau ];
DK = @(rho, gama) [ -R(rho)' *(gama*eye(2) + A11), zeros(2,2) ];
% Affine plant definition
% Scheduling parameter
% rho_grid = rho_bnd(1):0.01:rho_bnd(2);
% sigma_grid = -1:0.01:1;
% theta_grid = [ cos(rho_grid) ;...
% sin(rho_grid) ];
% nu_grid = -1:0.01:1;
% theta_bnd = [min(theta_grid, [], 2), max(theta_grid, [], 2)];
% nu_bnd = [min(min(nu_grid, [], 2)) max(max(nu_grid, [], 2))];
% utheta1 = ureal('utheta1', 0, 'Range', [theta_bnd(1, 1), theta_bnd(1, 2)]);
% utheta2 = ureal('utheta2', 0, 'Range', [theta_bnd(2, 1), theta_bnd(2, 2)]);
%
% uR = [ utheta1 utheta2 ;...
% -utheta2 utheta1 ];
%
% A_aff = [ A11 uR ;...
% zeros(2,2) -eye(2)*tau ];
%
% DK_aff = [ -uR' *(gama*eye(2) + A11), zeros(2,2) ];
% A0 = uA_aff.NominalValue;
% A1 = double(usubs(uA_aff - A0, 'utheta', 1));
%% 2. Setup LMI Solver and Solve Problem
yalmip('clear');
sdpoptions = sdpsettings('solver', 'sdpt3');
% Define decision variables
P0 = sdpvar( 4, 4, 'symmetric');
P1 = sdpvar( 4, 4, 'symmetric');
P2 = sdpvar( 4, 4, 'symmetric');
P = @(rho) P0 + cos(rho)* P1 + sin(rho)* P2;
dPdrho = @(rho) 0 -sin(rho)* P1 + cos(rho)* P2;
% Initialize constraints
LMIConstraints = [];
% Define LMIs
rho_grid = linspace(rho_bnd(1),rho_bnd(2),10);
for ii = 1:length(rho_grid)
rho = rho_grid(ii);
LMI02 = P(rho);
LMIConstraints = [LMIConstraints, LMI02 >= 0];
for jj = 1:length(sigma_bnd)
sigma = sigma_bnd(jj);
Acl = A(rho) + B*DK(rho, gamma);
LMI01 = P(rho)*Acl + Acl'*P(rho) + sigma*dPdrho(rho);
LMIConstraints = [LMIConstraints, LMI01 <= 0];
end
end
% Solve SDP
diagnostic = optimize(LMIConstraints, [], sdpoptions)
%% 3. Evaluate Solution
P0 = double(P0);
P1 = double(P1);
P2 = double(P2);
P = @(rho) P0 + cos(rho)* P1 + sin(rho)* P2;
dPdrho = @(rho) 0 -sin(rho)* P1 + cos(rho)* P2;
% Check on a denser grid
rho_grid = linspace(rho_bnd(1),rho_bnd(2),1000);
tic
for ii = 1:length(rho_grid)
rho = rho_grid(ii);
LMI01 = P(rho);
eigTest_current = eig(LMI01);
eigTest_current = sum(real(eigTest_current) < 0);
eigTest(ii,1) = eigTest_current;
for jj = 1:length(sigma_bnd)
sigma = sigma_bnd(jj);
Acl = A(rho) + B*DK(rho, gamma);
LMI01 = P(rho)*Acl + Acl'*P(rho) + sigma*dPdrho(rho);
eigTest_current = eig(LMI01);
eigTest_current = sum(real(eigTest_current) > 0);
eigTest(ii, 1+jj) = eigTest_current;
end
end
toc
eigTest = sum(sum(eigTest));
if (eigTest == 0)
fprintf('Exponential stability guaranteed on denser grid!\n')
else
fprintf('Exponential stability not guaranteed on denser grid!\n')
end
\ No newline at end of file
% Provides parameters for the Gyroscope simulation adn experiment. Is
% automatically loaded when the model is opened.
%initial conditions
q1dot_0 = 45;
q2_0 = 0;
q3_0 = 0;
%sampling time
ts = 1*0.000884; %in multiples of 0.000884 %max 1.15khz
% physical parameters taken from full nonlinear model by Hossam Abbas
sys.parameters.data.K_A = 0.0670;
sys.parameters.data.I_B = 0.0119;
sys.parameters.data.J_B = 0.0178;
sys.parameters.data.K_B = 0.0297;
sys.parameters.data.I_C = 0.0092;
sys.parameters.data.J_C = 0.0230;
sys.parameters.data.K_C = 0.0220;
sys.parameters.data.I_D = 0.0148;
sys.parameters.data.J_D = 0.0273;
%K_D =I_D
sys.parameters.data.fv1 = 1.8700e-4;
sys.parameters.data.fv2 = 0.0118;
sys.parameters.data.fv3 = 0.0027;
sys.parameters.data.fv4 = 0.0027;
\ No newline at end of file
%% Design an Integral Augmented LPV State Feedback Controller
% -------------------------------------------------------------------------
% script : exercise_Design_State_Feedback
% -------------------------------------------------------------------------
% Author : Julian Theis
% Version : November, 17th 2014, revised 14-nov-2016
% Copyright: TUHH, 2014
% -------------------------------------------------------------------------
%
% Requires LPVTools Toolbox
% Requires YALMIP and SDPT3
clearvars; clc; close all;
% addpath(genpath('../.')) % add all subdirectories to the path
%% PLANT AND SCALING
%
% We define the frequency range of interest for the design to be 0.01 rad/s
% to 1000 rad/s, mainly for convenience in plotting. Further some options
% for bodeplots are invoked.
w = {1e-2,1e2};
b = bodeoptions;
b.xlim = [1e-2 1e2];
b.ylim = [-1e2 1e2];
b.Grid = 'on';
%% Plant as grid-based LPV object
% Let's first define the grid we want to use. It's spanned by the three
% parameters 'q1dot', 'q3' and 'q4', all of which are (of course) rate
% bounded.
q1dot = pgrid('q1dot',30:15:60,[-10 10]);
q2 = pgrid('q2',[-25*pi/180:25*pi/180:25*pi/180], [-2 2]);
q3 = pgrid('q3',[-50*pi/180:25*pi/180:50*pi/180], [-2 2]);
dom = rgrid(q1dot,q2,q3);
% Now we evaluate the linearization of the nonlinear system at all these
% gridpoints, which gives an array of LTI state space models
G_ = linearize_gyro(q1dot.griddata,q2.griddata,q3.griddata);
% This array is now combined with the underlying grid to form a
% parameter-dependent state space model
Gp = pss(G_,dom);
% This pss object can be treated almost identical to an ss object, and we
% can perform tasks such as, e.g., bode or sigmaplots
figure(1)
bodemag(Gp,b,w)
figure(2)
sigma(Gp,w)
% Furthermore we can manipulate the grid, with commands like
% lpvsplit, lpvinterp, ...
%% Initial Scaling
%
% Scalings are of dire importance when working with frequency domain
% synthesis techniques. The following (very simple) scaling just normalizes
% the input and output values of the plant, such that 1 corresponds to both
% the maximum expected change in the reference signal and the maximum
% actuator capacity.
% I.e., if a unit step reference results in a control signal less than 1,
% the actuators are likely not to saturate. In terms of frequency domain
% indicators, this means that |KS|<1.
% Let's define the largest allowed input signals (in Nm)
umax = [0.666, 2.44];
% and the largest expected change in reference (in rad)
rmax = [45/180*pi, 90/180*pi];
% Now both input and output are scaled with this values
Du = diag(umax);
Dr = diag(rmax);
G = Dr\Gp*Du;
% G is now our synthesis model used to design a controller.
% When we apply this controller to the real plant, we need to remember to
% reverse the scalings, ie.
% u_real = Du*u and y = Dr^-1*y_real and r = Dr^-1*r_real
% For comparison, we will also create a single LTI controller at the
% gridpoint (45,0,0)
GLTI = lpvsplit(G,'q1dot',45,'q2',0,'q3',0);
figure(1); hold all;
bodemag(GLTI)
figure(2); hold all;
sigma(GLTI)
% Note that this design point does not cover any crosscouplings.
%% S/KS closed-loop shaping filter design
% We use the suggested weighting filters for the sensitivity
WS1 = ss(1/tf('s'));
WS2 = ss(1/tf('s'));
WS = mdiag(WS1,WS2);
% and the control sensitivity
WK1 = ss(1);
WK2 = ss(1);
WK = mdiag(WK1,WK2);
figure(3)
sigma(WS^-1,WK^-1,w)
%% Generalized Plant formulation
%
% For our synthesis tools to work, we need to first assemble a generalized
% plant that includes our performance inputs/outputs.
%
% r-----------------+
% |
% ------ v ------
% u--+---->| G |-+-o-->| WS |-->e1
% | ------ ------
% | ------
% ------------------>| WK |--->e2
% ------
%
% A convinient way to do this is to use the sysic command
systemnames = 'G WS WK';
inputvar = '[r{2}; u{2}]';
outputvar = '[WS; WK]';
input_to_G = '[u]';
input_to_WS = '[r-G]';
input_to_WK = '[u]';
cleanupsysic = 'yes';
P = sysic;
% Verify that this leads to a state space representation
% |dx| |A B1 B2 | |x|
% |e1| = |C11 0 0 | * |d|
% |e2| |C12 0 I | |u|
%
P.Data(:,:,1)
%% Synthesis LPV
%
% You know from class that a parameter-dependent Lyapunov matrix is less
% conservative since it takes rate variations into account. In order to
% arrive at a tractable formulation, we need to choose basis functions.
% There are no useful guidelines on how to do this other than trial and
% error.
% We define the basis functions in terms of the pgrid variables and need to
% provide their partial derivites with respect to the scheduling variables
clear Rb
Rb(1) = basis(1,0);
Rb(2) = basis(q1dot,1);
Rb(3) = basis(q2,1);
Rb(4) = basis(q3,1);
% We can now run the synthesis code, which takes our generalized plant, the
% number of control signals and the basis function object and returns a
% state feedback gain F as well as the performance index gamma.
% The synthesis may take some time, since it's solving that large system of
% LMIs that you learned about in class.
% There are two synthesis routines provided. The LPVTools original lpvsfsyn
% command solves the LMI problem as you know it.
% [F,gam,info] = lpvsfsyn(P,2,Rb);
% The modified version lpvsfsynth does essentially the same thing but
% solves two additional LMIs to minimize the condition number of the
% Lyapunov matrix in order to improve the numerics of the problem. This
% leads to what is called a suboptimal controller. USE THIS ONE, as it provides
% much more usuable results from a practical point of view. We will discuss
% this in the exercise. It further provides a data structure that includes
% the basis functions' coefficients. We will use this in the Simulink
% implementation of the controller.
[F,gam,info,data] = lpvsfsynth(P,2,Rb);
gam
% What might also be of interest for implementation is the fastest closed
% loop pole, although this concept is misleading in LPV systems.
% If you did not use the suboptimal synthesis, you will get a very large
% number here.
P_aug = pss(P.a,P.b,[P.c;eye(size(P.a,1))], [P.d; zeros(size(P.a,1),4)]);
CL = lft(P_aug,F);
fastestpole = max(double(lpvmax(abs(eig(CL.a)))))
%% CHOICE OF BASIS FUNCTIONS (for part b)
for ii=1:8
Rb(1) = basis(1,0);
Rb(2) = basis(q1dot,1);
Rb(3) = basis(q2,1);
Rb(4) = basis(q3,1);
Rb(5) = basis(q1dot^2,2*q1dot);
Rb(6) = basis(q2^2,2*q2);
Rb(7) = basis(q3^2,2*q3);
Rb(8) = basis(q1dot*q2*q3,'q1dot',q2*q3,'q2',q1dot*q3,'q3',q1dot*q2);
[F,gam,info,data] = lpvsfsynth(P,2,Rb(1:ii),synthopts('verbose',0));
fprintf(' number of basis functions: %1.0f \n',ii)
fprintf(' time required for synthesis: %1.1f s \n',info.time)
fprintf(' gamma_opt: %1.3f \n',info.gamma_opt)
gam_plot(ii) = info.gamma_opt;
time_plot(ii) = info.time;
end
figure(100)
subplot(121)
bar(1:8,gam_plot)
xlabel('Number of basis functions')
ylabel('Optimal gamma')
xlim([0.5,8.5])
subplot(122)
bar(1:8,time_plot)
xlabel('Number of basis functions')
ylabel('Time for synthesis')
xlim([0.5,8.5])
%% Synthesis LTI
% We use detuned weighting filters for the design, to arrive at a simlar
% time response.
WS1LTI = ss(0.1/tf('s'));
WS2LTI = ss(0.5/tf('s'));
WSLTI = WS;%mdiag(WS1LTI,WS2LTI);
WKLTI = WK;%mdiag(WK1,WK2);
systemnames = 'GLTI WSLTI WKLTI';
inputvar = '[r{2}; u{2}]';
outputvar = '[WSLTI; WKLTI]';
input_to_GLTI = '[u]';
input_to_WSLTI = '[r-GLTI]';
input_to_WKLTI = '[u]';
cleanupsysic = 'yes';
PLTI = sysic;
% We use the same synthesis routine as above, but only for one single grid
% point, which gives us an LTI controller
% [FLTI,gamLTI] = lpvsfsyn(PLTI,2,basis(1,0)); % optimal
[FLTI,gamLTI] = lpvsfsynth(PLTI,2,basis(1,0)); % suboptimal
F_LTI = double(FLTI);
% Again, we can check the fastest closed-loop pole
PLTI_aug = pss(PLTI.a,PLTI.b,[PLTI.c;eye(size(PLTI.a,1))], [PLTI.d; zeros(size(PLTI.a,1),4)]);
CLLTI = lft(PLTI_aug,FLTI);
fastestpole = max(double(abs(eig(CLLTI.a))))
% We now copy the gain to all gridpoints since an LTI controller
% is the same over all grid points
FLTI = pmat(repmat(FLTI.Data,[1,1,3,3,5]),dom);
%% Augmented state feedback - LPV
%
% Since we are using dynamic weights in our generalized plant, the state
% feedback gain that we just obtained needs access to the states of these
% filters as well. This means, that the filters are actually a part of the
% controller (with an interpretation as an INTEGRAL gain, where the state
% feedback gain itself is something like a PROPORTIONAL/DERIVATIVE gain)
% and consequently we end up with a dynamic controller.
% It looks like this:
% ------
% y-r -------->| WS |
% ---+--
% +--->|\
% |F>---+--------->u
% x ----------------->|/
%
% In order to assemble this controller, we can use sysic again.
% First we need to modifiy the weighting filters such that we get the
% STATES instead of the outputs
WSs = ss(WS.a,WS.b,eye(2),0);
% Second, we need to convert the gain into a pss object.
Fs = pss([],[],[],F);
% then we can connect all the components and get our controller
systemnames = 'Fs WSs';
inputvar = '[ry{2}; x{5}]';
outputvar = '[Fs]';
input_to_Fs = '[x;WSs]';
input_to_WSs = '[ry]';
cleanupsysic = 'yes';
K = sysic;
% To keep track of the inputs, let's make a note that the first 2 channels
% must be connected to the error signal r-y and the last 5 channels to the
% state x
K.InputGroup.e = [1:2];
K.InputGroup.x = [3:7];
% Let's finally take a look at the controller that we just synthesized.
figure(4)
bodemag(K,b,w)
% We see, that we have integral action in the first 2 channels (the
% error) and that we constant gains in all of the other channels.
% We further see, that the gains change for the different grid points,
% which is exactly why we call it gain-scheduling
% We build the closed-loop again by using sysic
G_aug = pss(G.a, G.b, [G.c;eye(size(G.a,1))], [G.d; zeros(size(G.a,1),size(G.b,2))]);
systemnames = 'G_aug K';
inputvar = '[r{2}]';
outputvar = '[G_aug(1:2);K]';
input_to_G_aug = '[K]';
input_to_K = '[r-G_aug(1:2); G_aug(3:7)]';
cleanupsysic = 'yes';
CL = sysic;
% and extract the complementary sensitivity and control sensitivity
T = CL(1:2,:);
KS = CL(3:4,:);
% We can then take a look at the step responses of these transfer functions
figure
step(T)
figure
step(KS)
title('Control Signal Response')
% and at the sigma plots
figure
subplot(211)
sigma(eye(2)-T,WS^-1,w)
subplot(212)
sigma(KS,WK^-1,w)
%% Augmented state feedback - LTI
% Let's do the same for the LTI design
FsLTI = pss([],[],[],FLTI);
WSsLTI = ss(WSLTI.a,WSLTI.b,eye(2),0);
systemnames = 'FsLTI WSsLTI';
inputvar = '[ry{2}; x{5}]';
outputvar = '[FsLTI]';
input_to_FsLTI = '[x;WSsLTI]';
input_to_WSsLTI = '[ry]';
cleanupsysic = 'yes';
KLTI = sysic;
figure(4); hold on
bodemag(KLTI,'r',b,w)
systemnames = 'G_aug KLTI';
inputvar = '[r{2}]';
outputvar = '[G_aug(1:2);KLTI]';
input_to_G_aug = '[KLTI]';
input_to_KLTI = '[r-G_aug(1:2); G_aug(3:7)]';
cleanupsysic = 'yes';
CLLTI = sysic;
TLTI = CLLTI(1:2,:);
KSLTI = CLLTI(3:4,:);
%% Comparison with varying one parameter at a time
figure(15)
for qq1 = 60:-5:30
clf
step(lpvinterp(T,'q1dot',qq1,'q2',0,'q3',0),10)
hold on;
step(lpvinterp(TLTI,'q1dot',qq1,'q2',0,'q3',0),'r',10)
title(['q1dot = ' int2str(qq1) 'rad/s'])
pause(0.2)
end
for qq2 = -25/180*pi:5/180*pi:25/180*pi
clf
step(lpvinterp(T,'q1dot',45,'q2',qq2,'q3',0),'b',10)
hold on;
step(lpvinterp(TLTI,'q1dot',45,'q2',qq2,'q3',0),'r',10)
title(['q2 = ' int2str(qq2*180/pi) 'deg'])
pause(0.2)
end
pause(2)
for qq3 = -50/180*pi:10/180*pi:50/180*pi
clf
step(lpvinterp(T,'q1dot',45,'q2',0,'q3',qq3),10)
hold on;
step(lpvinterp(TLTI,'q1dot',45,'q2',0,'q3',qq3),'r',10)
title(['q3 = ' int2str(qq3*180/pi) 'deg'])
pause(0.2)
end
pause(2)
%% Comparison over all grid points
figure(16)
for qq3 = -50/180*pi:25/180*pi:50/180*pi
for qq2 = -25/180*pi:25/180*pi:25/180*pi
for qq1 = 60:-15:30
clf
step(lpvinterp(T,'q1dot',qq1,'q2',qq2,'q3',qq3),10);hold on;
step(lpvinterp(TLTI,'q1dot',qq1,'q2',qq2,'q3',qq3),'r',10)
title(['q1dot = ' int2str(qq1) 'rad/s ' ' q2 = ' int2str(qq2*180/pi) 'deg ' ' q3 = ' int2str(qq3*180/pi) 'deg'])
pause(0.2)
end