441 lines
14 KiB
Matlab
441 lines
14 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./src/'); % Path for scripts
|
|
addpath('./mat/'); % Path for data
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Load measured data - hysteresis
|
|
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', '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
|
|
|
|
%% Measured displacement as a function of the output voltage
|
|
figure;
|
|
|
|
hold on;
|
|
for i = [6,5,4,2]
|
|
i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
|
|
plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
|
|
'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
|
|
end
|
|
hold off;
|
|
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
xlim([-20, 150]);
|
|
ylim([-120, 120]);
|
|
|
|
%% Load data for stiffness measurement
|
|
apa_nums = [1 2 4 5 6 8];
|
|
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
|
|
|
|
added_mass = 6.4; % Added mass [kg]
|
|
|
|
%% Plot the deflection at a function of time
|
|
figure;
|
|
hold on;
|
|
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
|
|
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
|
|
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
|
|
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
|
|
% first stroke for stiffness measurements
|
|
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
|
|
anArrow.Parent = gca;
|
|
anArrow.Position = [2, -0.4, 0, -37];
|
|
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');
|
|
|
|
% second stroke for stiffness measurements
|
|
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
|
|
anArrow.Parent = gca;
|
|
anArrow.Position = [18, -37.4, 0, 32.9];
|
|
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');
|
|
|
|
% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
|
|
hold off;
|
|
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');
|
|
|
|
%% Load Data
|
|
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat', 't', 'de');
|
|
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', '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));
|
|
|
|
%% Estimation of the stiffness in Open Circuit and Closed-Circuit
|
|
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_sc = 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)));
|
|
|
|
%% Identification using sweep sine (low frequency)
|
|
load('frf_data_sweep.mat');
|
|
load('frf_data_noise_hf.mat');
|
|
|
|
%% Sampling Frequency
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Fs = 1/Ts; % Sampling Frequency [Hz]
|
|
|
|
%% "Hanning" windows used for the spectral analysis:
|
|
Nfft = floor(2/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
|
|
% Only used to have the frequency vector "f"
|
|
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
|
|
i_lf = f <= 350;
|
|
i_hf = f > 350;
|
|
|
|
%% FRF estimation of the transfer function from u to de
|
|
enc_frf = zeros(length(f), length(apa_nums));
|
|
for i = 1:length(apa_nums)
|
|
[frf_lf, ~] = tfestimate(apa_sweep{i}.u, apa_sweep{i}.de, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.de, win, Noverlap, Nfft, 1/Ts);
|
|
enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% FRF estimation of the transfer function from u to Vs
|
|
iff_frf = zeros(length(f), length(apa_nums));
|
|
for i = 1:length(apa_nums)
|
|
[frf_lf, ~] = tfestimate(apa_sweep{i}.u, apa_sweep{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
|
|
end
|
|
|
|
%% Save the identified dynamics for further analysis
|
|
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
|
|
|
|
%% Plot the FRF from u to de
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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/u$ [m/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-8, 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]);
|
|
|
|
%% Plot the FRF from u to Vs
|
|
figure;
|
|
tiledlayout(2, 1, 'TileSpacing', 'Compact', '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/u$ [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]);
|
|
|
|
%% Long measurement
|
|
long_noise = load('frf_struts_align_3_noise_long.mat', 't', 'u', 'Vs');
|
|
|
|
% Long window for fine frequency axis
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(10/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
% Transfer function estimation
|
|
[frf_noise, f] = tfestimate(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[coh_noise, ~] = mscohere(long_noise.u, long_noise.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Bode plot of the FRF from u to de
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
nexttile();
|
|
hold on;
|
|
plot(f, coh_noise, '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
|
|
hold off;
|
|
xlim([38, 45]);
|
|
ylim([0, 1]);
|
|
|
|
%% Bode plot of the FRF from u to de
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(frf_noise), '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(frf_noise), '.-');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360); ylim([-180, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([38, 45]);
|
|
|
|
%% Load the data
|
|
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'u'); % With the resistor
|
|
wo_k = load('frf_data_1_sweep_lf.mat', 't', 'Vs', 'u'); % Without the resistor
|
|
|
|
%% Large Hanning window for good low frequency estimate
|
|
Nfft = floor(50/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Compute the transfer functions from u to Vs
|
|
[frf_wo_k, f] = tfestimate(wo_k.u, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
[frf_wi_k, ~] = tfestimate(wi_k.u, wi_k.Vs, win, Noverlap, Nfft, 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(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
|
|
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
|
|
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', 'RC model');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
|
hold off;
|
|
ylim([2e-1, 1e0]);
|
|
yticks([0.2, 0.5, 1]);
|
|
legend('location', 'southeast', 'FontSize', 8);
|
|
|
|
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--', 'DisplayName', 'RC');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:45:360); ylim([-5, 60]);
|
|
yticks([0, 15, 30, 45, 60]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([0.2, 8]);
|
|
xticks([0.2, 0.5, 1, 2, 5]);
|
|
|
|
%% Load identification Data
|
|
data = load("2023-03-17_11-28_iff_plant.mat");
|
|
|
|
%% Spectral Analysis setup
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(5/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Compute the transfer function from applied force to measured rotation
|
|
[G_iff, f] = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Basic manually tuned model
|
|
w0z = 2*pi*42.7; % Zero frequency
|
|
xiz = 0.004; % Zero damping
|
|
|
|
w0p = 2*pi*95.2; % Pole frequency
|
|
xip = 0.02; % Pole damping
|
|
|
|
G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));
|
|
|
|
%% Identified IFF plant and manually tuned model of the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
|
|
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, 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([-90, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([0.2, 1e3]);
|
|
|
|
%% Integral Force Feedback Controller
|
|
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
|
|
(s/(s + 2*pi*2)) * ... % HPF: limit low frequency gain
|
|
(1/(1 + s/2/pi/2e3)); % LPF: more robust to high frequency resonances
|
|
|
|
%% Load Data
|
|
data = load("2023-03-17_14-10_damped_plants_new.mat");
|
|
|
|
%% Spectral Analysis setup
|
|
Ts = 1e-4; % Sampling Time [s]
|
|
Nfft = floor(1/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
%% Get the frequency vector
|
|
[~, f] = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);
|
|
|
|
%% Gains used for analysis are between 1 and 50
|
|
i_kept = [5:10];
|
|
|
|
%% Identify the damped plant from u' to de for different IFF gains
|
|
G_dL_frf = {zeros(1,length(i_kept))};
|
|
|
|
for i = 1:length(i_kept)
|
|
[G_dL, ~] = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
|
|
G_dL_frf(i) = {G_dL};
|
|
end
|
|
|
|
%% Fit the data with 2nd order transfer function using vectfit3
|
|
opts = struct();
|
|
|
|
opts.stable = 1; % Enforce stable poles
|
|
opts.asymp = 1; % Force D matrix to be null
|
|
opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint
|
|
opts.skip_pole = 0; % Do NOT skip pole identification
|
|
opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E)
|
|
opts.cmplx_ss = 0; % Create real state space model with block diagonal A
|
|
|
|
opts.spy1 = 0; % No plotting for first stage of vector fitting
|
|
opts.spy2 = 0; % Create magnitude plot for fitting of f(s)
|
|
|
|
Niter = 100; % Number of iteration.
|
|
N = 2; % Order of approximation
|
|
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location
|
|
|
|
G_dL_id = {zeros(1,length(i_kept))};
|
|
|
|
% Identification just between two frequencies
|
|
f_keep = (f>5 & f<200);
|
|
|
|
for i = 1:length(i_kept)
|
|
%% Estimate resonance frequency and damping
|
|
for iter = 1:Niter
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
|
|
end
|
|
G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
|
|
end
|
|
|
|
%% Identified dynamics from u' to de for different IFF gains
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
for i = 1:length(i_kept)
|
|
plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
|
|
plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/u^\prime$ [m/V]');
|
|
xlim([10, 1e3]);
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
|
|
%% Root Locus of the APA300ML with Integral Force Feedback
|
|
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
|
|
gains = logspace(-1, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
G_iff_poles = pole(pade(G_iff_model));
|
|
i = imag(G_iff_poles) > 100; % Only keep relevant poles
|
|
plot(real(G_iff_poles(i)), imag(G_iff_poles(i)), 'kx', ...
|
|
'DisplayName', '$g = 0$');
|
|
G_iff_zeros = tzero(G_iff_model);
|
|
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
|
|
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(pade(G_iff_model), g*K_iff, 1));
|
|
i = imag(clpoles) > 100; % Only keep relevant poles
|
|
plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
for i = 1:length(i_kept)
|
|
plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
|
|
end
|
|
xlabel('Real Part')
|
|
ylabel('Imaginary Part')
|
|
axis equal
|
|
ylim([0, 610]);
|
|
xlim([-300,0]);
|
|
legend('location', 'southwest', 'FontSize', 8);
|