tdehaeze/test-bench-apa300ml
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add tangled matlab scripts

Browse Source
This commit is contained in:
Thomas Dehaeze
2021-06-17 23:23:08 +02:00
parent f890951c48
commit 31d71c3d4d
10 changed files with 2288 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

247
matlab/apa_meas_analysis_1.m Normal file
View File

@@ -0,0 +1,247 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
addpath('./mat/');
addpath('./src/');
%% Load data
apa_sweep = load(sprintf('mat/frf_data_%i_sweep.mat', 1), 't', 'Va', 'Vs', 'da', 'de');
%% Time vector
t = apa_sweep.t - apa_sweep.t(1) ; % Time vector [s]
%% Plot the excitation signal
figure;
plot(t, apa_sweep.Va)
xlabel('Time [s]'); ylabel('Excitation Voltage $V_a$ [V]');
%% Sampling Frequency / Time
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
win = hanning(ceil(1*Fs)); % Hannning Windows
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
%% Compute the coherence
[enc_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[int_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);
%% Plot the coherence
figure;
hold on;
plot(f, enc_coh, 'DisplayName', '$d_e/V_a$');
plot(f, int_coh, 'DisplayName', '$d_a/V_a$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
%% TF - Encoder and interferometer
[frf_enc, ~] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[frf_int, ~] = tfestimate(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_enc), 'color', colors(1, :), ...
'DisplayName', 'Encoder');
plot(f, abs(frf_int), 'color', colors(2, :), ...
'DisplayName', 'Interferometer');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');
ylim([1e-9, 1e-3]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_enc), 'color', colors(1, :));
plot(f, 180/pi*angle(frf_int), 'color', colors(2, :));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Compute the coherence from Va to Vs
[iff_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);
%% Plot the coherence
figure;
hold on;
plot(f, iff_coh, 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
%% Compute the TF from Va to Vs
[iff_sweep, ~] = tfestimate(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);
%% Plot the TF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_sweep), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_sweep), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'Va', 'de');
% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);
ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes
%% Plot the excitation voltages and measured displacements
figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
plot(apa_hyst.t, apa_hyst.Va)
xlabel('Time [s]'); ylabel('Output Voltage [V]');
ax2 = nexttile;
plot(apa_hyst.t, apa_hyst.de)
xlabel('Time [s]'); ylabel('Measured Displacement [m]');
%% Measured displacement as a function of the output voltage
figure;
tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([1,2]);
hold on;
for i = flip(1:6)
i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)), ...
'DisplayName', sprintf('$V_a = %.1f [V]$', ampls(i)))
end
hold off;
xlabel('Output Voltage [V]'); ylabel('Measured Displacement [m]');
legend('location', 'northeast');
xlim([-4, 4]); ylim([-1.2e-4, 1.2e-4]);
ax2 = nexttile;
hold on;
for i = flip(1:6)
i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)))
end
hold off;
xlim([-0.4, 0.4]); ylim([-0.8e-5, 0.8e-5]);
%% Load data for stiffness measurement
apa_mass = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');
apa_mass.de = apa_mass.de - mean(apa_mass.de(apa_mass.t<11));
%% Plot the deflection at a function of time
figure;
plot(apa_mass.t, apa_mass.de, 'k-');
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
added_mass = 6.4; % Added mass [kg]
k = 9.8 * added_mass / (mean(apa_mass.de(apa_mass.t > 12 & apa_mass.t < 12.5)) - mean(apa_mass.de(apa_mass.t > 20 & apa_mass.t < 20.5)));
wz = 2*pi*94; % [rad/s]
msus = 5.7; % [kg]
k = msus * wz^2;
%% Load Data
add_mass_oc = load(sprintf('frf_data_%i_add_mass_open_circuit.mat', 1), 't', 'de');
add_mass_cc = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');
%% Zero displacement at initial time
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));
figure;
hold on;
plot(add_mass_oc.t, add_mass_oc.de, 'DisplayName', 'Not connected');
plot(add_mass_cc.t, add_mass_cc.de, 'DisplayName', 'Connected');
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast');
apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
apa_k_cc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));
%% Load the data
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'Va'); % With the resistor
wo_k = load('frf_data_1_sweep_lf.mat', 't', 'Vs', 'Va'); % Without the resistor
win = hanning(ceil(50*Fs)); % Hannning Windows
%% Compute the transfer functions from Va to Vs
[frf_wo_k, f] = tfestimate(wo_k.Va, wo_k.Vs, win, [], [], 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.Va, wi_k.Vs, win, [], [], 1/Ts);
%% Model for the high pass filter
C = 5.1e-6; % Sensor Stack capacitance [F]
R = 80.6e3; % Parallel Resistor [Ohm]
f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]
G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);
%% Compare the HPF model and the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $k$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $k$');
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', sprintf('HPF $f_o = %.2f [Hz]$', f0));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-1, 1e0]);
legend('location', 'southeast')
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_wo_k));
plot(f, 180/pi*angle(frf_wi_k));
plot(f, 180/pi*angle(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-45, 90]);
linkaxes([ax1,ax2],'x');
xlim([0.2, 8]);

