1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
|
% Controller design for a ducted fan VTOL micro-UAV.
%
% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt
% ------------------------------------------------------------------------
% Clear environment and generate parameters
clear; clc; close all; s = tf('s');
% Flags to speed up running for debugging
do_plots = true;
do_lqr = false; % unused
do_hinf = true; % midterm
do_musyn = true; % endterm
fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
fprintf('Will do:\n')
if do_plots
fprintf(' - Produce plots\n')
end
if do_lqr
fprintf(' - LQR synthesis\n')
end
if do_hinf
fprintf(' - H-infinity synthesis\n')
end
if do_musyn
fprintf(' - Mu synthesis\n')
end
% Synthesized controllers will be stored here
ctrl = struct();
% ------------------------------------------------------------------------
%% Define system parameters
fprintf('Generating system parameters...\n')
params = uav_params();
% ------------------------------------------------------------------------
%% Define performance requirements
if do_hinf | do_musyn
fprintf('Generating performance requirements...\n')
perf = uav_performance(params, do_plots);
end
% ------------------------------------------------------------------------
%% Define stability requirements
if do_musyn
fprintf('Generating stability requirements...\n')
uncert = uav_uncertainty(params, do_plots);
end
% ------------------------------------------------------------------------
%% Create UAV model
fprintf('Generating system model...\n');
model = uav_model(params, perf, uncert);
% ------------------------------------------------------------------------
%% Perform LQR design
if do_lqr
fprintf('Performing LQR controller design...\n')
ctrl.lqr = uav_ctrl_lqr(params, model);
end
% ------------------------------------------------------------------------
%% Perform H-infinity design
if do_hinf
fprintf('Performing H-infinty controller design...\n')
idx = model.uncertain.index;
P = model.uncertain.StateSpace;
% Get nominal system without uncertainty (for lower LFT)
P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ...
[idx.InputExogenous; idx.InputNominal]), [], false);
nmeas = model.uncertain.Ny;
nctrl = model.uncertain.Nu;
hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
'AutoScale', 'off', 'RelTol', 1e-3);
[K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);
if gamma >= 1
fprintf('Failed to syntesize controller (closed loop is unstable).\n')
end
% ------------------------------------------------------------------------
%% Measure Performance of H-infinity design
fprintf('Simulating closed loop...\n');
nsamples = 500;
do_noise = true;
simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);
fprintf('Writing simulation results...\n');
cols = [
simout.StepX(:, simout.index.Position), ...
simout.StepX(:, simout.index.Velocity), ...
simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
simout.StepX(:, simout.index.Angles) * 180 / pi];
writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
end
% ------------------------------------------------------------------------
%% Perform mu-Analysis & DK iteration
if do_musyn
fprintf('Performing mu-synthesis controller design...\n')
idx = model.uncertain.index;
P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
[idx.InputUncertain; idx.InputExogenous; idx.InputNominal]);
% Initial values for D-K iteration
D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny);
D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu);
% Options for H-infinity
nmeas = model.uncertain.Ny;
nctrl = model.uncertain.Nu;
hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
'AutoScale', 'off', 'RelTol', 1e-3);
% Number of D-K iterations
niters = 4;
% Frequency raster resolution to fit D scales
nsamples = 51;
omega = logspace(-2, 2, nsamples);
% Start DK-iteration
for it = 1:niters
fprintf(' - Running D-K iteration %d ...\n', it);
% Find controller using H-infinity
[K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl);
fprintf(' H-infinity synthesis gamma: %g\n', gamma);
% Calculate frequency response of closed loop
N = minreal(lft(P, K), [], false); % slient
N_frd = frd(N, omega);
% Calculate upper bound D scaling
[mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU');
mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
fprintf(' Mu value for RP: %g\n', mu_rp)
% Fit D-scales
[dsysl, dsysr] = mussvunwrap(mu_info);
D_left = fitfrd(genphase(dsysl(1,1)), 1);
D_right = fitfrd(genphase(dsysr(1,1)), 1);
end
end
% ------------------------------------------------------------------------
%% Verify performance satisfaction via mu-analysis
% omega = logspace(-3, 3, 250);
%
% N_inf = lft(P, K_inf);
% N_inf_frd = frd(N_inf, omega);
%
% % robust stability
% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ...
% model.uncertain.BlockStructure);
%
% % robust performance
% blk_perf = [model.uncertain.BlockStructure;
% model.uncertain.BlockStructurePerf];
%
% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf);
%
% % nominal performance
% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous));
%
% if do_plots
% figure; hold on;
% bodemag(mu_inf_rs(1));
% bodemag(mu_inf_np(1));
% bodemag(mu_inf_rs(2));
% bodemag(mu_inf_np(2));
%
% grid on;
% title('$\mu_\Delta(N)$', 'Interpreter', 'latex');
% end
% vim: ts=2 sw=2 et:
|
