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
|
% Generate transfer functions for loop shaping performance requirements
% from parameters specified in uav_params.m
%
% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt
%
% Arguments:
% PARAMS Struct of design parameters and constants generated by uav_params
% PLOT When set to 'true' it plots the inverse magnitude of the
% performance transfer function
%
% Return value:
% PERF Struct performance transfer functions
function [perf] = uav_performance_hinf(params, do_plots)
% Laplace variable
s = tf('s');
% Bandwitdhs
T_alpha = params.actuators.ServoSecondsTo60Deg;
T_omega = 0.1;
T_xy = 5;
T_z = 4;
% Inverse performance functions
W_Palpha = tf(.8);
W_Pomega = tf(.5);
W_Ralpha = 1 / ((s * T_alpha)^2 + 2 * T_alpha * s + 1);
W_Romega = 1 / ((s * T_omega)^2 + 2 * T_omega * 1.2 * s + 1);
W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
W_Rz = 1 / ((s * T_z)^2 + 2 * T_z * s + 1);
W_Pxy = tf(12);
W_Pz = tf(1);
W_Pxydot = tf(.01);
W_Pzdot = tf(.01);
W_Pphitheta = tf(.005);
W_Ppsi = tf(.3);
% Construct performance vector by combining xy and z
W_Rref = blkdiag(W_Rxy * eye(2), W_Rz);
W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
perf = struct(...
'FlapAngle', W_Palpha * eye(4), ...
'Thrust', W_Pomega, ...
'ReferenceFilter', W_Rref, ...
'FlapAngleFilter', W_Ralpha * eye(4), ...
'ThrusterFilter', W_Romega, ...
'Position', W_PP, ...
'Velocity', W_PPdot, ...
'Angles', W_PTheta ...
);
if do_plots
% Bode plots of performance requirements
figure; hold on;
bodemag(W_Palpha);
bodemag(W_Pomega);
bodemag(W_Rxy);
bodemag(W_Rz);
bodemag(W_Pxy);
bodemag(W_Pz);
bodemag(W_Pxydot);
bodemag(W_Pzdot);
bodemag(W_Pphitheta);
bodemag(W_Ppsi);
grid on;
legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
'$W_{R,xy}$', '$W_{R,z}$', ...
'$W_{P,xy}$', '$W_{P,z}$', ...
'$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
'$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
'interpreter', 'latex', 'fontSize', 8);
title('\bfseries Performance Requirements ($\mathcal{H}_\infty$ Weights)', ...
'interpreter', 'latex');
% Step response of position requirements
figure; hold on;
step(W_Pxy); step(W_Pz);
step(W_Rxy); step(W_Rz);
step(W_Pxydot); step(W_Pzdot);
step(W_Palpha);
step(W_Pomega);
grid on;
legend('$W_{P,xy}$', '$W_{P,z}$', ...
'$W_{R,xy}$', '$W_{R,z}$', ...
'$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
'$W_{P,\alpha}$', '$W_{P,\omega}$', ...
'interpreter', 'latex', 'fontSize', 8);
title('\bfseries Step responses of $\mathcal{H}_\infty$ Weights', ...
'interpreter', 'latex');
end
end
% Make a n-order performance weight function
%
% Arguments:
% OMEGA Cutting frequency (-3dB)
% A Magnitude at DC, i.e. |Wp(0)|
% M Magnitude at infinity, i.e. |Wp(inf)|
% ORD Order
function [Wp] = make_weight(omega, A, M, ord)
if nargin > 3
n = ord;
else
n = 1;
end
s = tf('s');
Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n;
end
% vim: ts=2 sw=2 et:
|
