tdehaeze/dehaeze21_activ_dampin_rotat_platf_using
1
0 Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Update Matlab tangled files

Browse Source
This commit is contained in:
tdehaeze committed 2026-10-17 18:35:40 +02:00
1 parent 8f121be23b
commit 8e08e3f3d4
10 files changed
+776 -738
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

matlab/index.org
+56 -28
View File
@@ -45,7 +45,7 @@ To run the Matlab code, go in the =matlab= directory and run the following Matla
#+caption: Paper's sections and corresponding Matlab files #+caption: Paper's sections and corresponding Matlab files
| Sections | Matlab File | | Sections | Matlab File |
|------------------------------------+----------------------------| |------------------------------------+--------------------------|
| Section [[sec:system_description]] | =s1_system_description.m= | | Section [[sec:system_description]] | =s1_system_description.m= |
| Section [[sec:iff_pure_int]] | =s2_iff_pure_int.m= | | Section [[sec:iff_pure_int]] | =s2_iff_pure_int.m= |
| Section [[sec:iff_pseudo_int]] | =s3_iff_hpf.m= | | Section [[sec:iff_pseudo_int]] | =s3_iff_hpf.m= |
@@ -60,16 +60,19 @@ To run the Matlab code, go in the =matlab= directory and run the following Matla
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no #+begin_src matlab :tangle no :noweb yes
addpath('./matlab/'); <<m-init-path>>
addpath('./src/'); #+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src #+end_src
** System description ** System description
@@ -440,16 +443,19 @@ Two identical controllers $K_F$ are used to feedback each of the sensed force to
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no #+begin_src matlab :tangle no :noweb yes
addpath('./matlab/'); <<m-init-path>>
addpath('./src/'); #+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src #+end_src
** Plant Parameters ** Plant Parameters
@@ -764,16 +770,19 @@ It is shown that for non-null rotating speed, one pole is bound to the right-hal
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no #+begin_src matlab :tangle no :noweb yes
addpath('./matlab/'); <<m-init-path>>
addpath('./src/'); #+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src #+end_src
** Plant Parameters ** Plant Parameters
@@ -1160,16 +1169,19 @@ To find the optimum, the gain that maximize the simultaneous damping of the mode
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no #+begin_src matlab :tangle no :noweb yes
addpath('./matlab/'); <<m-init-path>>
addpath('./src/'); #+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src #+end_src
** Schematic ** Schematic
@@ -1761,16 +1773,19 @@ These two methods are now compared in terms of added damping, closed-loop compli
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no #+begin_src matlab :tangle no :noweb yes
addpath('./matlab/'); <<m-init-path>>
addpath('./src/'); #+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src #+end_src
** Plant Parameters ** Plant Parameters
@@ -2143,10 +2158,23 @@ Critical Damping corresponds to to $\xi = 1$, and thus:
#+latex: \printbibliography #+latex: \printbibliography
bibliography:ref.bib bibliography:ref.bib
* Helping Functions :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/'); % Path for scripts
addpath('./matlab/src/'); % Path for functions
#+END_SRC
#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
addpath('./src/'); % Path for functions
#+END_SRC
* Functions :noexport: * Functions :noexport:
** Sort Poles for the Root Locus ** Sort Poles for the Root Locus
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle src/rootLocusPolesSorted.m :header-args:matlab+: :tangle matlab/src/rootLocusPolesSorted.m
:header-args:matlab+: :comments none :mkdirp yes :eval no :header-args:matlab+: :comments none :mkdirp yes :eval no
:END: :END:
<<sec:rootLocusPolesSorted>> <<sec:rootLocusPolesSorted>>
@@ -2291,7 +2319,7 @@ This Matlab function is accessible [[file:src/rootLocusPolesSorted.m][here]].
#+end_src #+end_src
** =computeSimultaneousDamping= ** =computeSimultaneousDamping=
#+begin_src matlab :tangle src/computeSimultaneousDamping.m #+begin_src matlab :tangle matlab/src/computeSimultaneousDamping.m
function [xi_min] = computeSimultaneousDamping(g, G, K) function [xi_min] = computeSimultaneousDamping(g, G, K)
[w, xi] = damp(minreal(feedback(G, g*K))); [w, xi] = damp(minreal(feedback(G, g*K)));
xi_min = 1/min(xi); xi_min = 1/min(xi);
matlab/matlab/s1_system_description.m
+125 -123
View File
@@ -4,15 +4,17 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
addpath('./src/'); % Path for functions
% Numerical Values % Numerical Values
% Let's define initial values for the model. % Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m] k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)] c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg] m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m)); xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s] w0 = sqrt(k/m); % [rad/s]
% Campbell Diagram % Campbell Diagram
% The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed. % The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed.
@@ -20,19 +22,19 @@ w0 = sqrt(k/m); % [rad/s]
% It is shown in Figures [[fig:campbell_diagram_real]] and [[fig:campbell_diagram_imag]], and one can see that the system becomes unstable for $\Omega > \omega_0$ (the real part of one of the poles becomes positive). % It is shown in Figures [[fig:campbell_diagram_real]] and [[fig:campbell_diagram_imag]], and one can see that the system becomes unstable for $\Omega > \omega_0$ (the real part of one of the poles becomes positive).
Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s] Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s]
p_ws = zeros(4, length(Ws)); p_ws = zeros(4, length(Ws));
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
W = Ws(W_i); W = Ws(W_i);
pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2)); pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2));
[~, i_sort] = sort(imag(pole_G)); [~, i_sort] = sort(imag(pole_G));
p_ws(:, W_i) = pole_G(i_sort); p_ws(:, W_i) = pole_G(i_sort);
end end
clear pole_G; clear pole_G;
% Simscape Model % Simscape Model
% In order to validate all the equations of motion, a Simscape model of the same system has been developed. % In order to validate all the equations of motion, a Simscape model of the same system has been developed.
@@ -40,39 +42,39 @@ clear pole_G;
% The rotating speed for the Simscape Model is defined. % The rotating speed for the Simscape Model is defined.
W = 0.1; % Rotation Speed [rad/s] W = 0.1; % Rotation Speed [rad/s]
Kiff = tf(zeros(2)); Kiff = tf(zeros(2));
kp = 0; % Parallel Stiffness [N/m] kp = 0; % Parallel Stiffness [N/m]
cp = 0; % Parallel Damping [N/(m/s)] cp = 0; % Parallel Damping [N/(m/s)]
open('rotating_frame.slx'); open('rotating_frame.slx');
% The transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is identified from the Simscape model. % The transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is identified from the Simscape model.
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'rotating_frame'; mdl = 'rotating_frame';
%% Input/Output definition %% Input/Output definition
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io, 0); G = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
G.InputName = {'Fu', 'Fv'}; G.InputName = {'Fu', 'Fv'};
G.OutputName = {'du', 'dv'}; G.OutputName = {'du', 'dv'};
% The same transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is written down from the analytical model. % The same transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is written down from the analytical model.
Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]; -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
@@ -81,111 +83,111 @@ Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2)
% Both transfer functions are compared in Figure [[fig:plant_simscape_analytical]] and are found to perfectly match. % Both transfer functions are compared in Figure [[fig:plant_simscape_analytical]] and are found to perfectly match.
freqs = logspace(-1, 1, 1000); freqs = logspace(-1, 1, 1000);
figure; figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-') plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
title('$d_u/F_u$, $d_v/F_v$'); title('$d_u/F_u$, $d_v/F_v$');
ax2 = nexttile([2, 1]); ax2 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-') plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
title('$d_u/F_v$, $d_v/F_u$'); title('$d_u/F_v$, $d_v/F_u$');
ax3 = nexttile; ax3 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-') plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--') plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
ax4 = nexttile; ax4 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ... plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ...
'DisplayName', 'Simscape') 'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ... plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ...
'DisplayName', 'Analytical') 'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
legend('location', 'southwest', 'FontSize', 8); legend('location', 'southwest', 'FontSize', 8);
linkaxes([ax1,ax2,ax3,ax4],'x'); linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y'); linkaxes([ax1,ax2],'y');
% Effect of the rotation speed % Effect of the rotation speed
% The transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ are identified for the following rotating speeds. % The transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ are identified for the following rotating speeds.
Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s] Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
Gs = {zeros(2, 2, length(Ws))}; Gs = {zeros(2, 2, length(Ws))};
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
W = Ws(W_i); W = Ws(W_i);
Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]}; -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]};
end end
% They are compared in Figures [[fig:plant_compare_rotating_speed_direct]] and [[fig:plant_compare_rotating_speed_coupling]]. % They are compared in Figures [[fig:plant_compare_rotating_speed_direct]] and [[fig:plant_compare_rotating_speed_coupling]].
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ... plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
end end
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8); leg = legend('location', 'southwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 6; leg.ItemTokenSize(1) = 6;
ylim([1e-4, 1e2]); ylim([1e-4, 1e2]);
title('$d_u/F_u$, $d_v/F_v$'); title('$d_u/F_u$, $d_v/F_v$');
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs)))) plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs))))
end end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
@@ -195,32 +197,32 @@ xlim([freqs(1), freqs(end)]);
% [[file:figs/plant_compare_rotating_speed_direct.png]] % [[file:figs/plant_compare_rotating_speed_direct.png]]
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs))))
end end
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ylim([1e-4, 1e2]); ylim([1e-4, 1e2]);
title('$d_u/F_v$, $d_v/F_u$'); title('$d_u/F_v$, $d_v/F_u$');
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(2,1), freqs))))
end end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
matlab/matlab/s2_iff_pure_int.m
+105 -103
View File
@@ -4,51 +4,53 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
addpath('./src/'); % Path for functions
% Plant Parameters % Plant Parameters
% Let's define initial values for the model. % Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m] k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)] c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg] m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m)); xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s] w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m] kp = 0; % [N/m]
cp = 0; % [N/(m/s)] cp = 0; % [N/(m/s)]
% Comparison of the Analytical Model and the Simscape Model % Comparison of the Analytical Model and the Simscape Model
% The rotation speed is set to $\Omega = 0.1 \omega_0$. % The rotation speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0; % [rad/s] W = 0.1*w0; % [rad/s]
Kiff = tf(zeros(2)); Kiff = tf(zeros(2));
open('rotating_frame.slx'); open('rotating_frame.slx');
% And the transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is identified using the Simscape model. % And the transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is identified using the Simscape model.
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'rotating_frame'; mdl = 'rotating_frame';
%% Input/Output definition %% Input/Output definition
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;
Giff = linearize(mdl, io, 0); Giff = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
Giff.InputName = {'Fu', 'Fv'}; Giff.InputName = {'Fu', 'Fv'};
Giff.OutputName = {'fu', 'fv'}; Giff.OutputName = {'fu', 'fv'};
% The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model. % The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
@@ -56,108 +58,108 @@ Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2)
% The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match. % The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match.
freqs = logspace(-1, 1, 1000); freqs = logspace(-1, 1, 1000);
figure; figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_u$, $f_v/F_v$'); title('$f_u/F_u$, $f_v/F_v$');
ax2 = nexttile([2, 1]); ax2 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_v$, $f_v/F_u$'); title('$f_u/F_v$, $f_v/F_u$');
ax3 = nexttile; ax3 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
ax4 = nexttile; ax4 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ...
'DisplayName', 'Simscape') 'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ...
'DisplayName', 'Analytical') 'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
legend('location', 'northeast', 'FontSize', 8); legend('location', 'northeast', 'FontSize', 8);
linkaxes([ax1,ax2,ax3,ax4],'x'); linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y'); linkaxes([ax1,ax2],'y');
% Effect of the rotation speed % Effect of the rotation speed
% The transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ are identified for the following rotating speeds. % The transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ are identified for the following rotating speeds.
Ws = [0, 0.2, 0.7]*w0; % Rotating Speeds [rad/s] Ws = [0, 0.2, 0.7]*w0; % Rotating Speeds [rad/s]
Gsiff = {zeros(2, 2, length(Ws))}; Gsiff = {zeros(2, 2, length(Ws))};
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
W = Ws(W_i); W = Ws(W_i);
Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]}; (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]};
end end
% The obtained transfer functions are shown in Figure [[fig:plant_iff_compare_rotating_speed]]. % The obtained transfer functions are shown in Figure [[fig:plant_iff_compare_rotating_speed]].
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ... plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
end end
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
leg = legend('location', 'southeast', 'FontSize', 8); leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 6; leg.ItemTokenSize(1) = 6;
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs)))) plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))))
end end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
% Decentralized Integral Force Feedback % Decentralized Integral Force Feedback
% The decentralized IFF controller consists of pure integrators: % The decentralized IFF controller consists of pure integrators:
@@ -172,12 +174,12 @@ xlim([freqs(1), freqs(end)]);
% It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable. % It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable.
figure; figure;
gains = logspace(-2, 4, 100); gains = logspace(-2, 4, 100);
hold on; hold on;
for W_i = 1:length(Ws) for W_i = 1:length(Ws)
set(gca,'ColorOrderIndex',W_i); set(gca,'ColorOrderIndex',W_i);
plot(real(pole(Gsiff{W_i})), imag(pole(Gsiff{W_i})), 'x', ... plot(real(pole(Gsiff{W_i})), imag(pole(Gsiff{W_i})), 'x', ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)); 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0));
@@ -190,11 +192,11 @@ for W_i = 1:length(Ws)
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
end end
hold off; hold off;
axis square; axis square;
xlim([-2, 0.5]); ylim([0, 2.5]); xlim([-2, 0.5]); ylim([0, 2.5]);
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8); leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
matlab/matlab/s3_iff_hpf.m
+104 -102
View File
@@ -4,18 +4,20 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
addpath('./src/'); % Path for functions
% Plant Parameters % Plant Parameters
% Let's define initial values for the model. % Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m] k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)] c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg] m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m)); xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s] w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m] kp = 0; % [N/m]
cp = 0; % [N/(m/s)] cp = 0; % [N/(m/s)]
% Modified Integral Force Feedback Controller % Modified Integral Force Feedback Controller
% Let's modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used: % Let's modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used:
@@ -29,18 +31,18 @@ cp = 0; % [N/(m/s)]
% Let's arbitrary choose the following control parameters: % Let's arbitrary choose the following control parameters:
g = 2; g = 2;
wi = 0.1*w0; wi = 0.1*w0;
Kiff = (g/(wi+s))*eye(2); Kiff = (g/(wi+s))*eye(2);
% And the following rotating speed. % And the following rotating speed.
W = 0.1*w0; W = 0.1*w0;
Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
@@ -48,74 +50,74 @@ Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) *
% The obtained Loop Gain is shown in Figure [[fig:loop_gain_modified_iff]]. % The obtained Loop Gain is shown in Figure [[fig:loop_gain_modified_iff]].
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs)))) plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs))))
plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs)))) plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))))
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); ylabel('Loop Gain');
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*(g/s), freqs))), ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*(g/s), freqs))), ...
'DisplayName', 'IFF') 'DisplayName', 'IFF')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ...
'DisplayName', 'IFF + HPF') 'DisplayName', 'IFF + HPF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
leg = legend('location', 'southwest', 'FontSize', 8); leg = legend('location', 'southwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 6; leg.ItemTokenSize(1) = 6;
hold off; hold off;
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
% Root Locus % Root Locus
% As shown in the Root Locus plot (Figure [[fig:root_locus_modified_iff]]), for some value of the gain, the system remains stable. % As shown in the Root Locus plot (Figure [[fig:root_locus_modified_iff]]), for some value of the gain, the system remains stable.
figure; figure;
gains = logspace(-2, 4, 100); gains = logspace(-2, 4, 100);
hold on; hold on;
% Pure Integrator % Pure Integrator
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF'); plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF');
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
for g = gains for g = gains
clpoles = pole(feedback(Giff, (g/s)*eye(2))); clpoles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
end end
% Modified IFF % Modified IFF
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF'); plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF');
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
for g = gains for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2))); clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
end end
hold off; hold off;
axis square; axis square;
xlim([-2, 0.5]); ylim([-1.25, 1.25]); xlim([-2, 0.5]); ylim([-1.25, 1.25]);
leg = legend('location', 'northwest', 'FontSize', 8); leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
@@ -125,20 +127,20 @@ xlabel('Real Part'); ylabel('Imaginary Part');
% [[file:figs/root_locus_modified_iff.png]] % [[file:figs/root_locus_modified_iff.png]]
xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
legend('hide'); legend('hide');
% What is the optimal $\omega_i$ and $g$? % What is the optimal $\omega_i$ and $g$?
% In order to visualize the effect of $\omega_i$ on the attainable damping, the Root Locus is displayed in Figure [[fig:root_locus_wi_modified_iff]] for the following $\omega_i$: % In order to visualize the effect of $\omega_i$ on the attainable damping, the Root Locus is displayed in Figure [[fig:root_locus_wi_modified_iff]] for the following $\omega_i$:
wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s] wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s]
figure; figure;
gains = logspace(-2, 4, 100); gains = logspace(-2, 4, 100);
hold on; hold on;
for wi_i = 1:length(wis) for wi_i = 1:length(wis)
set(gca,'ColorOrderIndex',wi_i); set(gca,'ColorOrderIndex',wi_i);
wi = wis(wi_i); wi = wis(wi_i);
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0)); L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));
@@ -147,18 +149,18 @@ for wi_i = 1:length(wis)
set(gca,'ColorOrderIndex',wi_i); set(gca,'ColorOrderIndex',wi_i);
plot(real(clpoles), imag(clpoles), '.'); plot(real(clpoles), imag(clpoles), '.');
end end
end end
plot(real(pole(Giff)), imag(pole(Giff)), 'kx'); plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko'); plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
hold off; hold off;
axis square; axis square;
xlim([-2.3, 0.1]); ylim([-1.2, 1.2]); xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
xticks([-2:1:2]); yticks([-2:1:2]); xticks([-2:1:2]); yticks([-2:1:2]);
leg = legend(L, 'location', 'northwest', 'FontSize', 8); leg = legend(L, 'location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
clear L clear L
@@ -168,9 +170,9 @@ clear L
% [[file:figs/root_locus_wi_modified_iff.png]] % [[file:figs/root_locus_wi_modified_iff.png]]
xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]); xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
legend('hide'); legend('hide');
@@ -192,12 +194,12 @@ legend('hide');
% To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of $\omega_i$ (Figure [[fig:mod_iff_damping_wi]]). % To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of $\omega_i$ (Figure [[fig:mod_iff_damping_wi]]).
wis = logspace(-2, 1, 100)*w0; % [rad/s] wis = logspace(-2, 1, 100)*w0; % [rad/s]
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
opt_gain = zeros(1, length(wis)); % Corresponding optimal gain opt_gain = zeros(1, length(wis)); % Corresponding optimal gain
for wi_i = 1:length(wis) for wi_i = 1:length(wis)
wi = wis(wi_i); wi = wis(wi_i);
Kiff = 1/(s + wi)*eye(2); Kiff = 1/(s + wi)*eye(2);
@@ -206,23 +208,23 @@ for wi_i = 1:length(wis)
[g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((w0/W)^2 - 1)); [g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((w0/W)^2 - 1));
opt_xi(wi_i) = 1/xi_opt; opt_xi(wi_i) = 1/xi_opt;
opt_gain(wi_i) = g_opt; opt_gain(wi_i) = g_opt;
end end
figure; figure;
yyaxis left yyaxis left
plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin'); set(gca, 'YScale', 'lin');
ylim([0,1]); ylim([0,1]);
ylabel('Damping Ratio $\xi$'); ylabel('Damping Ratio $\xi$');
yyaxis right yyaxis right
hold on; hold on;
plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$'); plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$');
plot(wis, wis*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$'); plot(wis, wis*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$');
set(gca, 'YScale', 'lin'); set(gca, 'YScale', 'lin');
ylim([0,10]); ylim([0,10]);
ylabel('Controller gain $g$'); ylabel('Controller gain $g$');
xlabel('$\omega_i/\omega_0$'); xlabel('$\omega_i/\omega_0$');
set(gca, 'XScale', 'log'); set(gca, 'XScale', 'log');
legend('location', 'northeast', 'FontSize', 8); legend('location', 'northeast', 'FontSize', 8);
matlab/matlab/s4_iff_kp.m
+174 -172
View File
@@ -4,108 +4,110 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
addpath('./src/'); % Path for functions
% Plant Parameters % Plant Parameters
% Let's define initial values for the model. % Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m] k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)] c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg] m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m)); xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s] w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m] kp = 0; % [N/m]
cp = 0; % [N/(m/s)] cp = 0; % [N/(m/s)]
% Comparison of the Analytical Model and the Simscape Model % Comparison of the Analytical Model and the Simscape Model
% The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model. % The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
W = 0.1*w0; % [rad/s] W = 0.1*w0; % [rad/s]
kp = 1.5*m*W^2; kp = 1.5*m*W^2;
cp = 0; cp = 0;
Kiff = tf(zeros(2)); Kiff = tf(zeros(2));
open('rotating_frame.slx'); open('rotating_frame.slx');
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'rotating_frame'; mdl = 'rotating_frame';
%% Input/Output definition %% Input/Output definition
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;
Giff = linearize(mdl, io, 0); Giff = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
Giff.InputName = {'Fu', 'Fv'}; Giff.InputName = {'Fu', 'Fv'};
Giff.OutputName = {'fu', 'fv'}; Giff.OutputName = {'fu', 'fv'};
w0p = sqrt((k + kp)/m); w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m)); xip = c/(2*sqrt((k+kp)*m));
Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
Giff_th.InputName = {'Fu', 'Fv'}; Giff_th.InputName = {'Fu', 'Fv'};
Giff_th.OutputName = {'fu', 'fv'}; Giff_th.OutputName = {'fu', 'fv'};
freqs = logspace(-1, 1, 1000); freqs = logspace(-1, 1, 1000);
figure; figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_u$, $f_v/F_v$'); title('$f_u/F_u$, $f_v/F_v$');
ax2 = nexttile([2, 1]); ax2 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_v$, $f_v/F_u$'); title('$f_u/F_v$, $f_v/F_u$');
ax3 = nexttile; ax3 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-', ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-', ...
'DisplayName', 'Simscape') 'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--', ... plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--', ...
'DisplayName', 'Analytical') 'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
legend('location', 'southwest', 'FontSize', 8); legend('location', 'southwest', 'FontSize', 8);
ax4 = nexttile; ax4 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
linkaxes([ax1,ax2,ax3,ax4],'x'); linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y'); linkaxes([ax1,ax2],'y');
% Effect of the parallel stiffness on the IFF plant % Effect of the parallel stiffness on the IFF plant
% The rotation speed is set to $\Omega = 0.1 \omega_0$. % The rotation speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0; % [rad/s] W = 0.1*w0; % [rad/s]
@@ -119,69 +121,69 @@ W = 0.1*w0; % [rad/s]
% One can see that for $k_p > m \Omega^2$, the systems shows alternating complex conjugate poles and zeros. % One can see that for $k_p > m \Omega^2$, the systems shows alternating complex conjugate poles and zeros.
kp = 0; kp = 0;
w0p = sqrt((k + kp)/m); w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m)); xip = c/(2*sqrt((k+kp)*m));
Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
kp = 0.5*m*W^2; kp = 0.5*m*W^2;
k = 1 - kp; k = 1 - kp;
w0p = sqrt((k + kp)/m); w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m)); xip = c/(2*sqrt((k+kp)*m));
Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
kp = 1.5*m*W^2; kp = 1.5*m*W^2;
k = 1 - kp; k = 1 - kp;
w0p = sqrt((k + kp)/m); w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m)); xip = c/(2*sqrt((k+kp)*m));
Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), 'k-', ... plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), 'k-', ...
'DisplayName', '$k_p = 0$') 'DisplayName', '$k_p = 0$')
plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ... plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ...
'DisplayName', '$k_p < m\Omega^2$') 'DisplayName', '$k_p < m\Omega^2$')
plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ... plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ...
'DisplayName', '$k_p > m\Omega^2$') 'DisplayName', '$k_p > m\Omega^2$')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ylim([1e-4, 2e1]); ylim([1e-4, 2e1]);
legend('location', 'southeast', 'FontSize', 8); legend('location', 'southeast', 'FontSize', 8);
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), 'k-') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), 'k-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:') plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180); yticks(-180:90:180);
ylim([-180 180]); ylim([-180 180]);
hold off; hold off;
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
% IFF when adding a spring in parallel % IFF when adding a spring in parallel
% In Figure [[fig:root_locus_iff_kp]] is displayed the Root Locus in the three considered cases with % In Figure [[fig:root_locus_iff_kp]] is displayed the Root Locus in the three considered cases with
@@ -200,55 +202,55 @@ xlim([freqs(1), freqs(end)]);
% \end{equation} % \end{equation}
gains = logspace(-2, 2, 100); gains = logspace(-2, 2, 100);
figure; figure;
hold on; hold on;
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... plot(real(pole(Giff)), imag(pole(Giff)), 'x', ...
'DisplayName', '$k_p = 0$'); 'DisplayName', '$k_p = 0$');
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
for g = gains for g = gains
cl_poles = pole(feedback(Giff, (g/s)*eye(2))); cl_poles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x', ... plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x', ...
'DisplayName', '$k_p < m\Omega^2$'); 'DisplayName', '$k_p < m\Omega^2$');
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o', ... plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
for g = gains for g = gains
cl_poles = pole(feedback(Giff_s, (g/s)*eye(2))); cl_poles = pole(feedback(Giff_s, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
set(gca,'ColorOrderIndex',3); set(gca,'ColorOrderIndex',3);
plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x', ... plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x', ...
'DisplayName', '$k_p > m\Omega^2$'); 'DisplayName', '$k_p > m\Omega^2$');
set(gca,'ColorOrderIndex',3); set(gca,'ColorOrderIndex',3);
plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o', ... plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
for g = gains for g = gains
set(gca,'ColorOrderIndex',3); set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Giff_l, (g/s)*eye(2))); cl_poles = pole(feedback(Giff_l, (g/s)*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
hold off; hold off;
axis square; axis square;
xlim([-1, 0.2]); ylim([0, 1.2]); xlim([-1, 0.2]); ylim([0, 1.2]);
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8); leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
@@ -258,26 +260,26 @@ leg.ItemTokenSize(1) = 8;
% [[file:figs/root_locus_iff_kp.png]] % [[file:figs/root_locus_iff_kp.png]]
xlim([-0.04, 0.06]); ylim([0, 0.1]); xlim([-0.04, 0.06]); ylim([0, 0.1]);
legend('hide'); legend('hide');
% Effect of $k_p$ on the attainable damping % Effect of $k_p$ on the attainable damping
% However, having large values of $k_p$ may decrease the attainable damping. % However, having large values of $k_p$ may decrease the attainable damping.
% To study the second point, Root Locus plots for the following values of $k_p$ are shown in Figure [[fig:root_locus_iff_kps]]. % To study the second point, Root Locus plots for the following values of $k_p$ are shown in Figure [[fig:root_locus_iff_kps]].
kps = [2, 20, 40]*m*W^2; kps = [2, 20, 40]*m*W^2;
% It is shown that large values of $k_p$ decreases the attainable damping. % It is shown that large values of $k_p$ decreases the attainable damping.
figure; figure;
gains = logspace(-2, 4, 500); gains = logspace(-2, 4, 500);
hold on; hold on;
for kp_i = 1:length(kps) for kp_i = 1:length(kps)
kp = kps(kp_i); kp = kps(kp_i);
k = 1 - kp; k = 1 - kp;
@@ -301,23 +303,23 @@ for kp_i = 1:length(kps)
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
end end
hold off; hold off;
axis square; axis square;
xlim([-1.2, 0.2]); ylim([0, 1.4]); xlim([-1.2, 0.2]); ylim([0, 1.4]);
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8); leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
alphas = logspace(-2, 0, 100); alphas = logspace(-2, 0, 100);
opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain
Kiff = 1/s*eye(2); Kiff = 1/s*eye(2);
for alpha_i = 1:length(alphas) for alpha_i = 1:length(alphas)
kp = alphas(alpha_i); kp = alphas(alpha_i);
k = 1 - alphas(alpha_i); k = 1 - alphas(alpha_i);
@@ -333,22 +335,22 @@ for alpha_i = 1:length(alphas)
[g_opt, xi_opt] = fminsearch(fun, 2); [g_opt, xi_opt] = fminsearch(fun, 2);
opt_xi(alpha_i) = 1/xi_opt; opt_xi(alpha_i) = 1/xi_opt;
opt_gain(alpha_i) = g_opt; opt_gain(alpha_i) = g_opt;
end end
figure; figure;
yyaxis left yyaxis left
plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin'); set(gca, 'YScale', 'lin');
ylim([0,1]); ylim([0,1]);
ylabel('Damping Ratio $\xi$'); ylabel('Damping Ratio $\xi$');
yyaxis right yyaxis right
hold on; hold on;
plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$'); plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
set(gca, 'YScale', 'lin'); set(gca, 'YScale', 'lin');
ylim([0,2.5]); ylim([0,2.5]);
ylabel('Controller gain $g$'); ylabel('Controller gain $g$');
xlabel('$\alpha$'); xlabel('$\alpha$');
set(gca, 'XScale', 'log'); set(gca, 'XScale', 'log');
legend('location', 'northeast', 'FontSize', 8); legend('location', 'northeast', 'FontSize', 8);
matlab/matlab/s5_act_damp_comparison.m
+109 -107
View File
@@ -4,31 +4,33 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
addpath('./src/'); % Path for functions
% Plant Parameters % Plant Parameters
% Let's define initial values for the model. % Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m] k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)] c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg] m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m)); xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s] w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m] kp = 0; % [N/m]
cp = 0; % [N/(m/s)] cp = 0; % [N/(m/s)]
% The rotating speed is set to $\Omega = 0.1 \omega_0$. % The rotating speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0; W = 0.1*w0;
% Root Locus % Root Locus
% IFF with High Pass Filter % IFF with High Pass Filter
wi = 0.1*w0; % [rad/s] wi = 0.1*w0; % [rad/s]
Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
@@ -36,71 +38,71 @@ Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) *
% IFF With parallel Stiffness % IFF With parallel Stiffness
kp = 5*m*W^2; kp = 5*m*W^2;
k = k - kp; k = k - kp;
w0p = sqrt((k + kp)/m); w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m)); xip = c/(2*sqrt((k+kp)*m));
Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
k = k + kp; k = k + kp;
figure; figure;
gains = logspace(-2, 2, 100); gains = logspace(-2, 2, 100);
hold on; hold on;
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... plot(real(pole(Giff)), imag(pole(Giff)), 'x', ...
'DisplayName', 'IFF + LFP'); 'DisplayName', 'IFF + LFP');
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
for g = gains for g = gains
Kiff = (g/(wi + s))*eye(2); Kiff = (g/(wi + s))*eye(2);
cl_poles = pole(feedback(Giff, Kiff)); cl_poles = pole(feedback(Giff, Kiff));
set(gca,'ColorOrderIndex',1); set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff_kp)), imag(pole(Giff_kp)), 'x', ... plot(real(pole(Giff_kp)), imag(pole(Giff_kp)), 'x', ...
'DisplayName', 'IFF + $k_p$'); 'DisplayName', 'IFF + $k_p$');
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff_kp)), imag(tzero(Giff_kp)), 'o', ... plot(real(tzero(Giff_kp)), imag(tzero(Giff_kp)), 'o', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
for g = gains for g = gains
Kiffa = (g/s)*eye(2); Kiffa = (g/s)*eye(2);
cl_poles = pole(feedback(Giff_kp, Kiffa)); cl_poles = pole(feedback(Giff_kp, Kiffa));
set(gca,'ColorOrderIndex',2); set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.', ... plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
hold off; hold off;
axis square; axis square;
xlim([-1.2, 0.05]); ylim([0, 1.25]); xlim([-1.2, 0.05]); ylim([0, 1.25]);
xlabel('Real Part'); ylabel('Imaginary Part'); xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8); leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8; leg.ItemTokenSize(1) = 8;
% Controllers - Optimal Gains % Controllers - Optimal Gains
% In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case. % In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case.
fun = @(g)computeSimultaneousDamping(g, Giff, (1/(wi+s))*eye(2)); fun = @(g)computeSimultaneousDamping(g, Giff, (1/(wi+s))*eye(2));
[opt_gain_iff, opt_xi_iff] = fminsearch(fun, 0.5*(w0^2/W^2 - 1)*wi); [opt_gain_iff, opt_xi_iff] = fminsearch(fun, 0.5*(w0^2/W^2 - 1)*wi);
opt_xi_iff = 1/opt_xi_iff; opt_xi_iff = 1/opt_xi_iff;
fun = @(g)computeSimultaneousDamping(g, Giff_kp, 1/s*eye(2)); fun = @(g)computeSimultaneousDamping(g, Giff_kp, 1/s*eye(2));
[opt_gain_kp, opt_xi_kp] = fminsearch(fun, 2); [opt_gain_kp, opt_xi_kp] = fminsearch(fun, 2);
opt_xi_kp = 1/opt_xi_kp; opt_xi_kp = 1/opt_xi_kp;
% Passive Damping - Critical Damping % Passive Damping - Critical Damping
% \begin{equation} % \begin{equation}
@@ -113,116 +115,116 @@ opt_xi_kp = 1/opt_xi_kp;
% \end{equation} % \end{equation}
c_opt = 2*sqrt(k*m); c_opt = 2*sqrt(k*m);
% Transmissibility And Compliance % Transmissibility And Compliance
% <<sec:comp_transmissibilty>> % <<sec:comp_transmissibilty>>
open('rotating_frame.slx'); open('rotating_frame.slx');
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'rotating_frame'; mdl = 'rotating_frame';
%% Input/Output definition %% Input/Output definition
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1; io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/fd'], 1, 'input'); io_i = io_i + 1; io(io_i) = linio([mdl, '/fd'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
% Open Loop :ignore: % Open Loop :ignore:
Kiff = tf(zeros(2)); Kiff = tf(zeros(2));
kp = 0; kp = 0;
cp = 0; cp = 0;
G_ol = linearize(mdl, io, 0); G_ol = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
G_ol.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; G_ol.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
G_ol.OutputName = {'Dx', 'Dy'}; G_ol.OutputName = {'Dx', 'Dy'};
% Passive Damping % Passive Damping
kp = 0; kp = 0;
cp = 0; cp = 0;
c_old = c; c_old = c;
c = c_opt; c = c_opt;
Kiff = tf(zeros(2)); Kiff = tf(zeros(2));
G_pas = linearize(mdl, io, 0); G_pas = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
G_pas.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; G_pas.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
G_pas.OutputName = {'Dx', 'Dy'}; G_pas.OutputName = {'Dx', 'Dy'};
c = c_old; c = c_old;
% Pseudo Integrator IFF :ignore: % Pseudo Integrator IFF :ignore:
kp = 0; kp = 0;
cp = 0; cp = 0;
Kiff = opt_gain_iff/(wi + s)*tf(eye(2)); Kiff = opt_gain_iff/(wi + s)*tf(eye(2));
G_iff = linearize(mdl, io, 0); G_iff = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
G_iff.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; G_iff.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
G_iff.OutputName = {'Dx', 'Dy'}; G_iff.OutputName = {'Dx', 'Dy'};
% IFF With parallel Stiffness :ignore: % IFF With parallel Stiffness :ignore:
kp = 5*m*W^2; kp = 5*m*W^2;
cp = 0.01; cp = 0.01;
Kiff = opt_gain_kp/s*tf(eye(2)); Kiff = opt_gain_kp/s*tf(eye(2));
G_kp = linearize(mdl, io, 0); G_kp = linearize(mdl, io, 0);
%% Input/Output definition %% Input/Output definition
G_kp.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; G_kp.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
G_kp.OutputName = {'Dx', 'Dy'}; G_kp.OutputName = {'Dx', 'Dy'};
% Transmissibility :ignore: % Transmissibility :ignore:
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Dwx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Dwx'}), freqs))), ...
'DisplayName', 'IFF + HPF') 'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Dwx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Dwx'}), freqs))), ...
'DisplayName', 'IFF + $k_p$') 'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Dwx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Dwx'}), freqs))), ...
'DisplayName', 'Passive') 'DisplayName', 'Passive')
plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Dwx'}), freqs))), 'k-', ... plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Dwx'}), freqs))), 'k-', ...
'DisplayName', 'Open-Loop') 'DisplayName', 'Open-Loop')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-2, 3e1]); ylim([1e-2, 3e1]);
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]'); xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
legend('location', 'southwest', 'FontSize', 8); legend('location', 'southwest', 'FontSize', 8);
% Compliance :ignore: % Compliance :ignore:
freqs = logspace(-2, 1, 1000); freqs = logspace(-2, 1, 1000);
figure; figure;
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Fdx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Fdx'}), freqs))), ...
'DisplayName', 'IFF + HPF') 'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Fdx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Fdx'}), freqs))), ...
'DisplayName', 'IFF + $k_p$') 'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Fdx'}), freqs))), ... plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Fdx'}), freqs))), ...
'DisplayName', 'Passive') 'DisplayName', 'Passive')
plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Fdx'}), freqs))), 'k-', ... plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Fdx'}), freqs))), 'k-', ...
'DisplayName', 'Open-Loop') 'DisplayName', 'Open-Loop')
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-2, 3e1]); ylim([1e-2, 3e1]);
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]'); xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
legend('location', 'southwest', 'FontSize', 8); legend('location', 'southwest', 'FontSize', 8);
matlab/matlab/src/computeSimultaneousDamping.m
+10
View File
@@ -0,0 +1,10 @@
% =computeSimultaneousDamping=
function [xi_min] = computeSimultaneousDamping(g, G, K)
[w, xi] = damp(minreal(feedback(G, g*K)));
xi_min = 1/min(xi);
if xi_min < 0
xi_min = 1e8;
end
end
matlab/matlab/src/rootLocusPolesSorted.m
+93
View File
@@ -0,0 +1,93 @@
function [poles] = rootLocusPolesSorted(G, K, gains, args)
% rootLocusPolesSorted -
%
% Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
%
% Inputs:
% - G, K, gains, args -
%
% Outputs:
% - poles -
arguments
G
K
gains
args.minreal double {mustBeNumericOrLogical} = false
args.p_half double {mustBeNumericOrLogical} = false
args.d_max double {mustBeNumeric} = -1
end
if args.minreal
p1 = pole(minreal(feedback(G, gains(1)*K)));
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
p1 = p1(i_uniq);
poles = zeros(length(p1), length(gains));
poles(:, 1) = p1;
else
p1 = pole(feedback(G, gains(1)*K));
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
p1 = p1(i_uniq);
poles = zeros(length(p1), length(gains));
poles(:, 1) = p1;
end
if args.minreal
p2 = pole(minreal(feedback(G, gains(2)*K)));
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
p2 = p2(i_uniq);
poles(:, 2) = p2;
else
p2 = pole(feedback(G, gains(2)*K));
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
p2 = p2(i_uniq);
poles(:, 2) = p2;
end
for g_i = 3:length(gains)
% Estimated value of the poles
poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
% New values for the poles
poles_gi = pole(feedback(G, gains(g_i)*K));
[~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
poles_gi = poles_gi(i_uniq);
% Array of distances between all the poles
poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));
% Get indices corresponding to distances from lowest to highest
[~, c] = sort(min(poles_dist));
as = 1:length(poles_gi);
% for each column of poles_dist corresponding to the i'th pole
% with closest previous poles
for p_i = c
% Get the indice a_i of the previous pole that is the closest
% to pole c(p_i)
[~, a_i] = min(poles_dist(:, p_i));
poles(as(a_i), g_i) = poles_gi(p_i);
% Remove old poles that are already matched
% poles_gi(as(a_i), :) = [];
poles_dist(a_i, :) = [];
as(a_i) = [];
end
end
if args.d_max > 0
poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
end
if args.p_half
poles = poles(1:round(end/2), :);
end
[~, s_p] = sort(imag(poles(:,1)), 'descend');
poles = poles(s_p, :);
poles = poles.';
matlab/src/computeSimultaneousDamping.m
-10
View File
@@ -1,10 +0,0 @@
% =computeSimultaneousDamping=
function [xi_min] = computeSimultaneousDamping(g, G, K)
[w, xi] = damp(minreal(feedback(G, g*K)));
xi_min = 1/min(xi);
if xi_min < 0
xi_min = 1e8;
end
end
matlab/src/rootLocusPolesSorted.m
-93
View File
@@ -1,93 +0,0 @@
function [poles] = rootLocusPolesSorted(G, K, gains, args)
% rootLocusPolesSorted -
%
% Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
%
% Inputs:
% - G, K, gains, args -
%
% Outputs:
% - poles -
arguments
G
K
gains
args.minreal double {mustBeNumericOrLogical} = false
args.p_half double {mustBeNumericOrLogical} = false
args.d_max double {mustBeNumeric} = -1
end
if args.minreal
p1 = pole(minreal(feedback(G, gains(1)*K)));
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
p1 = p1(i_uniq);
poles = zeros(length(p1), length(gains));
poles(:, 1) = p1;
else
p1 = pole(feedback(G, gains(1)*K));
[~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
p1 = p1(i_uniq);
poles = zeros(length(p1), length(gains));
poles(:, 1) = p1;
end
if args.minreal
p2 = pole(minreal(feedback(G, gains(2)*K)));
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
p2 = p2(i_uniq);
poles(:, 2) = p2;
else
p2 = pole(feedback(G, gains(2)*K));
[~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
p2 = p2(i_uniq);
poles(:, 2) = p2;
end
for g_i = 3:length(gains)
% Estimated value of the poles
poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
% New values for the poles
poles_gi = pole(feedback(G, gains(g_i)*K));
[~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
poles_gi = poles_gi(i_uniq);
% Array of distances between all the poles
poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));
% Get indices corresponding to distances from lowest to highest
[~, c] = sort(min(poles_dist));
as = 1:length(poles_gi);
% for each column of poles_dist corresponding to the i'th pole
% with closest previous poles
for p_i = c
% Get the indice a_i of the previous pole that is the closest
% to pole c(p_i)
[~, a_i] = min(poles_dist(:, p_i));
poles(as(a_i), g_i) = poles_gi(p_i);
% Remove old poles that are already matched
% poles_gi(as(a_i), :) = [];
poles_dist(a_i, :) = [];
as(a_i) = [];
end
end
if args.d_max > 0
poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
end
if args.p_half
poles = poles(1:round(end/2), :);
end
[~, s_p] = sort(imag(poles(:,1)), 'descend');
poles = poles(s_p, :);
poles = poles.';