217
matlab/apa_meas_analysis_all.m Normal file
View File

@@ -0,0 +1,217 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('./mat/');
addpath('./src/');
added_mass = 6.4; % Added mass [kg]
apa_nums = [1 2 4 5 6 7 8];
%% Load Data
apa_mass = {};
for i = 1:length(apa_nums)
apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
% The initial displacement is set to zero
apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
end
%% Plot the time domain measured deflection
figure;
hold on;
for i = 1:length(apa_nums)
plot(apa_mass{i}.t, apa_mass{i}.de, 'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
%% Compute the stiffness
apa_k = zeros(length(apa_nums), 1);
for i = 1:length(apa_nums)
apa_k(i) = 9.8 * added_mass / (mean(apa_mass{i}.de(apa_mass{i}.t > 12 & apa_mass{i}.t < 12.5)) - mean(apa_mass{i}.de(apa_mass{i}.t > 20 & apa_mass{i}.t < 20.5)));
end
%% Second identification
apa_sweep = {};
for i = 1:length(apa_nums)
apa_sweep(i) = {load(sprintf('frf_data_%i_sweep.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
%% Third identification
apa_noise_hf = {};
for i = 1:length(apa_nums)
apa_noise_hf(i) = {load(sprintf('frf_data_%i_noise_hf.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
%% Time vector
t = apa_sweep{1}.t - apa_sweep{1}.t(1) ; % Time vector [s]
%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
win = hanning(ceil(0.5*Fs)); % Hannning Windows
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.Va, apa_sweep{1}.de, win, [], [], 1/Ts);
i_lf = f <= 350;
i_hf = f > 350;
%% Coherence computation
coh_enc = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
[coh_lf, ~] = mscohere(apa_sweep{i}.Va, apa_sweep{i}.de, win, [], [], 1/Ts);
[coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
coh_enc(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
figure;
hold on;
for i = 1:length(apa_nums)
plot(f, coh_enc(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
%% Transfer function estimation
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
[frf_lf, ~] = tfestimate(apa_sweep{i}.Va, apa_sweep{i}.de, win, [], [], 1/Ts);
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(enc_frf(:, i)), ...
'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-9, 1e-3]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(enc_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(enc_frf(:, i)), ...
'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([2e-5, 4e-4]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(enc_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-10, 180]);
linkaxes([ax1,ax2],'x');
xlim([80, 120]);
%% Compute the Coherence
coh_iff = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
[coh_lf, ~] = mscohere(apa_sweep{i}.Va, apa_sweep{i}.Vs, win, [], [], 1/Ts);
[coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
coh_iff(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
%% Plot the coherence
figure;
hold on;
for i = 1:length(apa_nums)
plot(f, coh_iff(:, i));
end;
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);
%% FRF estimation of the transfer function from Va to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
[frf_lf, ~] = tfestimate(apa_sweep{i}.Va, apa_sweep{i}.Vs, win, [], [], 1/Ts);
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
%% Plot the FRF from Va to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, abs(iff_frf(:, i)), ...
'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(iff_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Remove the APA 7 (6 in the list) from measurements
apa_nums(6) = [];
enc_frf(:,6) = [];
iff_frf(:,6) = [];
%% Save the measured FRF
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

383
matlab/apa_simscape_model_comp.m Normal file
View File

@@ -0,0 +1,383 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Add useful folders to the path
addpath('test_bench_apa300ml/');
addpath('png/');
addpath('mat/');
addpath('src/');
%% Frequency vector used for many plots
freqs = 2*logspace(0, 3, 1000);
%% Open Simscape Model
options = linearizeOptions;
options.SampleTime = 0;
% Name of the Simulink File
mdl = 'test_bench_apa300ml';
open(mdl)
%% Initialize the structure with default values
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
'type', '2dof', ...
'Ga', 1, ... % Actuator constant [N/V]
'Gs', 1); % Sensor constant [V/m]
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer
%% Run the linearization
Ga = linearize(mdl, io, 0.0, options);
Ga.InputName = {'Va'};
Ga.OutputName = {'Vs', 'de', 'da'};
%% Bode plot of the transfer function from u to taum
freqs = logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
%% Bode plot of the transfer function from Va to de and da
freqs = logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
plot(freqs, abs(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])
linkaxes([ax1,ax2],'x');
%% Load Data
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
%% Initialize a 2DoF APA with Ga=Gs=1
n_hexapod.actuator = initializeAPA(...
'type', '2dof', ...
'ga', 1, ...
'gs', 1);
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Attocube
%% Identification
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
%% Estimated Actuator Constant
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
%% Estimated Sensor Constant
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
'type', '2dof', ...
'ga', ga, ... % Actuator gain [N/V]
'gs', gs); % Sensor gain [V/m]
%% Identify again the dynamics with correct Ga,Gs
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
%% Bode plot of the transfer function from u to de
freqs = logspace(1,4,1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
freqs = logspace(1,4,1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Initialize the APA as a flexible body
n_hexapod.actuator = initializeAPA(...
'type', 'flexible', ...
'ga', 1, ...
'gs', 1);
%% Identify the dynamics
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
%% Actuator Constant
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
%% Sensor Constant
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
'type', 'flexible', ...
'ga', ga, ... % Actuator gain [N/V]
'gs', gs); % Sensor gain [V/m]
%% Identify with updated constants
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
%% Bode plot of the transfer function from V_a to d_e (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Optimized parameters
n_hexapod.actuator = initializeAPA('type', '2dof', ...
'Ga', -32.2, ...
'Gs', 0.088, ...
'k', ones(6,1)*0.38e6, ...
'ke', ones(6,1)*1.75e6, ...
'ka', ones(6,1)*3e7, ...
'c', ones(6,1)*1.3e2, ...
'ce', ones(6,1)*1e1, ...
'ca', ones(6,1)*1e1 ...
);
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
%% Identification with optimized parameters
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de'};
%% Comparison of the experimental data and Simscape Model
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
ax1b = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
ax2b = nexttile;
hold on;
for i = 1:length(apa_nums)
plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2,ax1b,ax2b],'x');
xlim([10, 2e3]);

45
matlab/basic_meas_geometrical.m Normal file
View File

@@ -0,0 +1,45 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('./mat/');
%% Measured height for all the APA at the 8 locations
apa1 = 1e-6*[0, -0.5 , 3.5 , 3.5 , 42 , 45.5, 52.5 , 46];
apa2 = 1e-6*[0, -2.5 , -3 , 0 , -1.5 , 1 , -2 , -4];
apa3 = 1e-6*[0, -1.5 , 15 , 17.5 , 6.5 , 6.5 , 21 , 23];
apa4 = 1e-6*[0, 6.5 , 14.5 , 9 , 16 , 22 , 29.5 , 21];
apa5 = 1e-6*[0, -12.5, 16.5 , 28.5 , -43 , -52 , -22.5, -13.5];
apa6 = 1e-6*[0, -8 , -2 , 5 , -57.5, -62 , -55.5, -52.5];
apa7 = 1e-6*[0, 19.5 , -8 , -29.5, 75 , 97.5, 70 , 48];
apa7b = 1e-6*[0, 9 , -18.5, -30 , 31 , 46.5, 16.5 , 7.5];
apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7b};
%% X-Y positions of the measurements points
W = 20e-3; % Width [m]
L = 61e-3; % Length [m]
d = 1e-3; % Distance from border [m]
l = 15.5e-3; % [m]
pos = [[-L/2 + d; W/2 - d],
[-L/2 + l - d; W/2 - d],
[-L/2 + l - d; -W/2 + d],
[-L/2 + d; -W/2 + d],
[L/2 - l + d; W/2 - d],
[L/2 - d; W/2 - d],
[L/2 - d; -W/2 + d],
[L/2 - l + d; -W/2 + d]];
%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7);
for i = 1:7
fun = @(x)max(abs(([pos; apa{i}]-[0;0;x(1)])'*([x(2:3);1]/norm([x(2:3);1]))));
x0 = [0;0;0];
[x, min_d] = fminsearch(fun,x0);
apa_d(i) = min_d;
end

97
matlab/basic_meas_spurious_res.m Normal file
View File

@@ -0,0 +1,97 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('mat/');
%% Load Data
bending_X = load('apa300ml_bending_X_top.mat');
%% Spectral Analysis setup
Ts = bending_X.Track1_X_Resolution; % Sampling Time [s]
win = hann(ceil(1/Ts));
%% Compute the transfer function from applied force to measured rotation
[G_bending_X, f] = tfestimate(bending_X.Track1, bending_X.Track2, win, [], [], 1/Ts);
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_X), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-1]);
text(280, 5.5e-2,{'280Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(840, 2.0e-3,{'840Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1400, 7.0e-3,{'1400Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
%% Load Data
bending_Y = load('apa300ml_bending_Y_top.mat');
%% Compute the transfer function
[G_bending_Y, ~] = tfestimate(bending_Y.Track1, bending_Y.Track2, win, [], [], 1/Ts);
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_Y), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 3e-2])
text(412, 1.5e-2,{'412Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1218, 1.5e-2,{'1220Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
%% Load Data
torsion = load('apa300ml_torsion_left.mat');
%% Compute transfer function
[G_torsion, ~] = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_torsion), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 6e-4,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
%% Load data
torsion = load('apa300ml_torsion_top.mat');
%% Compute transfer function
[G_torsion_top, ~] = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);
%% Plot the two transfer functions
figure;
hold on;
plot(f, abs(G_torsion), 'k-', 'DisplayName', 'Left excitation');
plot(f, abs(G_torsion_top), '-', 'DisplayName', 'Top excitation');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 2e-3,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
legend('location', 'northwest');
figure;
hold on;
plot(f, abs(G_torsion), 'DisplayName', 'Torsion');
plot(f, abs(G_bending_X), 'DisplayName', 'Bending - X');
plot(f, abs(G_bending_Y), 'DisplayName', 'Bending - Y');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 1e-1]);
legend('location', 'southeast');

112
matlab/basic_meas_stroke.m Normal file
View File

@@ -0,0 +1,112 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('./mat/');
%% Load the measurements
apa300ml_1s = {};
for i = 1:7
apa300ml_1s(i) = {load(['mat/stroke_apa_1stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end
%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
t = apa300ml_1s{i}.t;
apa300ml_1s{i}.d = apa300ml_1s{i}.d - mean(apa300ml_1s{i}.d(t > 1.9 & t < 2.0));
apa300ml_1s{i}.d = apa300ml_1s{i}.d(t > 2.0 & t < 10.0);
apa300ml_1s{i}.V = apa300ml_1s{i}.V(t > 2.0 & t < 10.0);
apa300ml_1s{i}.t = apa300ml_1s{i}.t(t > 2.0 & t < 10.0);
end
%% Applied voltage as a function of time
figure;
plot(apa300ml_1s{1}.t, 20*apa300ml_1s{1}.V)
xlabel('Time [s]'); ylabel('Voltage [V]');
ylim([-20,160]); yticks([-20 0 20 40 60 80 100 120 140 160]);
%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
plot(apa300ml_1s{i}.t, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
plot(20*apa300ml_1s{i}.V, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-140, 0]);
%% Load the measurements
apa300ml_2s = {};
for i = 1:7
apa300ml_2s(i) = {load(['mat/stroke_apa_2stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end
%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
t = apa300ml_2s{i}.t;
apa300ml_2s{i}.d = apa300ml_2s{i}.d - mean(apa300ml_2s{i}.d(t > 1.9 & t < 2.0));
apa300ml_2s{i}.d = apa300ml_2s{i}.d(t > 2.0 & t < 10.0);
apa300ml_2s{i}.V = apa300ml_2s{i}.V(t > 2.0 & t < 10.0);
apa300ml_2s{i}.t = apa300ml_2s{i}.t(t > 2.0 & t < 10.0);
end
%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
plot(apa300ml_2s{i}.t, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)
ylim([-250, 0]);
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
plot(20*apa300ml_2s{i}.V, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-250, 0]);
%% Motion induced by applying a voltage to the three stack is the sum to the previous two measured displacements
apa300ml_3s = {};
for i = 1:7
apa300ml_3s(i) = apa300ml_1s(i);
apa300ml_3s{i}.d = apa300ml_1s{i}.d + apa300ml_2s{i}.d;
end
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
plot(20*apa300ml_3s{i}.V, 1e6*apa300ml_3s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-400, 0]);
%% Estimate the maximum stroke
apa300ml_stroke = zeros(1, 7);
for i = 1:7
apa300ml_stroke(i) = max(apa300ml_3s{i}.d) - min(apa300ml_3s{i}.d);
end

369
matlab/strut_meas_analysis_1.m Normal file
View File

@@ -0,0 +1,369 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('./mat/');
addpath('./src/');
%% Load Data
leg_sweep = load(sprintf('frf_data_leg_%i_sweep.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
leg_noise_hf = load(sprintf('frf_data_leg_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
%% Time vector
t = leg_sweep.t - leg_sweep.t(1) ; % Time vector [s]
%% Sampling frequency/time
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
win = hanning(ceil(0.5*Fs)); % Hannning Windows
% Only used to have the frequency vector "f"
[~, f] = tfestimate(leg_sweep.Va, leg_sweep.de, win, [], [], 1/Ts);
i_lf = f <= 350; % Indices used for the low frequency
i_hf = f > 350; % Indices used for the low frequency
%% Compute the coherence for both excitation signals
[int_coh_sweep, ~] = mscohere(leg_sweep.Va, leg_sweep.da, win, [], [], 1/Ts);
[int_coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);
%% Combine the coherence
int_coh = [int_coh_sweep(i_lf); int_coh_noise_hf(i_hf)];
%% Plot the coherence
figure;
hold on;
plot(f, int_coh(:, 1), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([10, 2e3]); ylim([0, 1]);
%% Compute FRF function from Va to da
[frf_sweep, ~] = tfestimate(leg_sweep.Va, leg_sweep.da, win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);
%% Combine the FRF
int_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];
%% Plot the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(int_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Compute the coherence for both excitation signals
[iff_coh_sweep, ~] = mscohere(leg_sweep.Va, leg_sweep.Vs, win, [], [], 1/Ts);
[iff_coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);
%% Combine the coherence
iff_coh = [iff_coh_sweep(i_lf); iff_coh_noise_hf(i_hf)];
%% Plot the coherence
figure;
hold on;
plot(f, iff_coh, 'k-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([10, 2e3]); ylim([0, 1]);
%% Compute the FRF
[frf_sweep, ~] = tfestimate(leg_sweep.Va, leg_sweep.Vs, win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);
%% Combine the FRF
iff_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];
%% Plot the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Load data
leg_enc_sweep = load(sprintf('frf_data_leg_coder_badly_align_%i_noise.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
leg_enc_noise_hf = load(sprintf('frf_data_leg_coder_badly_align_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
%% Compute the coherence for both excitation signals
[int_coh_sweep, ~] = mscohere(leg_enc_sweep.Va, leg_enc_sweep.da, win, [], [], 1/Ts);
[int_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.da, win, [], [], 1/Ts);
%% Combine the coherinte
int_coh = [int_coh_sweep(i_lf); int_coh_noise_hf(i_hf)];
%% Plot the coherence
figure;
hold on;
plot(f, int_coh(:, 1), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([10, 2e3]); ylim([0, 1]);
%% Compute FRF function from Va to da
[frf_sweep, ~] = tfestimate(leg_enc_sweep.Va, leg_enc_sweep.da, win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.da, win, [], [], 1/Ts);
%% Combine the FRF
int_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];
%% Plot the FRF from Va to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_with_enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(int_with_enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Plot the FRF from Va to da with and without the encoder
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_with_enc_frf), '-', 'DisplayName', 'With encoder');
plot(f, abs(int_frf), '-', 'DisplayName', 'Without encoder');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);
legend('location', 'northeast')
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(int_with_enc_frf), '-');
plot(f, 180/pi*angle(int_frf), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Compute the coherence for both excitation signals
[enc_coh_sweep, ~] = mscohere(leg_enc_sweep.Va, leg_enc_sweep.de, win, [], [], 1/Ts);
[enc_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);
%% Combine the coherence
enc_coh = [enc_coh_sweep(i_lf); enc_coh_noise_hf(i_hf)];
%% Plot the coherence
figure;
hold on;
plot(f, enc_coh(:, 1), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([10, 2e3]); ylim([0, 1]);
%% Compute FRF function from Va to da
[frf_sweep, ~] = tfestimate(leg_enc_sweep.Va, leg_enc_sweep.de, win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);
%% Combine the FRF
enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];
%% Plot the FRF from Va to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf), 'DisplayName', 'Encoder');
plot(f, abs(int_with_enc_frf), 'DisplayName', 'Interferometer');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(enc_frf));
plot(f, 180/pi*angle(int_with_enc_frf));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Transfer function from Vs to de with indicated resonances
figure;
hold on;
plot(f, abs(enc_frf), 'k-');
text(93, 4e-4, {'93Hz'}, 'VerticalAlignment','bottom','HorizontalAlignment','center')
text(200, 1.3e-4,{'197Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(300, 4e-6, {'290Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(400, 1.4e-6,{'376Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); xlabel('Frequency [Hz]');
hold off;
ylim([1e-7, 1e-3]); xlim([10, 2e3]);
%% Compute the coherence for both excitation signals
[iff_coh_sweep, ~] = mscohere(leg_enc_sweep.Va, leg_enc_sweep.Vs, win, [], [], 1/Ts);
[iff_coh_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);
%% Combine the coherence
iff_coh = [iff_coh_sweep(i_lf); iff_coh_noise_hf(i_hf)];
%% Plot the coherence
figure;
hold on;
plot(f, iff_coh, 'k-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([10, 2e3]); ylim([0, 1]);
%% Compute FRF function from Va to da
[frf_sweep, ~] = tfestimate(leg_enc_sweep.Va, leg_enc_sweep.Vs, win, [], [], 1/Ts);
[frf_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);
%% Combine the FRF
iff_with_enc_frf = [frf_sweep(i_lf); frf_noise_hf(i_hf)];
%% Plot FRF of the transfer function from Va to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_with_enc_frf), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_with_enc_frf), 'k');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Compare the IFF plant with and without the encoders
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_with_enc_frf), 'DisplayName', 'With Encoder');
plot(f, abs(iff_frf), 'DisplayName', 'Without Encoder');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8);
ylim([1e-2, 1e2]);
ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_with_enc_frf));
plot(f, 180/pi*angle(iff_frf));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);

217
matlab/strut_meas_analysis_all.m Normal file
View File

@@ -0,0 +1,217 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
colors = colororder;
addpath('./mat/');
addpath('./src/');
%% Numnbers of the measured legs
leg_nums = [1 2 3 4 5];
%% First identification (low frequency noise)
leg_noise = {};
for i = 1:length(leg_nums)
leg_noise(i) = {load(sprintf('frf_data_leg_coder_%i_noise.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
%% Second identification (high frequency noise)
leg_noise_hf = {};
for i = 1:length(leg_nums)
leg_noise_hf(i) = {load(sprintf('frf_data_leg_coder_%i_noise_hf.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
%% Time vector
t = leg_noise{1}.t - leg_noise{1}.t(1) ; % Time vector [s]
%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
win = hanning(ceil(0.5*Fs)); % Hannning Windows
% Only used to have the frequency vector "f"
[~, f] = tfestimate(leg_noise{1}.Va, leg_noise{1}.de, win, [], [], 1/Ts);
i_lf = f <= 350;
i_hf = f > 350;
%% Coherence computation
coh_enc = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[coh_lf, ~] = mscohere(leg_noise{i}.Va, leg_noise{i}.de, win, [], [], 1/Ts);
[coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.de, win, [], [], 1/Ts);
coh_enc(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
%% Plot the coherence
figure;
hold on;
for i = 1:length(leg_nums)
plot(f, coh_enc(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([10, 2e3]); ylim([0, 1]);
%% Transfer function estimation
enc_frf = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[frf_lf, ~] = tfestimate(leg_noise{i}.Va, leg_noise{i}.de, win, [], [], 1/Ts);
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.de, win, [], [], 1/Ts);
enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
%% Bode plot of the FRF from Va to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
plot(f, abs(enc_frf(:, i)), ...
'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(enc_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Coherence computation
coh_int = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[coh_lf, ~] = mscohere(leg_noise{i}.Va, leg_noise{i}.da, win, [], [], 1/Ts);
[coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.da, win, [], [], 1/Ts);
coh_int(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
%% Plot coherence
figure;
hold on;
for i = 1:length(leg_nums)
plot(f, coh_int(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([10, 2e3]); ylim([0, 1]);
%% Transfer function estimation
int_frf = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[frf_lf, ~] = tfestimate(leg_noise{i}.Va, leg_noise{i}.da, win, [], [], 1/Ts);
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.da, win, [], [], 1/Ts);
int_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
%% Plot the FRF from Va to de (interferometer)
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
plot(f, abs(int_frf(:, i)), ...
'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-9, 1e-3]);
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(int_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Coherence
coh_iff = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[coh_lf, ~] = mscohere(leg_noise{i}.Va, leg_noise{i}.Vs, win, [], [], 1/Ts);
[coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.Vs, win, [], [], 1/Ts);
coh_iff(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
%% Plot the coherence
figure;
hold on;
for i = 1:length(leg_nums)
plot(f, coh_iff(:, i));
end;
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([10, 2e3]); ylim([0, 1]);
%% FRF estimation of the transfer function from Va to Vs
iff_frf = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
[frf_lf, ~] = tfestimate(leg_noise{i}.Va, leg_noise{i}.Vs, win, [], [], 1/Ts);
[frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.Vs, win, [], [], 1/Ts);
iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
%% Plot the FRF from Va to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
plot(f, abs(iff_frf(:, i)), ...
'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(iff_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Save the estimated FRF for further analysis
save('mat/meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');

597
matlab/strut_simscape_model_comp.m Normal file
View File

@@ -0,0 +1,597 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Add useful folders to the path
addpath('test_bench_struts/');
addpath('png/');
addpath('mat/');
addpath('src/');
%% Frequency vector used for many plots
freqs = 2*logspace(0, 3, 1000);
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'test_bench_struts';
%% Open the Simulink File
open(mdl)
%% Initialize structure containing data for the Simscape model
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', '2dof');
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer
%% Run the linearization
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
%% Bode plot of the transfer functions
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
plot(freqs, abs(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');
ax1b = nexttile([2,1]);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 180])
ax2b = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([0, 180])
linkaxes([ax1,ax2,ax1b,ax2b],'x');
xlim([10, 2e3]);
%% Load measured FRF
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
%% Add time delay to the Simscape model
Gs = exp(-s*Ts)*Gs;
%% Compare the FRF and identified dynamics from Va to Vs and da
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(int_frf(:, 1)), 'color', [0,0,0,0.2], ...
'DisplayName', 'Meas. FRF');
for i = 2:length(leg_nums)
plot(f, abs(int_frf(:, i)), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
'DisplayName', 'Meas. FRF');
for i = 1:length(leg_nums)
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast');
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(int_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
ax2b = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2,ax1b,ax2b],'x');
xlim([10, 2e3]);
%% Compare the FRF and identified dynamics from Va to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], ...
'DisplayName', 'Meas. FRF');
for i = 2:length(leg_nums)
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([20, 2e3]);
%% Load measured FRF of the struts
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
%% Initialize Simscape data
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', 'flexible');
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
%% Identification
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName = {'Va'};
Gs.OutputName = {'de'};
%% Measured FRF from Vs to de and identified dynamics using the flexible APA
freqs = 2*logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
'DisplayName', 'Meas. FRF');
for i = 2:length(leg_nums)
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Considered misalignments
dy_aligns = [-0.5, -0.1, 0, 0.1, 0.5]*1e-3; % [m]
%% Transfer functions from u to de for all the misalignment in y direction
Gs_align = {zeros(length(dy_aligns), 1)};
for i = 1:length(dy_aligns)
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0; dy_aligns(i); 0]);
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs_align(i) = {G};
end
%% Transfer function from Vs to de - effect of x-misalignment
freqs = 2*logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dy_aligns)
plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$d_y = %.1f$ [mm]', 1e3*dy_aligns(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(dy_aligns)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Considered misalignments
dx_aligns = [-0.1, -0.05, 0, 0.05, 0.1]*1e-3; % [m]
%% Transfer functions from u to de for all the misalignment in x direction
Gs_align = {zeros(length(dx_aligns), 1)};
for i = 1:length(dx_aligns)
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [dx_aligns(i); 0; 0]);
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs_align(i) = {G};
end
%% Transfer function from Vs to de - effect of x-misalignment
freqs = 2*logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dx_aligns)
plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$d_x = %.2f$ [mm]', 1e3*dx_aligns(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(dx_aligns)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
%% Tuned misalignment [m]
d_aligns = [[-0.05, -0.3, 0];
[ 0, 0.5, 0];
[-0.1, -0.3, 0];
[ 0, 0.3, 0];
[-0.05, 0.05, 0]]'*1e-3;
%% Idenfity the transfer function from actuator to encoder for all cases
Gs_align = {zeros(size(d_aligns,2), 1)};
for i = 1:size(d_aligns,2)
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', d_aligns(:,i));
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs_align(i) = {G};
end
%% Comparison of the plants (encoder output) when tuning the misalignment
freqs = 2*logspace(0, 3, 1000);
figure;
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 1)));
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('de', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
ax2 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 2)));
plot(freqs, abs(squeeze(freqresp(Gs_align{2}('de', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
ax3 = nexttile(4);
hold on;
plot(f, abs(enc_frf(:, 3)), 'DisplayName', 'Meas.');
plot(freqs, abs(squeeze(freqresp(Gs_align{3}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', 'Model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
legend('location', 'southwest', 'FontSize', 8);
ax4 = nexttile(5);
hold on;
plot(f, abs(enc_frf(:, 4)));
plot(freqs, abs(squeeze(freqresp(Gs_align{4}('de', 'Va'), freqs, 'Hz'))));
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = nexttile(6);
hold on;
plot(f, abs(enc_frf(:, 5)));
plot(freqs, abs(squeeze(freqresp(Gs_align{5}('de', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
linkaxes([ax1,ax2,ax3,ax4,ax5],'xy');
xlim([20, 2e3]); ylim([1e-8, 1e-3]);
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
%% APA Initialization
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0.1e-3; 0.5e-3; 0]);
%% Tested bending stiffnesses [Nm/rad]
kRs = [3, 4, 5, 6, 7];
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kRs), 1)};
for i = 1:length(kRs)
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
'type', '4dof', ...
'kRx', kRs(i), ...
'kRy', kRs(i));
n_hexapod.flex_top = initializeTopFlexibleJoint(...
'type', '4dof', ...
'kRx', kRs(i), ...
'kRy', kRs(i));
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs(i) = {G};
end
%% Plot the obtained transfer functions for all the bending stiffnesses
freqs = 2*logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kRs)
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$k_R = %.0f$ [Nm/rad]', kRs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(kRs)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([20, 2e3]);
%% Tested axial stiffnesses [N/m]
kzs = [5e7 7.5e7 1e8 2.5e8];
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kzs), 1)};
for i = 1:length(kzs)
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
'type', '4dof', ...
'kz', kzs(i));
n_hexapod.flex_top = initializeTopFlexibleJoint(...
'type', '4dof', ...
'kz', kzs(i));
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs(i) = {G};
end
%% Plot the obtained transfer functions for all the axial stiffnesses
freqs = 2*logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kzs)
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$k_z = %.1e$ [N/m]', kzs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');
ax2 = nexttile;
hold on;
for i = 1:length(kzs)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([20, 2e3]);
%% Tested bending dampings [Nm/(rad/s)]
cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
%% Idenfity the transfer function from actuator to encoder for all bending dampins
Gs = {zeros(length(kRs), 1)};
for i = 1:length(kRs)
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
'type', '4dof', ...
'cRx', cRs(i), ...
'cRy', cRs(i));
n_hexapod.flex_top = initializeTopFlexibleJoint(...
'type', '4dof', ...
'cRx', cRs(i), ...
'cRy', cRs(i));
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
G.InputName = {'Va'};
G.OutputName = {'de'};
Gs(i) = {G};
end
%% Plot the obtained transfer functions for all the bending stiffnesses
freqs = 2*logspace(1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kRs)
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'southwest');
ax2 = nexttile;
hold on;
for i = 1:length(kRs)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);
linkaxes([ax1,ax2],'x');
xlim([20, 2e3]);