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
|
% 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();
do_plots = true;
% ------------------------------------------------------------------------
%% Define performance requirements
fprintf('Generating performance requirements...\n')
perf = uav_requirements(params, do_plots);
% ------------------------------------------------------------------------
%% Define stability requirements
W_malpha = tf(1,1);
W_momega = tf(1,1);
W_mState = tf(1,1);
% if do_plots
% figure; hold on;
%
% bodemag(W_malpha);
% bodemag(W_momega);
% bodemag(W_mState);
%
% grid on;
% legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
% '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
% 'Interpreter', 'latex', 'fontSize', 8);
% title('Uncertainties')
% end
uncert = struct(...
'FlapAngle', W_malpha * eye(4), ...
'Thrust', W_momega, ...
'StateLinApprox', W_mState * eye(12));
% ------------------------------------------------------------------------
%% Create UAV model
fprintf('Generating system model...\n');
model = uav_model(params, perf, uncert);
% ------------------------------------------------------------------------
%% Perform LQR design
% fprintf('Performing LQR controller design...\n')
% ctrl.lqr = uav_ctrl_lqr(params, model);
% ------------------------------------------------------------------------
%% Perform H-infinity design
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
fprintf('Simulating closed loop...\n');
nsamples = 500;
do_noise = true;
% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots);
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')
% ------------------------------------------------------------------------
%% 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
% ------------------------------------------------------------------------
% Perform mu-Analysis & DK iteration
% vim: ts=2 sw=2 et:
|
