Commit 5975216b by Patrick Göttsch

### 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 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 = ; Cd = ; 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 = ; Ws = diag(ones(1,ny))*w_S; w_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 = ; 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