370 lines
11 KiB
Mathematica
370 lines
11 KiB
Mathematica
|
%% 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]);
|