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
|
% 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
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:
|
