add all files
This commit is contained in:
123
B3-control/detail_control_1_complementary_filtering.m
Normal file
123
B3-control/detail_control_1_complementary_filtering.m
Normal file
@@ -0,0 +1,123 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./src/'); % Path for functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Initialize Frequency Vector
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
%% Weighting Function Design
|
||||
% Parameters
|
||||
n = 3; w0 = 2*pi*10; G0 = 1e-3; G1 = 1e1; Gc = 2;
|
||||
|
||||
% Formulas
|
||||
W = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
|
||||
|
||||
% Function generateWF can be used to easily design the weighting filters
|
||||
% W = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1e-3, 'Ginf', 10, 'Gc', 2);
|
||||
|
||||
%% Magnitude of the weighting function with parameters
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(W, freqs, 'Hz'))), 'k-');
|
||||
|
||||
plot([1e-3 1e0], [G0 G0], 'k--', 'LineWidth', 1)
|
||||
text(1e0, G0, '$\quad G_0$')
|
||||
|
||||
plot([1e1 1e3], [G1 G1], 'k--', 'LineWidth', 1)
|
||||
text(1e1,G1,'$G_{\infty}\quad$','HorizontalAlignment', 'right')
|
||||
|
||||
plot([w0/2/pi w0/2/pi], [1 2*Gc], 'k--', 'LineWidth', 1)
|
||||
text(w0/2/pi,1,'$\omega_c$','VerticalAlignment', 'top', 'HorizontalAlignment', 'center')
|
||||
|
||||
plot([w0/2/pi/2 2*w0/2/pi], [Gc Gc], 'k--', 'LineWidth', 1)
|
||||
text(w0/2/pi/2, Gc, '$G_c \quad$','HorizontalAlignment', 'right')
|
||||
|
||||
text(w0/5/pi/2, abs(evalfr(W, j*w0/5)), 'Slope: $n \quad$', 'HorizontalAlignment', 'right')
|
||||
|
||||
text(w0/2/pi, abs(evalfr(W, j*w0)), '$\bullet$', 'HorizontalAlignment', 'center')
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
hold off;
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
ylim([5e-4, 20]);
|
||||
yticks([1e-4, 1e-3, 1e-2, 1e-1, 1, 1e1]);
|
||||
|
||||
%% Synthesis of Complementary Filters using H-infinity synthesis
|
||||
% Design of the Weighting Functions
|
||||
W1 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.45);
|
||||
W2 = generateWF('n', 2, 'w0', 2*pi*10, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.45);
|
||||
|
||||
% Generalized Plant
|
||||
P = [W1 -W1;
|
||||
0 W2;
|
||||
1 0];
|
||||
|
||||
% H-Infinity Synthesis
|
||||
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||||
|
||||
% Define H1 to be the complementary of H2
|
||||
H1 = 1 - H2;
|
||||
|
||||
% The function generateCF can also be used to synthesize the complementary filters.
|
||||
% [H1, H2] = generateCF(W1, W2);
|
||||
|
||||
%% Bode plot of the Weighting filters and Obtained complementary filters
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'color', colors(1,:),'DisplayName', '$|W_1|^{-1}$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'color', colors(2,:),'DisplayName', '$|W_2|^{-1}$');
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5,'DisplayName', '$H_1$');
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5,'DisplayName', '$H_2$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([8e-4, 20]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
leg = legend('location', 'south', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Synthesis of a set of three complementary filters
|
||||
% Design of the Weighting Functions
|
||||
W1 = generateWF('n', 2, 'w0', 2*pi*1, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.5);
|
||||
W2 = 0.22*(1 + s/2/pi/1)^2/(sqrt(1e-4) + s/2/pi/1)^2*(1 + s/2/pi/10)^2/(1 + s/2/pi/1000)^2;
|
||||
W3 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.5);
|
||||
|
||||
% Generalized plant for the synthesis of 3 complementary filters
|
||||
P = [W1 -W1 -W1;
|
||||
0 W2 0 ;
|
||||
0 0 W3;
|
||||
1 0 0];
|
||||
|
||||
% Standard H-Infinity Synthesis
|
||||
[H, ~, gamma, ~] = hinfsyn(P, 1, 2,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||||
|
||||
% Synthesized H2 and H3 filters
|
||||
H2 = tf(H(1));
|
||||
H3 = tf(H(2));
|
||||
|
||||
% H1 is defined as the complementary filter of H2 and H3
|
||||
H1 = 1 - H2 - H3;
|
||||
|
||||
%% Bode plot of the inverse weighting functions and of the three complementary filters obtained using the H-infinity synthesis
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'color', colors(1,:),'DisplayName', '$|W_1|^{-1}$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'color', colors(2,:),'DisplayName', '$|W_2|^{-1}$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), '--', 'color', colors(3,:),'DisplayName', '$|W_3|^{-1}$');
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5,'DisplayName', '$H_1$');
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5,'DisplayName', '$H_2$');
|
||||
plot(freqs, abs(squeeze(freqresp(H3, freqs, 'Hz'))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5,'DisplayName', '$H_3$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
xlim([freqs(1), freqs(end)]); ylim([1e-4, 20]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
248
B3-control/detail_control_2_decoupling.m
Normal file
248
B3-control/detail_control_2_decoupling.m
Normal file
@@ -0,0 +1,248 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./src/'); % Path for functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Initialize Frequency Vector
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
%% Compute Equation of motion
|
||||
l = 1; h=2;
|
||||
la = 0.5; % Horizontal position of actuators [m]
|
||||
ha = 0.2; % Vertical of actuators [m]
|
||||
|
||||
m = 40; % Payload mass [kg]
|
||||
I = 5; % Payload rotational inertia [kg m^2]
|
||||
|
||||
c = 2e2; % Actuator Damping [N/(m/s)]
|
||||
k = 1e6; % Actuator Stiffness [N/m]
|
||||
|
||||
% Unit vectors of the actuators
|
||||
s1 = [1;0];
|
||||
s2 = [0;1];
|
||||
s3 = [0;1];
|
||||
|
||||
% Stiffnesss and Damping matrices of the struts
|
||||
Kr = diag([k,k,k]);
|
||||
Cr = diag([c,c,c]);
|
||||
|
||||
% Location of the joints with respect to the center of mass
|
||||
Mb1 = [-l/2;-ha];
|
||||
Mb2 = [-la; -h/2];
|
||||
Mb3 = [ la; -h/2];
|
||||
|
||||
% Jacobian matrix (Center of Mass)
|
||||
J_CoM = [s1', Mb1(1)*s1(2)-Mb1(2)*s1(1);
|
||||
s2', Mb2(1)*s2(2)-Mb2(2)*s2(1);
|
||||
s3', Mb3(1)*s3(2)-Mb3(2)*s3(1)];
|
||||
|
||||
% Mass Matrix in frame {M}
|
||||
M = diag([m,m,I]);
|
||||
|
||||
% Stiffness Matrix in frame {M}
|
||||
K = J_CoM'*Kr*J_CoM;
|
||||
|
||||
% Damping Matrix in frame {M}
|
||||
C = J_CoM'*Cr*J_CoM;
|
||||
|
||||
% Plant in the frame of the struts
|
||||
G_L = J_CoM*inv(M*s^2 + C*s + K)*J_CoM';
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
for out_i = 1:3
|
||||
for in_i = 1:3
|
||||
nexttile;
|
||||
plot(freqs, abs(squeeze(freqresp(G_L(out_i,in_i), freqs, 'Hz'))), 'k-', ...
|
||||
'DisplayName', sprintf('$\\mathcal{L}_%i/\\tau_%i$', out_i, in_i));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlim([freqs(1), freqs(end)]); ylim([2e-8, 4e-5]);
|
||||
xticks([1e0, 1e1, 1e2])
|
||||
yticks([1e-7, 1e-6, 1e-5])
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
if in_i == 1
|
||||
ylabel('Mag. [m/N]')
|
||||
else
|
||||
set(gca, 'YTickLabel',[]);
|
||||
end
|
||||
|
||||
if out_i == 3
|
||||
xlabel('Frequency [Hz]')
|
||||
else
|
||||
set(gca, 'XTickLabel',[]);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%% Jacobian Decoupling - Center of Mass
|
||||
G_CoM = pinv(J_CoM)*G_L*pinv(J_CoM');
|
||||
G_CoM.InputName = {'Fx', 'Fy', 'Mz'};
|
||||
G_CoM.OutputName = {'Dx', 'Dy', 'Rz'};
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 3), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$D_{x,\{M\}}/M_{z,\{M\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 1), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$R_{z,\{M\}}/F_{x,\{M\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_{x,\{M\}}/F_{x,\{M\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$D_{y,\{M\}}/F_{y,\{M\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$R_{z,\{M\}}/M_{z,\{M\}}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([1e-10, 1e-3]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Jacobian Decoupling - Center of Mass
|
||||
% Location of the joints with respect to the center of stiffness
|
||||
Mb1 = [-l/2; 0];
|
||||
Mb2 = [-la; -h/2+ha];
|
||||
Mb3 = [ la; -h/2+ha];
|
||||
|
||||
% Jacobian matrix (Center of Stiffness)
|
||||
J_CoK = [s1', Mb1(1)*s1(2)-Mb1(2)*s1(1);
|
||||
s2', Mb2(1)*s2(2)-Mb2(2)*s2(1);
|
||||
s3', Mb3(1)*s3(2)-Mb3(2)*s3(1)];
|
||||
|
||||
G_CoK = pinv(J_CoK)*G_L*pinv(J_CoK');
|
||||
G_CoK.InputName = {'Fx', 'Fy', 'Mz'};
|
||||
G_CoK.OutputName = {'Dx', 'Dy', 'Rz'};
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_{x,\{K\}}/F_{x,\{K\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$D_{y,\{K\}}/F_{y,\{K\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$R_{z,\{K\}}/M_{z,\{K\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 3), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$D_{x,\{K\}}/M_{z,\{K\}}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 1), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$R_{z,\{K\}}/F_{x,\{K\}}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Kagnitude');
|
||||
ylim([1e-10, 1e-3]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Modal decoupling
|
||||
% Compute the eigen vectors
|
||||
[phi, wi] = eig(M\K);
|
||||
% Sort the eigen vectors by increasing associated frequency
|
||||
[~, i] = sort(diag(wi));
|
||||
phi = phi(:, i);
|
||||
|
||||
% Plant in the modal space
|
||||
Gm = inv(phi)*inv(J_CoM)*G_L*inv(J_CoM')*inv(phi');
|
||||
|
||||
%% Modal decoupled plant
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gm(1,1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$\mathcal{X}_{m,1}/\tau_{m,1}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gm(2,2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$\mathcal{X}_{m,2}/\tau_{m,2}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gm(3,3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$\mathcal{X}_{m,3}/\tau_{m,3}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([1e-8, 1e-4]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% SVD Decoupling
|
||||
wc = 2*pi*100; % Decoupling frequency [rad/s]
|
||||
% System's response at the decoupling frequency
|
||||
H1 = evalfr(G_L, j*wc);
|
||||
|
||||
% Real approximation of G(j.wc)
|
||||
D = pinv(real(H1'*H1));
|
||||
H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
|
||||
|
||||
[U,S,V] = svd(H1);
|
||||
|
||||
Gsvd = inv(U)*G_L*inv(V');
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i_in = 1:3
|
||||
for i_out = [i_in+1:3]
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$G_{SVD}(i,j)\ i \neq j$');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
for i_in_out = 1:3
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_{SVD}(%d,%d)$', i_in_out, i_in_out));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([1e-10, 2e-4]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Simscape model with relative motion sensor at alternative positions
|
||||
mdl = 'detail_control_decoupling_test_model';
|
||||
open(mdl)
|
||||
|
||||
deq = 0.2; % Length of the actuators [m]
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F1'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/F2'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/F3'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Payload'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Payload'], 2, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Payload'], 3, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
G_L_alt = linearize(mdl, io);
|
||||
G_L_alt.InputName = {'F1', 'F2', 'F3'};
|
||||
G_L_alt.OutputName = {'d1', 'd2', 'd32'};
|
||||
|
||||
% SVD Decoupling with the new plant
|
||||
wc = 2*pi*100; % Decoupling frequency [rad/s]
|
||||
% System's response at the decoupling frequency
|
||||
H1 = evalfr(G_L_alt, j*wc);
|
||||
|
||||
% Real approximation of G(j.wc)
|
||||
D = pinv(real(H1'*H1));
|
||||
H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
|
||||
|
||||
[U,S,V] = svd(H1);
|
||||
|
||||
Gsvd_alt = inv(U)*G_L_alt*inv(V');
|
||||
|
||||
%% Obtained plant after SVD decoupling - Relative motion sensors are not collocated with the actuators
|
||||
figure;
|
||||
hold on;
|
||||
for i_in = 1:3
|
||||
for i_out = [i_in+1:3]
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
end
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'DisplayName', '$G_{SVD}(i,j)\ i \neq j$');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
for i_in_out = 1:3
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_{SVD}(%d,%d)$', i_in_out, i_in_out));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([5e-11, 7e-5]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
238
B3-control/detail_control_3_close_loop_shaping.m
Normal file
238
B3-control/detail_control_3_close_loop_shaping.m
Normal file
@@ -0,0 +1,238 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./src/'); % Path for functions
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Initialize Frequency Vector
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
%% Analytical Complementary Filters - Effect of alpha
|
||||
freqs_study = logspace(-2, 2, 1000);
|
||||
alphas = [0.1, 1, 10];
|
||||
w0 = 2*pi*1;
|
||||
s = tf('s');
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(alphas)
|
||||
alpha = alphas(i);
|
||||
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$\\alpha = %g$', alphas(i)));
|
||||
plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Magnitude');
|
||||
hold off;
|
||||
ylim([1e-3, 20]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Analytical Complementary Filters - Effect of w0
|
||||
freqs_study = logspace(-1, 3, 1000);
|
||||
alpha = [1];
|
||||
w0s = [2*pi*1, 2*pi*10, 2*pi*100];
|
||||
s = tf('s');
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(w0s)
|
||||
w0 =w0s(i);
|
||||
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'color', colors(i,:), 'DisplayName', sprintf('$\\omega_0 = %g$ Hz', w0/2/pi));
|
||||
plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'color', colors(i,:), 'HandleVisibility', 'off');
|
||||
end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
hold off;
|
||||
xlim([freqs_study(1), freqs_study(end)]); ylim([1e-3, 20]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Test model
|
||||
freqs = logspace(0, 3, 1000); % Frequency Vector [Hz]
|
||||
|
||||
m = 20; % mass [kg]
|
||||
k = 1e6; % stiffness [N/m]
|
||||
c = 1e2; % damping [N/(m/s)]
|
||||
|
||||
% Plant dynamics
|
||||
G = 1/(m*s^2 + c*s + k);
|
||||
|
||||
% Uncertainty weight
|
||||
wI = generateWF('n', 2, 'w0', 2*pi*50, 'G0', 0.1, 'Ginf', 10, 'Gc', 1);
|
||||
|
||||
%% Bode plot of the plant with dynamical uncertainty
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2,1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'k-', 'DisplayName', 'G');
|
||||
plotMagUncertainty(wI, freqs, 'G', G, 'DisplayName', '$\Pi_i$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-8, 7e-5]);
|
||||
hold off;
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plotPhaseUncertainty(wI, freqs, 'G', G);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))), 'k-');
|
||||
set(gca,'xscale','log');
|
||||
yticks(-360:90:90);
|
||||
ylim([-270 45]);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Analytical Complementary Filters
|
||||
w0 = 2*pi*20;
|
||||
alpha = 1;
|
||||
|
||||
Hh = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
Hl = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||||
|
||||
%% Specifications
|
||||
figure;
|
||||
hold on;
|
||||
plot([1, 100], [0.01, 100], ':', 'color', colors(2,:));
|
||||
plot([300, 1000], [0.01, 0.01], ':', 'color', colors(1,:));
|
||||
plot(freqs, 1./abs(squeeze(freqresp(wI, freqs, 'Hz'))), ':', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), '-', 'color', colors(1,:));
|
||||
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), '-', 'color', colors(2,:));
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
hold off;
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
ylim([1e-3, 10]);
|
||||
xticks([0.1, 1, 10, 100, 1000]);
|
||||
|
||||
%% Obtained controller
|
||||
omega = 2*pi*1000;
|
||||
|
||||
K = 1/(Hh*G) * 1/((1+s/omega+(s/omega)^2));
|
||||
K = zpk(minreal(K));
|
||||
|
||||
%% Bode plot of the controller K
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
plot(freqs, abs(squeeze(freqresp(K*Hl, freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
||||
ylim([8e3, 1e8])
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(K*Hl, freqs, 'Hz'))), 'k-');
|
||||
set(gca,'xscale','log');
|
||||
yticks(-180:45:180);
|
||||
ylim([-180 45]);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
num_delta_points = 50;
|
||||
theta = linspace(0, 2*pi, num_delta_points);
|
||||
delta_points = exp(1j * theta);
|
||||
|
||||
% Get frequency responses for all components
|
||||
G_resp = squeeze(freqresp(G, freqs, 'Hz'));
|
||||
K_resp = squeeze(freqresp(K, freqs, 'Hz'));
|
||||
Hl_resp = squeeze(freqresp(Hl, freqs, 'Hz'));
|
||||
wI_resp = squeeze(freqresp(wI, freqs, 'Hz'));
|
||||
|
||||
% Calculate nominal responses
|
||||
nom_L = G_resp .* K_resp .* Hl_resp;
|
||||
nom_S = 1 ./ (1 + nom_L);
|
||||
nom_T = nom_L ./ (1 + nom_L);
|
||||
|
||||
% Store all the points in the complex plane that L can take
|
||||
loop_region_points = zeros(length(freqs), num_delta_points);
|
||||
|
||||
% Initialize arrays to store magnitude bounds
|
||||
S_mag_min = ones(length(freqs), 1) * inf;
|
||||
S_mag_max = zeros(length(freqs), 1);
|
||||
T_mag_min = ones(length(freqs), 1) * inf;
|
||||
T_mag_max = zeros(length(freqs), 1);
|
||||
|
||||
% Calculate magnitude bounds for all delta values
|
||||
for i = 1:num_delta_points
|
||||
% Perturbed loop gain
|
||||
loop_perturbed = nom_L .* (1 + wI_resp .* delta_points(i));
|
||||
loop_region_points(:,i) = loop_perturbed;
|
||||
|
||||
% Perturbed sensitivity function
|
||||
S_perturbed = 1 ./ (1 + loop_perturbed);
|
||||
S_mag = abs(S_perturbed);
|
||||
|
||||
% Update S magnitude bounds
|
||||
S_mag_min = min(S_mag_min, S_mag);
|
||||
S_mag_max = max(S_mag_max, S_mag);
|
||||
|
||||
% Perturbed complementary sensitivity function
|
||||
T_perturbed = loop_perturbed ./ (1 + loop_perturbed);
|
||||
T_mag = abs(T_perturbed);
|
||||
|
||||
% Update T magnitude bounds
|
||||
T_mag_min = min(T_mag_min, T_mag);
|
||||
T_mag_max = max(T_mag_max, T_mag);
|
||||
end
|
||||
|
||||
% At frequencies where |wI| > 1, T min is zero
|
||||
T_mag_min(abs(wI_resp)>1) = 1e-10;
|
||||
|
||||
%% Nyquist plot to check Robust Stability
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(squeeze(freqresp(G*K*Hl, freqs, 'Hz'))), imag(squeeze(freqresp(G*K*Hl, freqs, 'Hz'))), 'k', 'DisplayName', '$L(j\omega)$ - Nominal');
|
||||
plot(alphaShape(real(loop_region_points(:)), imag(loop_region_points(:)), 0.1), 'FaceColor', [0, 0, 0], 'EdgeColor', 'none', 'FaceAlpha', 0.3, 'DisplayName', '$L(j\omega)$ - $\forall G \in \Pi_i$');
|
||||
plot(-1, 0, 'k+', 'MarkerSize', 5, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
grid on;
|
||||
axis equal
|
||||
xlim([-1.4, 0.2]); ylim([-1.2, 0.4]);
|
||||
xticks(-1.4:0.2:0.2); yticks(-1.2:0.2:0.4);
|
||||
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
%% Robust Performance
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(nom_S), 'color', colors(2,:), 'DisplayName', '$|S|$ - Nom.');
|
||||
plot(freqs, abs(nom_T), 'color', colors(1,:), 'DisplayName', '$|T|$ - Nom.');
|
||||
|
||||
patch([freqs, fliplr(freqs)], [S_mag_max', fliplr(S_mag_min')], colors(2,:), 'FaceAlpha', 0.2, 'EdgeColor', 'none', 'HandleVisibility', 'off');
|
||||
patch([freqs, fliplr(freqs)], [T_mag_max', fliplr(T_mag_min')], colors(1,:), 'FaceAlpha', 0.2, 'EdgeColor', 'none', 'HandleVisibility', 'off');
|
||||
|
||||
plot([1, 100], [0.01, 100], ':', 'color', colors(2,:), 'DisplayName', 'Specs.');
|
||||
plot([300, 1000], [0.01, 0.01], ':', 'color', colors(1,:), 'DisplayName', 'Specs.');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(wI, freqs, 'Hz'))), ':', 'color', colors(1,:), 'HandleVisibility', 'off');
|
||||
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
ylim([1e-4, 5]);
|
||||
xticks([0.1, 1, 10, 100, 1000]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 3);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
BIN
B3-control/detail_control_decoupling_test_model.slx
Normal file
BIN
B3-control/detail_control_decoupling_test_model.slx
Normal file
Binary file not shown.
34
B3-control/src/generateCF.m
Normal file
34
B3-control/src/generateCF.m
Normal file
@@ -0,0 +1,34 @@
|
||||
function [H1, H2] = generateCF(W1, W2, args)
|
||||
% generateCF -
|
||||
%
|
||||
% Syntax: [H1, H2] = generateCF(W1, W2, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - W1 - Weighting Function for H1
|
||||
% - W2 - Weighting Function for H2
|
||||
% - args:
|
||||
% - method - H-Infinity solver ('lmi' or 'ric')
|
||||
% - display - Display synthesis results ('on' or 'off')
|
||||
%
|
||||
% Outputs:
|
||||
% - H1 - Generated H1 Filter
|
||||
% - H2 - Generated H2 Filter
|
||||
|
||||
%% Argument validation
|
||||
arguments
|
||||
W1
|
||||
W2
|
||||
args.method char {mustBeMember(args.method,{'lmi', 'ric'})} = 'ric'
|
||||
args.display char {mustBeMember(args.display,{'on', 'off'})} = 'on'
|
||||
end
|
||||
|
||||
%% The generalized plant is defined
|
||||
P = [W1 -W1;
|
||||
0 W2;
|
||||
1 0];
|
||||
|
||||
%% The standard H-infinity synthesis is performed
|
||||
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', args.method, 'DISPLAY', args.display);
|
||||
|
||||
%% H1 is defined as the complementary of H2
|
||||
H1 = 1 - H2;
|
||||
43
B3-control/src/generateWF.m
Normal file
43
B3-control/src/generateWF.m
Normal file
@@ -0,0 +1,43 @@
|
||||
function [W] = generateWF(args)
|
||||
% generateWF -
|
||||
%
|
||||
% Syntax: [W] = generateWeight(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - n - Weight Order (integer)
|
||||
% - G0 - Low frequency Gain
|
||||
% - G1 - High frequency Gain
|
||||
% - Gc - Gain of the weight at frequency w0
|
||||
% - w0 - Frequency at which |W(j w0)| = Gc [rad/s]
|
||||
%
|
||||
% Outputs:
|
||||
% - W - Generated Weighting Function
|
||||
|
||||
%% Argument validation
|
||||
arguments
|
||||
args.n (1,1) double {mustBeInteger, mustBePositive} = 1
|
||||
args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Ginf (1,1) double {mustBeNumeric, mustBePositive} = 10
|
||||
args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
end
|
||||
|
||||
% Verification of correct relation between G0, Gc and Ginf
|
||||
mustBeBetween(args.G0, args.Gc, args.Ginf);
|
||||
|
||||
%% Initialize the Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% Create the weighting function according to formula
|
||||
W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.Ginf)^(2/args.n)))*s + ...
|
||||
(args.G0/args.Gc)^(1/args.n))/...
|
||||
((1/args.Ginf)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.Ginf)^(2/args.n)))*s + ...
|
||||
(1/args.Gc)^(1/args.n)))^args.n;
|
||||
|
||||
%% Custom validation function
|
||||
function mustBeBetween(a,b,c)
|
||||
if ~((a > b && b > c) || (c > b && b > a))
|
||||
eid = 'createWeight:inputError';
|
||||
msg = 'Gc should be between G0 and Ginf.';
|
||||
throwAsCaller(MException(eid,msg))
|
||||
end
|
||||
42
B3-control/src/plotMagUncertainty.m
Normal file
42
B3-control/src/plotMagUncertainty.m
Normal file
@@ -0,0 +1,42 @@
|
||||
function [p] = plotMagUncertainty(W, freqs, args)
|
||||
% plotMagUncertainty -
|
||||
%
|
||||
% Syntax: [p] = plotMagUncertainty(W, freqs, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - W - Multiplicative Uncertainty Weight
|
||||
% - freqs - Frequency Vector [Hz]
|
||||
% - args - Optional Arguments:
|
||||
% - G
|
||||
% - color_i
|
||||
% - opacity
|
||||
%
|
||||
% Outputs:
|
||||
% - p - Plot Handle
|
||||
|
||||
arguments
|
||||
W
|
||||
freqs double {mustBeNumeric, mustBeNonnegative}
|
||||
args.G = tf(1)
|
||||
args.color_i (1,1) double {mustBeInteger, mustBeNonnegative} = 0
|
||||
args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
|
||||
args.DisplayName char = ''
|
||||
end
|
||||
|
||||
% Get defaults colors
|
||||
colors = get(groot, 'defaultAxesColorOrder');
|
||||
|
||||
p = patch([freqs flip(freqs)], ...
|
||||
[abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
|
||||
flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
|
||||
'DisplayName', args.DisplayName);
|
||||
|
||||
if args.color_i == 0
|
||||
p.FaceColor = [0; 0; 0];
|
||||
else
|
||||
p.FaceColor = colors(args.color_i, :);
|
||||
end
|
||||
p.EdgeColor = 'none';
|
||||
p.FaceAlpha = args.opacity;
|
||||
|
||||
end
|
||||
52
B3-control/src/plotPhaseUncertainty.m
Normal file
52
B3-control/src/plotPhaseUncertainty.m
Normal file
@@ -0,0 +1,52 @@
|
||||
function [p] = plotPhaseUncertainty(W, freqs, args)
|
||||
% plotPhaseUncertainty -
|
||||
%
|
||||
% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - W - Multiplicative Uncertainty Weight
|
||||
% - freqs - Frequency Vector [Hz]
|
||||
% - args - Optional Arguments:
|
||||
% - G
|
||||
% - color_i
|
||||
% - opacity
|
||||
%
|
||||
% Outputs:
|
||||
% - p - Plot Handle
|
||||
|
||||
arguments
|
||||
W
|
||||
freqs double {mustBeNumeric, mustBeNonnegative}
|
||||
args.G = tf(1)
|
||||
args.unwrap logical {mustBeNumericOrLogical} = false
|
||||
args.color_i (1,1) double {mustBeInteger, mustBeNonnegative} = 0
|
||||
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
|
||||
args.DisplayName char = ''
|
||||
end
|
||||
|
||||
% Get defaults colors
|
||||
colors = get(groot, 'defaultAxesColorOrder');
|
||||
|
||||
% Compute Phase Uncertainty
|
||||
Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
|
||||
Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;
|
||||
|
||||
% Compute Plant Phase
|
||||
if args.unwrap
|
||||
G_ang = 180/pi*unwrap(angle(squeeze(freqresp(args.G, freqs, 'Hz'))));
|
||||
else
|
||||
G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));
|
||||
end
|
||||
|
||||
p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
|
||||
'DisplayName', args.DisplayName);
|
||||
|
||||
if args.color_i == 0
|
||||
p.FaceColor = [0; 0; 0];
|
||||
else
|
||||
p.FaceColor = colors(args.color_i, :);
|
||||
end
|
||||
p.EdgeColor = 'none';
|
||||
p.FaceAlpha = args.opacity;
|
||||
|
||||
end
|
||||
Reference in New Issue
Block a user