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

Gitlab Runner Update from atc-aux-files/master:Merge branch 'prepare-for-mpc' into 'master'

parent a06eaf23
%% Stability of a Multi-Vehicle Formation
% -------------------------------------------------------------------------
% script : exercise_MAS_stability
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : December, 6th 2013
% Copyright: MB, 2013
% -------------------------------------------------------------------------
%
% 1. Compute the closed loop poles for a single subsystem
% 2. Compute the closed loop eigenvalues of the global system
% 3. Plot the Nyquist diagram of the forward chain of a subsystem
%
% -------------------------------------------------------------------------
%% Initialization
close all
% Define number of agents
N = 5;
% Define plant model
m = 10;
b = 0.5;
c = 0;
PA = [0, 1; -c/m, -b/m];
PB = [0; 1/m];
PC = [1 0];
PD = 0;
P = ss(PA,PB,PC,PD);
nx = size(PA,1); % number of states
% Define controller
K = 20*tf([1 0.05],[1 2.4]);
[KA,KB,KC,KD] = ssdata(K);
% Define the Laplacian
L = [1, -1/2, -1/2, 0, 0;
0, 1, 0, -1, 0;
-1/2, 0, 1, -1/2, 0;
0 -1/2, -1/2, 1, 0;
0 -1/2, 0, -1/2, 1];
la = sort(eig(L));
%% Closed loop poles at a single subsystem for all eigenvalues of L
ClPoles = zeros(size(PA,1)+size(KA,1), N);
for ii = 1:N
Pp = ss(PA,PB,la(ii)*PC,la(ii)*PD);
Cl = feedback(Pp*K,1);
ClPoles(:,ii) = pole(Cl);
end
disp('Closed loop poles for single subsystem:');
ClPoles
%% Closed loop poles of the global system
KB = -KB; % include negative sign due to negative feedback
KD = -KD;
A_11 = kron(eye(N),PA) + kron(eye(N),PB*KD*PC)*kron(L,eye(nx));
A_12 = kron(eye(N),PB*KC);
A_21 = kron(eye(N),KB*PC)*kron(L,eye(nx));
A_22 = kron(eye(N),KA);
A_ = [A_11, A_12;
A_21, A_22];
disp('Closed loop poles for the global closed loop system');
pA_ = eig(A_)
%% Nyquist plot
nyquist(P*K);
\ No newline at end of file
%% Design of a Formation Controller
% -------------------------------------------------------------------------
% script : exercise_MAS_qcopter
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : December 10th, 2015
% Copyright: MB, 2015
% -------------------------------------------------------------------------
%
% 1. Define the agent model given in the appendix of the Lecture Notes
% 2. Formulate agent + uncertain topology in LFT form
% 3. Generate a generalized plant
% 4. Synthesize a robust formation controller
%
% IMPORTANT: This is the exercise version.
% Lines marked with <FIXME> are to be filled by you!
% Unless you do so, the script will not be executable.
%
% -------------------------------------------------------------------------
%% state space model of the quadrocopter
g = 9.81; % Gravity constant
m = 0.640; % Mass of the Quadrocopter
A = [ 0 1 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 -g 0 0 0 ;
0 0 0 1 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 g 0 ;
0 0 0 0 0 1 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 1 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 1 ;
0 0 0 0 0 0 0 0 0 0 0 0 ] ;
B = [ 0 0 0 0 0 1/m 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 1 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 1 ]';
nx = size(A,1); % number of states
C = zeros(3,nx);
C(1,1) = 1; % x
C(2,3) = 1; % y
C(3,5) = 1; % z
nu = size(B,2); % number of inputs
ny = size(C,1); % number of outputs
D = zeros(ny,nu);
P = ss(A,B,C,D);
Paug = ss(A,B,[C; eye(nx)], [D; zeros(nx,nu)]);
% Augmented plant with state vector as additional output
%% setup of the generalized plant
% Factorize C
Dd = <FIXME>;
Cd = <FIXME>;
nd = size(Cd,1); % size of Delta
% Generate an LFT model of one agent
Blft = [zeros(nx,nd), B];
Clft = [Cd; C; eye(nx)];
Dlft = [zeros(nd,nd), zeros(nd,nu);
Dd, zeros(ny,nu);
zeros(nx,nd), zeros(nx,nu)];
Plft = ss(A,Blft,Clft,Dlft);
% Define the shaping filters
w_S = <FIXME>;
Ws = diag(ones(1,ny))*w_S;
w_KS = <FIXME>;
Wk = diag(ones(1,nu))*w_KS;
% Compose the generalized plant
systemnames = 'Plft Ws Wk';
inputvar = sprintf('[wd(%d); r(%d); u(%d)]', nd,ny,nu);
input_to_Plft = '[wd; u]';
input_to_Ws = sprintf('[r-Plft(%d:%d)]', nd+1,nd+ny);
input_to_Wk = '[u]';
outputvar = sprintf('[Plft(1:%d); Ws; Wk; r-Plft(%d:%d); -Plft(%d:%d)]', nd,nd+1,nd+ny,nd+ny+1,nd+ny+nx);
cleanupsysic = 'yes';
GP = sysic; % compose the defined setup
NMEAS = ny+nx; % number of measured outputs
NCON = nu; % number of control inputs
%% Controller Synthesis
[K,CL,gam1,INFO] = hinfsyn(GP,NMEAS,NCON,'METHOD','LMI');
%% Simulation Setup
% Define the topology
L = <FIXME>;
Lq = kron(L,eye(ny));
% Define the reference
ref = [2 4 6 8]';
iny = [1; 0; 0];
r = kron(eye(4),iny)*ref;
\ No newline at end of file
%% Design of a Formation Controller
% -------------------------------------------------------------------------
% script : exercise_MAS_qcopter
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : December 10th, 2013
% Copyright: MB, 2013
% -------------------------------------------------------------------------
%
% 1. Define the agent model given in the appendix of the Lecture Notes
% 2. Formulate agent + uncertain topology in LFT form
% 3. Generate a generalized plant
% 4. Synthesize a robust formation controller
%
% -------------------------------------------------------------------------
%% state space model of the quadrocopter
g = 9.81; % Gravity constant
m = 0.640; % Mass of the Quadrocopter
A = [ 0 1 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 -g 0 0 0 ;
0 0 0 1 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 g 0 ;
0 0 0 0 0 1 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 1 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 1 ;
0 0 0 0 0 0 0 0 0 0 0 0 ] ;
B = [ 0 0 0 0 0 1/m 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 1 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 1 0 0 ;
0 0 0 0 0 0 0 0 0 0 0 1 ]';
nx = size(A,1); % number of states
C = zeros(3,nx);
C(1,1) = 1; % x
C(2,3) = 1; % y
C(3,5) = 1; % z
nu = size(B,2); % number of inputs
ny = size(C,1); % number of outputs
D = zeros(ny,nu);
P = ss(A,B,C,D);
Paug = ss(A,B,[C; eye(nx)], [D; zeros(nx,nu)]);
% Augmented plant with state vector as additional output
%% setup of the generalized plant
% Factorize C
[U,S,V] = svd(C);
Dd = U*S;
Cd = V';
nd = size(Cd,1); % size of Delta
% Generate an LFT model of one agent
Blft = [zeros(nx,nd), B];
Clft = [Cd; C; eye(nx)];
Dlft = [zeros(nd,nd), zeros(nd,nu);
Dd, zeros(ny,nu);
zeros(nx,nd), zeros(nx,nu)];
Plft = ss(A,Blft,Clft,Dlft);
% Define the shaping filters
omega_S = 1e-3;
M_S = 1e-3;
omega_KS = 1e3;
M_KS = 10;
c_KS = 1e3;
w_S = tf(omega_S /M_S , [1 omega_S]);
Ws = diag(ones(1,ny))*w_S;
w_KS = (c_KS/M_KS) * tf([1 omega_KS],[1 c_KS*omega_KS]);
Wk = diag(ones(1,nu))*w_KS;
% Compose the generalized plant
systemnames = 'Plft Ws Wk';
inputvar = sprintf('[wd(%d); r(%d); u(%d)]', nd,ny,nu);
input_to_Plft = '[wd; u]';
input_to_Ws = sprintf('[r-Plft(%d:%d)]', nd+1,nd+ny);
input_to_Wk = '[u]';
outputvar = sprintf('[Plft(1:%d); Ws; Wk; r-Plft(%d:%d); -Plft(%d:%d)]', nd,nd+1,nd+ny,nd+ny+1,nd+ny+nx);
cleanupsysic = 'yes';
GP = sysic; % compose the defined setup
NMEAS = ny+nx; % number of measured outputs
NCON = nu; % number of control inputs
%% Controller Synthesis
[K,CL,gam1,INFO] = hinfsyn(GP,NMEAS,NCON,'METHOD','LMI');
%% Simulation Setup
% Define the topology
L = [1, 0, -1, 0;
0, 1, -1, 0;
-1/2, -1/2, 1, 0;
-1/2, 0, -1/2, 1];
Lq = kron(L,eye(ny));
% Define the reference
ref = [2 4 6 8]';
iny = [1; 0; 0]; % Assign 'ref' values to x-direction of every agent
r = kron(eye(4),iny)*ref;
\ No newline at end of file
%% Information Flow Filter for Formation Control
% -------------------------------------------------------------------------
% script : exercise_MAS_IFFdesign
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : December 10th, 2015
% Copyright: MB, 2015
% -------------------------------------------------------------------------
%
% 1. Formulate information flow filter + uncertain topology in LFT form
% 2. Generate a generalized plant
% 3. Synthesize a robust formation controller
%
% IMPORTANT: This is the exercise version.
% Lines marked with <FIXME> are to be filled by you!
% Unless you do so, the script will not be executable.
%
% -------------------------------------------------------------------------
%% Shaping filter definition
Wsf = <FIXME>;
%% Generalized Plant setup
Pf = tf(1); % Here an auxiliary filter could be inserted to make generalized plant strictly proper
systemnames = 'Wsf Pf';
inputvar = sprintf('[w(%d); r(%d); p(%d)]', 1,1,1);
input_to_Pf = sprintf('[w+r-p]');
input_to_Wsf = sprintf('[Pf]');
outputvar = sprintf('[r-p; Wsf; Pf]');
cleanupsysic = 'yes';
GPf = sysic; % compose the defined setup
NMEAS = 1; % number of measured outputs
NCON = 1; % number of control inputs
%% Information Flow Filter synthesis
[f,CL,gam1,INFO] = hinfsyn(GPf,NMEAS,NCON,'METHOD','LMI');
%% Simulation setup
L = [1, -1/2, -1/2, 0, 0;
0, 1, 0, -1, 0;
-1/2, 0, 1, -1/2, 0;
0 -1/2, -1/2, 1, 0;
0 -1/2, 0, -1/2, 1];
\ No newline at end of file
%% Information Flow Filter for Formation Control
% -------------------------------------------------------------------------
% script : exercise_MAS_IFFdesign
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : December 10th, 2015
% Copyright: MB, 2015
% -------------------------------------------------------------------------
%
% 1. Formulate information flow filter + uncertain topology in LFT form
% 2. Generate a generalized plant
% 3. Synthesize a robust formation controller
%
% -------------------------------------------------------------------------
%% Shaping filter definition
omega_Sf = 1e-2;
M_Sf = 3e-2;
Wsf = tf(omega_Sf /M_Sf , [1 omega_Sf]);
%% Generalized Plant setup
Pf = tf(1); % Here an auxiliary filter could be inserted to make generalized plant strictly proper
systemnames = 'Wsf Pf';
inputvar = sprintf('[w(%d); r(%d); p(%d)]', 1,1,1);
input_to_Pf = sprintf('[w+r-p]');
input_to_Wsf = sprintf('[Pf]');
outputvar = sprintf('[r-p; Wsf; Pf]');
cleanupsysic = 'yes';
GPf = sysic; % compose the defined setup
NMEAS = 1; % number of measured outputs
NCON = 1; % number of control inputs
%% Information Flow Filter synthesis
[f,CL,gam1,INFO] = hinfsyn(GPf,NMEAS,NCON,'METHOD','LMI');
%% Simulation setup
L = [1, -1/2, -1/2, 0, 0;
0, 1, 0, -1, 0;
-1/2, 0, 1, -1/2, 0;
0 -1/2, -1/2, 1, 0;
0 -1/2, 0, -1/2, 1];
% modified Laplacian for additional agent
L6 = [ 1, -1/3, -1/3, 0, 0, -1/3;
0, 1, 0, -1/2, 0, -1/2;
-1/2, 0, 1, -1/2, 0, 0;
0 -1/2, -1/2, 1, 0, 0;
0 -1/2, 0, -1/2, 1, 0;
0, 0, 0, 0, 0, 1];
\ No newline at end of file
%% Disturbance Rejection in Formation Control
% -------------------------------------------------------------------------
% script : exercise_MAS_IFFdist
% -------------------------------------------------------------------------
% Author : Marcus Bartels
% Version : January 12th, 2015
% Copyright: MB, 2015
% -------------------------------------------------------------------------
%% Initialization
close all
% Define number of agents
N = 5;
% Define plant model
m = 10;
b = 0.5;
c = 0;
PA = [0, 1; -c/m, -b/m];
PB = [0; 1/m];
PC = [1 0];
PD = 0;
P = ss(PA,PB,PC,PD);
nx = size(PA,1); % number of states
% Define controller
K = 20*tf([1 0.05],[1 2.4]);
[KA,KB,KC,KD] = ssdata(K);
% Define the Laplacian
L = [1, -1/2, -1/2, 0, 0;
0, 1, 0, -1, 0;
-1/2, 0, 1, -1/2, 0;
0 -1/2, -1/2, 1, 0;
0 -1/2, 0, -1/2, 1];
la = sort(eig(L));
%% setup information flow filter and local controller
f = tf([1 8.5],[1e-4 1 0.01]);
Kl = 0.15*tf([60 1],[0.6 1]);
%% modified Laplacian for additional agent
L6 = [ 1, -1/3, -1/3, 0, 0, -1/3;
0, 1, 0, -1/2, 0, -1/2;
-1/2, 0, 1, -1/2, 0, 0;
0 -1/2, -1/2, 1, 0, 0;
0 -1/2, 0, -1/2, 1, 0;
0, 0, 0, 0, 0, 1];
\ No newline at end of file
% -------------------------------------------------------------------------
% function : A2L
% -------------------------------------------------------------------------
% Author : Annika Eichler
% Version : November, 27th 2013
% Copyright: AE, 2013
% -------------------------------------------------------------------------
%
% Get Laplacian from Adjacency Matrix
%
% -------------------------------------------------------------------------
function [L]= A2L(A)
Degree=sum(A,2);
L=diag(Degree)-A;
end
\ No newline at end of file
% -------------------------------------------------------------------------
% function : A2L
% -------------------------------------------------------------------------
% Author : Annika Eichler
% Version : November, 27th 2013
% Copyright: AE, 2013
% -------------------------------------------------------------------------
%
% Get Adjacency from Laplacian Matrix
%
% -------------------------------------------------------------------------
function [A]= L2A(L)
A=-(L-diag(diag(L)));
end
\ No newline at end of file
% -------------------------------------------------------------------------
% function : getConvRate
% -------------------------------------------------------------------------
% Author : Annika Eichler
% Version : November, 27th 2013
% Copyright: AE, 2013
% -------------------------------------------------------------------------
%
% Get convergence rate
%
% INPUTS:
% L = Laplacian Matrix
%
% OUTPUTS
% cR = convergence rate
%
% -------------------------------------------------------------------------
function [cR]= getConvRate(L)
eigL = eig(L);
[~,IX] = sort(real(eigL));
eigL = eigL(IX);
cR = eigL(2);
end
\ No newline at end of file
% -------------------------------------------------------------------------
% function : getGraphDirected
% -------------------------------------------------------------------------
% Author : Annika Eichler
% Version : November, 27th 2013
% Copyright: AE, 2013
% -------------------------------------------------------------------------
%
% Get random directed graph with n nodes and m edges
%
% INPUTS:
% n = number of agents
% m = number of edges
%
% OUTPUTS
% L = Laplacian Matrix
% A = Adjacency Matrix
%
% -------------------------------------------------------------------------
function [L, A]= getGraphDirected(n, m)
%% check the number of edges
if m<n-1
error('There are too less links to get a connected graph')
end
if m>n*(n-1)
error('There are too many links given')
end
%% start to define A
A=zeros(n,n);
tree_nodes=1;
ntree_nodes=2:n;
%% neccessary edges to have a rooted out-branching graph
for ind=1:n-1
end_node_ind = randi(length(ntree_nodes),1);
end_node=ntree_nodes(end_node_ind);
start_node=tree_nodes(randi(length(tree_nodes),1));
A(end_node,start_node)=1;
tree_nodes=[tree_nodes, end_node];
ntree_nodes(end_node_ind)=[];
end
%% add the missing edges
[row,col] = find(A==0);
for ind=n:m
newEdgeInd = randi(length(row),1);
A(row(newEdgeInd),col(newEdgeInd))=1;
row(newEdgeInd) = [];
col(newEdgeInd) = [];
end
%% get the Laplacian
L=A2L(A);
end
% -------------------------------------------------------------------------
% function : getGraphUndirected
% -------------------------------------------------------------------------
% Author : Annika Eichler
% Version : November, 27th 2013
% Copyright: AE, 2013
% -------------------------------------------------------------------------
%
% Get random undirected graph with n nodes and m edges
%
% INPUTS:
% n = number of agents
% m = number of edges
%
% OUTPUTS
% L = Laplacian Matrix
% A = Adjacency Matrix
%
% -------------------------------------------------------------------------
function [L, A]= getGraphUndirected(n, m)
if m<n-1
error('There are too less links to get a connected graph')
end
if m>n*(n-1)/2