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

Gitlab Runner Update from atc-aux-files/master:Merge branch 'update_63_64_65' into 'master'

parent 5494e58c
%% 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
%
% -------------------------------------------------------------------------
%% 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment