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