tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Change the way control is generated. Add script to test control

Browse Source
This commit is contained in:
Thomas Dehaeze 2018-06-21 11:45:02 +02:00
parent 156dd833bd
commit 601078cdab
7 changed files with 159 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
Analysis/ground_motion_ol_cl.m
View File

@ -1,13 +1,24 @@
%%
clear; close all; clc;
%% Used to get Ts
run init_sim_configuration;
%% Used to get sim_conf.Ts
load('./mat/sim_conf.mat', 'sim_conf');
%% Load simulation results
gm_ol = load('./data/ground_motion_ol.mat', 'Dsample');
gm_cl = load('./data/ground_motion.mat', 'Dsample');
%%
figure;
hold on;
plot(gm_ol.Dsample.Data(:, 1),gm_ol.Dsample.Data(:, 3))
plot(gm_cl.Dsample.Data(:, 1),gm_cl.Dsample.Data(:, 3))
legend({'OL', 'CL'})
hold off;
xlabel('Displacement - $x$ [s]'); ylabel('Displacement - $z$ [m]');
exportFig('gm_control_xz', 'half-short')
%% Compare OL and CL - Time
figure;
hold on;
@ -41,12 +52,45 @@ xlabel('Time [s]'); ylabel('Displacement [m]');
exportFig('gm_control_time_z', 'normal-normal')
%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 4));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 4));
legend({'$\theta_x$ - OL', '$\theta_x$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');
exportFig('gm_control_time_rx', 'normal-normal')
%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 5));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 5));
legend({'$\theta_y$ - OL', '$\theta_y$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');
exportFig('gm_control_time_ry', 'normal-normal')
%% Compare OL and CL - Time
figure;
hold on;
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 6));
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 6));
legend({'$\theta_z$ - OL', '$\theta_z$ - CL'})
hold off;
xlabel('Time [s]'); ylabel('Angle [rad]');
exportFig('gm_control_time_rz', 'normal-normal')
%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_x_ol, freqs_x_ol] = pwelch(gm_ol.Dsample.Data(:, 1), han_windows_ol, 0, [], 1/Ts);
[psd_x_ol, freqs_x_ol] = pwelch(gm_ol.Dsample.Data(:, 1), han_windows_ol, 0, [], 1/sim_conf.Ts);
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_x, freqs_x] = pwelch(gm_cl.Dsample.Data(:, 1), han_windows, 0, [], 1/Ts);
[psd_x, freqs_x] = pwelch(gm_cl.Dsample.Data(:, 1), han_windows, 0, [], 1/sim_conf.Ts);
figure;
hold on;
@ -54,17 +98,17 @@ plot(freqs_x_ol, sqrt(psd_x_ol));
plot(freqs_x, sqrt(psd_x));
set(gca,'xscale','log'); set(gca,'yscale','log');
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
legend({'y - OL', 'y - CL'})
legend({'x - OL', 'x - CL'})
hold off;
exportFig('gm_control_psd_x', 'normal-normal')
%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dsample.Data(:, 2), han_windows_ol, 0, [], 1/Ts);
[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dsample.Data(:, 2), han_windows_ol, 0, [], 1/sim_conf.Ts);
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_y, freqs_y] = pwelch(gm_cl.Dsample.Data(:, 2), han_windows, 0, [], 1/Ts);
[psd_y, freqs_y] = pwelch(gm_cl.Dsample.Data(:, 2), han_windows, 0, [], 1/sim_conf.Ts);
figure;
hold on;
@ -87,10 +131,10 @@ dz_ol = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3), freqs, 'Hz')));
dz_cl = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3)*S(3, 3), freqs, 'Hz')));
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
[psd_z_ol, freqs_z_ol] = pwelch(gm_ol.Dsample.Data(:, 3), han_windows_ol, 0, [], 1/Ts);
[psd_z_ol, freqs_z_ol] = pwelch(gm_ol.Dsample.Data(:, 3), han_windows_ol, 0, [], 1/sim_conf.Ts);
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
[psd_z, freqs_z] = pwelch(gm_cl.Dsample.Data(:, 3), han_windows, 0, [], 1/Ts);
[psd_z, freqs_z] = pwelch(gm_cl.Dsample.Data(:, 3), han_windows, 0, [], 1/sim_conf.Ts);
figure;
hold on;

2
Analysis/tomography_experiment.m
View File

@ -62,4 +62,4 @@ exportFig('tomo_psd_rotations', 'normal-normal')
%%
save('../data/tomography_exp_ol.mat', 'Dmeas')
save('./data/tomography_exp_ol.mat', 'Dmeas')

40
Analysis/tomography_ol_cl.m
View File

