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
|
% 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');
fprintf('Generating system parameters...\n')
params = uav_params();
ctrl = struct();
% Flags to speed up running for debugging
do_plots = true;
do_lqr = false; % unused
do_hinf = false; % midterm
do_musyn = true; % endterm
% ------------------------------------------------------------------------
%% 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]));
nmeas = max(size(idx.OutputNominal)); % size of y
nctrl = max(size(idx.InputNominal)); % size of u
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:
|
