Add complementary filter section
This commit is contained in:
239
matlab/detail_control_1_complementary_filtering.m
Normal file
239
matlab/detail_control_1_complementary_filtering.m
Normal file
@@ -0,0 +1,239 @@
|
||||
%% 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 obtained complementary filters
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||||
ylim([8e-4, 20]);
|
||||
yticks([1e-3, 1e-2, 1e-1, 1, 1e1]);
|
||||
yticklabels({'', '$10^{-2}$', '', '$10^0$', ''})
|
||||
leg = legend('location', 'south', 'FontSize', 8, 'NumColumns', 2);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
yticks([-180:90:180]);
|
||||
ylim([-180, 200])
|
||||
yticklabels({'-180', '', '0', '', '180'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% Design of "Closed-loop" complementary filters
|
||||
% 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 for "closed-loop" complementary filter synthesis
|
||||
P = [ W1 0 1;
|
||||
-W1 W2 -1];
|
||||
|
||||
% Standard H-Infinity Synthesis
|
||||
[L, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||||
|
||||
% Complementary filters
|
||||
H1 = inv(1 + L);
|
||||
H2 = 1 - H1;
|
||||
|
||||
%% Bode plot of the obtained complementary filters after H-infinity mixed-sensitivity synthesis
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
|
||||
plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), 'k--', 'DisplayName', '$|L|$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||||
ylim([1e-3, 1e3]);
|
||||
yticks([1e-3, 1e-2, 1e-1, 1, 1e1, 1e2, 1e3]);
|
||||
yticklabels({'', '$10^{-2}$', '', '$10^0$', '', '$10^2$', ''});
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
yticks([-180:90:180]);
|
||||
ylim([-180, 200])
|
||||
yticklabels({'-180', '', '0', '', '180'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
%% 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;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
|
||||
set(gca,'ColorOrderIndex',3)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), '--', 'DisplayName', '$|W_3|^{-1}$');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
set(gca,'ColorOrderIndex',3)
|
||||
plot(freqs, abs(squeeze(freqresp(H3, freqs, 'Hz'))), '-', 'DisplayName', '$H_3$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude');
|
||||
set(gca, 'XTickLabel',[]);
|
||||
ylim([1e-4, 20]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8);
|
||||
leg.ItemTokenSize(1) = 18;
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',3)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H3, freqs, 'Hz'))));
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
set(gca, 'XScale', 'log');
|
||||
yticks([-180:90:180]); ylim([-220, 220]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
34
matlab/src/generateCF.m
Normal file
34
matlab/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
matlab/src/generateWF.m
Normal file
43
matlab/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
|
||||
Reference in New Issue
Block a user