@ -1,7 +1,22 @@
tomo_ol = load('../data/tomography_exp_ol.mat', 'Dmeas');
tomo_cl = load('../data/tomography_exp_001.mat', 'Dmeas');
%%
clear; close all; clc;
%%
tomo_ol = load('./data/tomography_exp_ol.mat', 'Dmeas');
tomo_cl = load('./data/tomography_exp.mat', 'Dmeas');
%% Compare OL and CL - Time
figure;
hold on;
plot(tomo_ol.Dmeas.Time, tomo_ol.Dmeas.Data(:, 1));
plot(tomo_cl.Dmeas.Time, tomo_cl.Dmeas.Data(:, 1));
legend({'x - OL', 'x - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
exportFig('tomo_control_time_x', 'normal-normal')
figure;
hold on;
plot(tomo_ol.Dmeas.Time, tomo_ol.Dmeas.Data(:, 2));
@ -13,6 +28,27 @@ xlabel('Time [s]'); ylabel('Displacement [m]');
exportFig('tomo_control_time_y', 'normal-normal')
figure;
hold on;
plot(tomo_ol.Dmeas.Time, tomo_ol.Dmeas.Data(:, 3));
plot(tomo_cl.Dmeas.Time, tomo_cl.Dmeas.Data(:, 3));
legend({'z - OL', 'z - CL'})
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
exportFig('tomo_control_time_z', 'normal-normal')
%%
figure;
hold on;
plot(tomo_ol.Dmeas.Data(:, 1),tomo_ol.Dmeas.Data(:, 3))
plot(tomo_cl.Dmeas.Data(:, 1),tomo_cl.Dmeas.Data(:, 3))
legend({'OL', 'CL'})
hold off;
xlabel('Displacement - $x$ [s]'); ylabel('Displacement - $z$ [m]');
%% Compare OL and CL - PSD
han_windows_ol = hanning(ceil(length(tomo_ol.Dmeas.Time)/10));
[psd_y_ol, freqs_y_ol] = pwelch(tomo_ol.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts);

6
Control/cl_tf.m
View File

@ -3,12 +3,12 @@ clear; close all; clc;
%% Load Plant
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
load('./mat/G_f_to_d.mat', 'G_f_to_d');
load('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50');
load('./mat/controller.mat', 'K');
%%
S = minreal(inv(tf(eye(6))+G_f_to_d*K));
T = minreal((tf(eye(6))+G_f_to_d*K)\G_f_to_d*K);
S = minreal(inv(tf(eye(6))+G_20*K));
T = minreal((tf(eye(6))+G_20*K)\G_20*K);
bodeFig({S(1,1), T(1,1)})
legend({'$S_x$', '$T_x$'})

21
Control/generate_control.m
View File

@ -8,6 +8,9 @@ load('./mat/G_f_to_d.mat', 'G_20');
load('./mat/control_K_tx.mat', 'K_tx');
load('./mat/control_K_ty.mat', 'K_ty');
load('./mat/control_K_tz.mat', 'K_tz');
load('./mat/control_K_rx.mat', 'K_rx');
load('./mat/control_K_ry.mat', 'K_ry');
load('./mat/control_K_rz.mat', 'K_rz');
%%
sisotool('bode', G_20(1, 1), K_tx);
@ -24,11 +27,29 @@ sisotool('bode', G_20(3, 3), K_tz);
K_tz = C;
save('./mat/control_K_tz.mat', 'K_tz');
%%
sisotool('bode', G_20(4, 4), K_rx);
K_rx = C;
save('./mat/control_K_rx.mat', 'K_rx');
%%
sisotool('bode', G_20(5, 5), K_ry);
K_ry = C;
save('./mat/control_K_ry.mat', 'K_ry');
%%
sisotool('bode', G_20(6, 6), K_rz);
K_rz = C;
save('./mat/control_K_rz.mat', 'K_rz');
%%
K = tf(zeros(6));
K(1,1) = K_tx;
K(2,2) = K_ty;
K(3,3) = K_tz;
K(4,4) = K_rx;
K(5,5) = K_ry;
K(6,6) = K_rz;
%% Save the MIMO control
save('./mat/controller.mat', 'K');

39
src/identifyGd.m Normal file
View File

@ -0,0 +1,39 @@
function [Gd, Gd_raw] = identifyGd(opts_param)
%% Default values for opts
opts = struct('cl', true, ... % CL or OL
'f_low', 0.1, ...
'f_high', 10000 ...
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
if opts.cl
mdl = 'Assemblage';
else
mdl = 'Micro_Station_Identification';
end
%% Centralized control (Cartesian coordinates)
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Gm'], 1,'input');
io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
% Run the linearization
Gd_raw = linearize(mdl,io, 0);
Gd_raw.InputName = {'Dgx', 'Dgy', 'Dgz'};
Gd_raw.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
Gd = preprocessIdTf(Gd_raw, opts.f_low, opts.f_high);
Gd.InputName = {'Dgx', 'Dgy', 'Dgz'};
Gd.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
end

4
test_control.m Normal file
View File

@ -0,0 +1,4 @@
%% Open Loop simulation and save the final state
initializeSimConf(struct('Tsim', 2, 'cl_time', 0));
sim('Assemblage')