Work on HAC-DVF control architecture
This commit is contained in:
parent
94eb5e16a7
commit
163b80d8a2
BIN
docs/figs/opt_stdvf_dvf_root_locus.pdf
Normal file
BIN
docs/figs/opt_stdvf_dvf_root_locus.pdf
Normal file
Binary file not shown.
BIN
docs/figs/opt_stdvf_dvf_root_locus.png
Normal file
BIN
docs/figs/opt_stdvf_dvf_root_locus.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 96 KiB |
BIN
mat/conf_log.mat
BIN
mat/conf_log.mat
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
mat/stages.mat
BIN
mat/stages.mat
Binary file not shown.
BIN
mat/tomo_exp_hac_dvf.mat
Normal file
BIN
mat/tomo_exp_hac_dvf.mat
Normal file
Binary file not shown.
Binary file not shown.
@ -81,18 +81,12 @@ The nano-hexapod is considered to be a rigid body.
|
||||
initializeSample('mass', 1);
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
#+end_src
|
||||
|
||||
No controller is used (Open Loop).
|
||||
#+begin_src matlab
|
||||
initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
And we put some gravity.
|
||||
We don't gravity.
|
||||
#+begin_src matlab
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
#+end_src
|
||||
@ -121,6 +115,12 @@ And we initialize the disturbances to be equal to zero.
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
@ -202,19 +202,25 @@ And we save the obtained data.
|
||||
In this section, we also perform a tomography experiment with the sample's center of mass aligned with the rotation axis.
|
||||
However this time, we include perturbations such as ground motion and stage vibrations.
|
||||
|
||||
** Simulation Setup
|
||||
** TODO Simulation Setup
|
||||
We now activate the disturbances.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', true, ... % Ground Motion - X direction
|
||||
'Dwy', true, ... % Ground Motion - Y direction
|
||||
'Dwz', true, ... % Ground Motion - Z direction
|
||||
'Fty_x', true, ... % Translation Stage - X direction
|
||||
'Fty_z', true, ... % Translation Stage - Z direction
|
||||
'Fty_x', false, ... % Translation Stage - X direction
|
||||
'Fty_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_z', true ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
@ -292,11 +298,106 @@ And we save the obtained data.
|
||||
[[file:figs/exp_tomo_dist.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
#+begin_important
|
||||
Error motion is what expected from the disturbance measurements.
|
||||
#+end_important
|
||||
|
||||
* Tomography Experiment with Ty raster scans
|
||||
<<sec:tomo_dist_ty_scans>>
|
||||
** Introduction :ignore:
|
||||
In this section, we also perform a tomography experiment with scans of the Translation stage.
|
||||
All the perturbations are included.
|
||||
|
||||
** Simulation Setup
|
||||
We now activate the disturbances.
|
||||
#+begin_src matlab
|
||||
initializeDisturbances(...
|
||||
'Dwx', true, ... % Ground Motion - X direction
|
||||
'Dwy', true, ... % Ground Motion - Y direction
|
||||
'Dwz', true, ... % Ground Motion - Z direction
|
||||
'Fty_x', true, ... % Translation Stage - X direction
|
||||
'Fty_z', true, ... % Translation Stage - Z direction
|
||||
'Frz_z', true ... % Spindle - Z direction
|
||||
);
|
||||
#+end_src
|
||||
We initialize the reference path for all the stages.
|
||||
The Spindle which is rotating at 60rpm and the translation stage is following a triangular path.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1, ...
|
||||
'Dy_type', 'triangular', 'Dy_amplitude', 5e-3, 'Dy_period', 10);
|
||||
#+end_src
|
||||
|
||||
We simulate the model.
|
||||
#+begin_src matlab
|
||||
sim('nass_model');
|
||||
#+end_src
|
||||
|
||||
And we save the obtained data.
|
||||
#+begin_src matlab
|
||||
scans_rz_align_dist = simout;
|
||||
save('./mat/experiment_tomography.mat', 'scans_rz_align_dist', '-append');
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(2, 3, 1);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 1))
|
||||
hold off;
|
||||
ylabel('Displacement $\epsilon_x$ [m]');
|
||||
|
||||
ax2 = subplot(2, 3, 2);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 2))
|
||||
hold off;
|
||||
ylabel('Displacement $\epsilon_y$ [m]');
|
||||
|
||||
ax3 = subplot(2, 3, 3);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 3))
|
||||
hold off;
|
||||
ylabel('Displacement $\epsilon_z$ [m]');
|
||||
|
||||
ax4 = subplot(2, 3, 4);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4))
|
||||
hold off;
|
||||
ylabel('Rotation $\epsilon_{R_x}$ [rad]');
|
||||
|
||||
ax5 = subplot(2, 3, 5);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5))
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Rotation $\epsilon_{R_y}$ [rad]');
|
||||
|
||||
ax6 = subplot(2, 3, 6);
|
||||
hold on;
|
||||
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6))
|
||||
hold off;
|
||||
ylabel('Rotation $\epsilon_{R_z}$ [rad]');
|
||||
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
|
||||
xlim([0.5, inf]);
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/exp_scans_rz_dist.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:exp_scans_rz_dist
|
||||
#+CAPTION: X-Y-Z translations and rotations of the sample w.r.t. the granite when performing tomography experiment and scans with the translation stage at the same time ([[./figs/exp_scans_rz_dist.png][png]], [[./figs/exp_scans_rz_dist.pdf][pdf]])
|
||||
[[file:figs/exp_scans_rz_dist.png]]
|
||||
|
||||
** Conclusion
|
||||
|
||||
* Tomography when the micro-hexapod is not centered
|
||||
<<sec:tomo_hexa_trans>>
|
||||
** Introduction :ignore:
|
||||
|
||||
@ -28,7 +28,7 @@
|
||||
|
||||
* Introduction :ignore:
|
||||
|
||||
* Low Authority Control - Decentralized Integral Force Feedback
|
||||
* Low Authority Control - Decentralized Direct Velocity Feedback
|
||||
** Introduction :ignore:
|
||||
|
||||
** Matlab Init :noexport:ignore:
|
||||
@ -62,30 +62,37 @@ We initialize all the stages with the default parameters.
|
||||
initializeMirror();
|
||||
#+end_src
|
||||
|
||||
We set the references that corresponds to a tomography experiment.
|
||||
#+begin_src matlab
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 1);
|
||||
initializeSimscapeConfiguration();
|
||||
initializeDisturbances('enable', false);
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
initializeController('type', 'hac-iff');
|
||||
initializeController('type', 'hac-dvf');
|
||||
#+end_src
|
||||
|
||||
We set the stiffness of the payload fixation:
|
||||
#+begin_src matlab
|
||||
Kp = 1e8; % [N/m]
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
Kx = tf(zeros(6));
|
||||
Kiff = tf(zeros(6));
|
||||
K = tf(zeros(6));
|
||||
Kdvf = tf(zeros(6));
|
||||
#+end_src
|
||||
|
||||
We identify the system for the following payload masses:
|
||||
#+begin_src matlab
|
||||
Ms = [1, 10, 50];
|
||||
|
||||
Gm_iff = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
Gm_dvf = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
The nano-hexapod has the following leg's stiffness and damping.
|
||||
#+begin_src matlab
|
||||
initializeNanoHexapod('k', 1e5, 'c', 2e2);
|
||||
#+end_src
|
||||
@ -97,25 +104,22 @@ We set the references that corresponds to a tomography experiment.
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G_iff = linearize(mdl, io);
|
||||
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
||||
Gm_iff(i) = {G_iff};
|
||||
G_dvf = linearize(mdl, io);
|
||||
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||
Gm_dvf(i) = {G_dvf};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
save('./mat/optimal_stiffness_control.mat', 'Gm_iff');
|
||||
#+end_src
|
||||
|
||||
** Controller Design
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
@ -125,17 +129,16 @@ We set the references that corresponds to a tomography experiment.
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('Diagonal elements of the Plant');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
@ -152,19 +155,19 @@ Root Locus
|
||||
#+begin_src matlab :exports none :post
|
||||
figure;
|
||||
|
||||
gains = logspace(0, 3, 300);
|
||||
gains = logspace(1, 4, 300);
|
||||
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ...
|
||||
plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ...
|
||||
plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ...
|
||||
'HandleVisibility', 'off');
|
||||
for k = 1:length(gains)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
|
||||
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
|
||||
plot(real(cl_poles), imag(cl_poles), '.', ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
@ -178,13 +181,13 @@ Root Locus
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/opt_stiff_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
||||
exportFig('figs/opt_stdvf_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
||||
#+end_src
|
||||
|
||||
#+name: fig:opt_stiff_iff_root_locus
|
||||
#+name: fig:opt_stdvf_dvf_root_locus
|
||||
#+caption: Root Locus for the
|
||||
#+RESULTS:
|
||||
[[file:figs/opt_stiff_iff_root_locus.png]]
|
||||
[[file:figs/opt_stdvf_dvf_root_locus.png]]
|
||||
|
||||
Damping as function of the gain
|
||||
#+begin_src matlab :exports none
|
||||
@ -199,48 +202,38 @@ Damping as function of the gain
|
||||
|
||||
figure;
|
||||
|
||||
gains = logspace(0, 3, 100);
|
||||
gains = logspace(1, 4, 100);
|
||||
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
for k = 1:length(gains)
|
||||
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
|
||||
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
xlabel('IFF Gain'); ylabel('Modal Damping');
|
||||
xlabel('DVF Gain'); ylabel('Modal Damping');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylim([0, 1]);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Kiff = -200/s*eye(6);
|
||||
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
||||
#+end_src
|
||||
|
||||
* Primary Control
|
||||
* Identification of the dynamics for the Primary controller
|
||||
** Introduction :ignore:
|
||||
Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
|
||||
\[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]
|
||||
|
||||
Then, we compute both the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
|
||||
\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
|
||||
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
and from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
|
||||
\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
Gm_x = {zeros(length(Ms), 1)};
|
||||
Gm_l = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
#+end_src
|
||||
We identify these dynamics with and without using the DVF controller.
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
%% Name of the Simulink File
|
||||
@ -252,9 +245,55 @@ Damping as function of the gain
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
#+end_src
|
||||
|
||||
** Identification of the undamped plant :ignore:
|
||||
#+begin_src matlab :exports none
|
||||
Kdvf_backup = Kdvf;
|
||||
Kdvf = tf(zeros(6));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
G_x = {zeros(length(Ms), 1)};
|
||||
G_l = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||
|
||||
Gx = -G*inv(nano_hexapod.J');
|
||||
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G_x(i) = {Gx};
|
||||
|
||||
Gl = -nano_hexapod.J*G;
|
||||
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||
G_l(i) = {Gl};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
Kdvf = Kdvf_backup;
|
||||
#+end_src
|
||||
|
||||
** Identification of the damped plant :ignore:
|
||||
#+begin_src matlab :exports none
|
||||
Gm_x = {zeros(length(Ms), 1)};
|
||||
Gm_l = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
@ -271,71 +310,107 @@ Damping as function of the gain
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Controller in the task space
|
||||
** Obtained dynamics for the Undamped plant :ignore:
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('Diagonal elements of the Plant');
|
||||
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
|
||||
|
||||
ax2 = subplot(2, 2, 3);
|
||||
ax2 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_z/\mathcal{F}_z$')
|
||||
|
||||
ax3 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend();
|
||||
|
||||
ax3 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('Off-Diagonal elements of the Plant');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
*** Translation
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
* Primary Control in the task space
|
||||
** Introduction :ignore:
|
||||
|
||||
** Plant in the task space
|
||||
Let's look $\bm{G}_\mathcal{X}(s)$.
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
@ -394,85 +469,6 @@ Damping as function of the gain
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Kx = tf(zeros(6));
|
||||
|
||||
h = 1.5;
|
||||
Kx(1,1) = 2e6 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10);
|
||||
|
||||
Kx(2,2) = Kx(1,1);
|
||||
|
||||
h = 1.5;
|
||||
Kx(3,3) = 1e7 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
|
||||
|
||||
ax2 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_z/\mathcal{F}_z$')
|
||||
|
||||
ax3 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
*** Rotations
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -533,20 +529,95 @@ Damping as function of the gain
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
** Control in the task space
|
||||
#+begin_src matlab
|
||||
Kx = tf(zeros(6));
|
||||
|
||||
h = 2.5;
|
||||
Kx(1,1) = 3e7 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1);
|
||||
|
||||
Kx(2,2) = Kx(1,1);
|
||||
|
||||
h = 2.5;
|
||||
Kx(3,3) = 3e7 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
h = 1.5;
|
||||
Kx(4,4) = 1e5 * ...
|
||||
Kx(4,4) = 5e5 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10);
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1);
|
||||
|
||||
Kx(5,5) = Kx(4,4);
|
||||
|
||||
h = 1.5;
|
||||
Kx(6,6) = 2e5 * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10);
|
||||
Kx(6,6) = 5e4 * ...
|
||||
1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
title('Loop Gain $x$ and $y$')
|
||||
|
||||
ax2 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
title('Loop Gain $z$')
|
||||
|
||||
ax3 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -618,67 +689,8 @@ Damping as function of the gain
|
||||
|
||||
** Simulation
|
||||
|
||||
** Control in the leg space
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('Diagonal elements of the Plant');
|
||||
|
||||
ax2 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$d\\mathcal{L}_%i / \\tau_%i$', i, i));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend();
|
||||
|
||||
ax3 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('Off-Diagonal elements of the Plant');
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
* Primary Control in the leg space
|
||||
** Plant in the task space
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
@ -702,24 +714,26 @@ Damping as function of the gain
|
||||
for i = 1:length(Ms)
|
||||
for j = 1:6
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
|
||||
end
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
** Control in the leg space
|
||||
#+begin_src matlab
|
||||
h = 1.5;
|
||||
Kl = 5e6 * eye(6) * ...
|
||||
Kl = 2e7 * eye(6) * ...
|
||||
1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
|
||||
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
|
||||
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10);
|
||||
(s/2/pi/10 + 1)/(s/2/pi/10) * ...
|
||||
1/(1 + s/2/pi/500);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
@ -762,5 +776,281 @@ Damping as function of the gain
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
** Simulations
|
||||
#+begin_src matlab
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
K = Kl*nano_hexapod.J;
|
||||
#+end_src
|
||||
|
||||
* Simulations
|
||||
#+begin_src matlab
|
||||
initializeDisturbances('Fty_x', false, 'Fty_z', false);
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '2');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
hac_dvf_L = {zeros(length(Ms)), 1};
|
||||
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i));
|
||||
|
||||
sim('nass_model');
|
||||
hac_dvf_L(i) = {simout};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
|
||||
#+end_src
|
||||
|
||||
** Results
|
||||
#+begin_src matlab
|
||||
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
n_av = 4;
|
||||
han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
t = simout.Em.En.Time;
|
||||
Ts = t(2)-t(1);
|
||||
|
||||
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
|
||||
|
||||
pxx_dvf_L = zeros(length(f), 6, length(Ms));
|
||||
for i = 1:length(Ms)
|
||||
[pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
|
||||
pxx_dvf_L(:, :, i) = pxx;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(2, 3, 1);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 1)))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 1, i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
ax2 = subplot(2, 3, 2);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 2)))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 2, i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
ax3 = subplot(2, 3, 3);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 3)))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 3, i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
ax4 = subplot(2, 3, 4);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 4)))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 4, i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
ax5 = subplot(2, 3, 5);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 5)))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 5, i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
ax6 = subplot(2, 3, 6);
|
||||
hold on;
|
||||
plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
|
||||
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
|
||||
legend('location', 'southwest');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
|
||||
xlim([f(2), f(end)])
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(2, 3, 1);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $D_x$ [$m$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
ax2 = subplot(2, 3, 2);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $D_y$ [$m$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
ax3 = subplot(2, 3, 3);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $D_z$ [$m$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
ax4 = subplot(2, 3, 4);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $R_x$ [$rad$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
ax5 = subplot(2, 3, 5);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $R_y$ [$rad$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
ax6 = subplot(2, 3, 6);
|
||||
hold on;
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
|
||||
for i = 1:length(Ms)
|
||||
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
|
||||
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('CAS $R_z$ [$rad$]');
|
||||
legend('location', 'southwest');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-11, 1e-5]);
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
|
||||
xlim([f(2), f(end)])
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
ax1 = subplot(2, 3, 1);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Dx [m]');
|
||||
|
||||
ax2 = subplot(2, 3, 2);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Dy [m]');
|
||||
|
||||
ax3 = subplot(2, 3, 3);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Dz [m]');
|
||||
|
||||
ax4 = subplot(2, 3, 4);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Rx [rad]');
|
||||
|
||||
ax5 = subplot(2, 3, 5);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Ry [rad]');
|
||||
|
||||
ax6 = subplot(2, 3, 6);
|
||||
hold on;
|
||||
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
|
||||
'DisplayName', '$\mu$-Station')
|
||||
for i = 1:length(Ms)
|
||||
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
|
||||
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Rz [rad]');
|
||||
legend();
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
xlim([0.5, inf]);
|
||||
#+end_src
|
||||
|
||||
Loading…
Reference in New Issue
Block a user