From 601078cdab82cca64a4bcf0fc7d052f9537e4786 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Thu, 21 Jun 2018 11:45:02 +0200 Subject: [PATCH] Change the way control is generated. Add script to test control --- Analysis/ground_motion_ol_cl.m | 62 +++++++++++++++++++++++++++----- Analysis/tomography_experiment.m | 2 +- Analysis/tomography_ol_cl.m | 40 +++++++++++++++++++-- Control/cl_tf.m | 6 ++-- Control/generate_control.m | 21 +++++++++++ src/identifyGd.m | 39 ++++++++++++++++++++ test_control.m | 4 +++ 7 files changed, 159 insertions(+), 15 deletions(-) create mode 100644 src/identifyGd.m create mode 100644 test_control.m diff --git a/Analysis/ground_motion_ol_cl.m b/Analysis/ground_motion_ol_cl.m index 44a5f3b..2dd51cc 100644 --- a/Analysis/ground_motion_ol_cl.m +++ b/Analysis/ground_motion_ol_cl.m @@ -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; diff --git a/Analysis/tomography_experiment.m b/Analysis/tomography_experiment.m index 010afeb..6f4c730 100644 --- a/Analysis/tomography_experiment.m +++ b/Analysis/tomography_experiment.m @@ -62,4 +62,4 @@ exportFig('tomo_psd_rotations', 'normal-normal') %% -save('../data/tomography_exp_ol.mat', 'Dmeas') \ No newline at end of file +save('./data/tomography_exp_ol.mat', 'Dmeas') \ No newline at end of file diff --git a/Analysis/tomography_ol_cl.m b/Analysis/tomography_ol_cl.m index ad53667..2beab8c 100644 --- a/Analysis/tomography_ol_cl.m +++ b/Analysis/tomography_ol_cl.m @@ -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); diff --git a/Control/cl_tf.m b/Control/cl_tf.m index 1c22f66..8cc6696 100644 --- a/Control/cl_tf.m +++ b/Control/cl_tf.m @@ -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$'}) diff --git a/Control/generate_control.m b/Control/generate_control.m index 9b7d0a8..6685a1b 100644 --- a/Control/generate_control.m +++ b/Control/generate_control.m @@ -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'); diff --git a/src/identifyGd.m b/src/identifyGd.m new file mode 100644 index 0000000..0745d6a --- /dev/null +++ b/src/identifyGd.m @@ -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 diff --git a/test_control.m b/test_control.m new file mode 100644 index 0000000..3eb394d --- /dev/null +++ b/test_control.m @@ -0,0 +1,4 @@ +%% Open Loop simulation and save the final state +initializeSimConf(struct('Tsim', 2, 'cl_time', 0)); + +sim('Assemblage') \ No newline at end of file