dcm-simscape-model/matlab/dcm_identification.m

203 lines
5.3 KiB
Matlab

% Matlab Init :noexport:ignore:
%% dcm_identification.m
% Extraction of system dynamics using Simscape model
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
%% Simscape Model - Nano Hexapod
addpath('./STEPS/')
%% Initialize Parameters for Simscape model
controller.type = 0; % Open Loop Control
%% Options for Linearization
options = linearizeOptions;
options.SampleTime = 0;
%% Open Simulink Model
mdl = 'simscape_dcm';
open(mdl)
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(1, 3, 1000);
% #+name: fig:schematic_system_inputs_outputs
% #+caption: Dynamical system with inputs and outputs
% #+RESULTS:
% [[file:figs/schematic_system_inputs_outputs.png]]
% The system is identified from the Simscape model.
%% Input/Output definition
clear io; io_i = 1;
%% Inputs
% Control Input {3x1} [N]
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
%% Outputs
% Interferometers {3x1} [m]
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
%% Extraction of the dynamics
G = linearize(mdl, io);
%% Input and Output names
G.InputName = {'u_ur', 'u_uh', 'u_d'};
G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'};
% Plant in the frame of the fastjacks
load('dcm_kinematics.mat');
% #+name: fig:schematic_jacobian_frame_fastjack
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
% #+RESULTS:
% [[file:figs/schematic_jacobian_frame_fastjack.png]]
%% Compute the system in the frame of the fastjacks
G_pz = J_a_111*inv(J_s_111)*G;
% #+name: tab:dc_gain_plan_fj
% #+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
% #+attr_latex: :center t :booktabs t
% #+RESULTS:
% | 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
% | 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
% | 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |
% The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].
%% Bode plot for the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
'DisplayName', 'ur');
for i = 1:2
for j = i+1:3
plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
ylim([1e-13, 1e-6]);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), 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([freqs(1), freqs(end)]);
% #+name: fig:schematic_jacobian_frame_crystal
% #+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
% #+RESULTS:
% [[file:figs/schematic_jacobian_frame_crystal.png]]
G_mr = inv(J_s_111)*G*inv(J_a_111');
% #+RESULTS:
% | 1.9978e-09 | 3.9657e-09 | 7.7944e-09 |
% | 3.9656e-09 | 8.4979e-08 | -1.5135e-17 |
% | 7.7944e-09 | -3.9252e-17 | 1.834e-07 |
%% Bode plot for the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
'DisplayName', 'd');
plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
'DisplayName', 'uh');
plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
'DisplayName', 'ur');
for i = 1:2
for j = i+1:3
plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), 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([freqs(1), freqs(end)]);
%% Bode plot for the plant
fig = figure;
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
for i_out = 1:3
for i_in = 1:3
ax = nexttile;
plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
end
end
linkaxes(findall(fig, 'type', 'axes'),'xy');
xlim([freqs(1), freqs(end)]);