% Experimentaldaten-basierte Modellierung des Dreifachpendels // Experimental data-based modeling of the triple pendulum 
% Max-Planck-Institut Magdeburg 
% Kevin Jakisch in co-operation with Jun. Prof. Jan Heiland

%% DMDc für einen Versuch // DMDc for one experiment
clear
clc

%% Setup
sampletime = 1/150; %Schrittweite h = delta_t // Step size
Versuch = 16; %Auswahl des Versuches für Plots // Selection of the experiment
length_V = 190000; %Versuchslänge (mind. 38000, max. 191000) // Experiment length
time_train = 2; %Länge der Zeit [s] in der A und B trainiert werden sollen 
                %DMDc-Anlernzeit // DMDc training time

%% Einlesen der Messdaten // Reading in the measurement data
[phi1, phi2, phi3, s, time] = Auswertung_Messdaten(Versuch,sampletime,length_V);
    
%% Konstruieren der Zustände für X, X'=Z und Y // Construct the states X, X'=Z und Y
phi_all = [phi1; phi2; phi3];               %Winkelkoordinaten / Drehwinkel // Rotation angle
phi_dot = gradient(phi_all)/sampletime;     %Ableitungen der Drehwinkel // Derivations
phi_sqr = phi_all.^2;                       %Quadrate der Drehwinkel // Squares
phi_dot_sqr = phi_dot.^2;                   %(Ableitungen der Drehwinkel)^2 // (Derivations)^2
phi_crs = phi_all .* phi_dot;               %Drehwinkel * Ableitungen // Rotation angle * derivations
phi_crs_sqr = phi_crs.^2;                   %(Drehwinkel * Ableitungen)^2 // (Rotation angle * derivations)^2 

s_dot = gradient(s)/sampletime;             %Geschwindigkeit des Pendelwagens // Velocity of the shuttle
s_dot_dot = gradient(s_dot)/sampletime;     %Beschleunigung des Pendelwagens // Acceleration of the shuttle
s_sqr = s.^2;                               %s^2

%% Konstruieren der kompletten Datenmatrix X & zeitversetzten Datenmatrix Z
% Construct the complete data matrix X & time-shifted data matrix Z
% 6 Zustandskombinationen // 6 combinations of states
X{1} = [phi_all(:,1:end-1)];
Z{1} = [phi_all(:,2:end)];
X{2} = [phi_all(:,1:end-1); phi_dot(:,1:end-1)];
Z{2} = [phi_all(:,2:end); phi_dot(:,2:end)]; 
X{3} = [phi_all(:,1:end-1); phi_dot(:,1:end-1); phi_sqr(:,1:end-1);]; 
Z{3} = [phi_all(:,2:end); phi_dot(:,2:end); phi_sqr(:,2:end)];
X{4} = [phi_all(:,1:end-1); phi_dot(:,1:end-1); phi_sqr(:,1:end-1); phi_dot_sqr(:,1:end-1);];
Z{4} = [phi_all(:,2:end); phi_dot(:,2:end); phi_sqr(:,2:end); phi_dot_sqr(:,2:end)];    
X{5} = [phi_all(:,1:end-1); phi_dot(:,1:end-1); phi_sqr(:,1:end-1); phi_dot_sqr(:,1:end-1);...
        phi_crs(:,1:end-1)];
Z{5} = [phi_all(:,2:end); phi_dot(:,2:end); phi_sqr(:,2:end); phi_dot_sqr(:,2:end);...
        phi_crs(:,2:end)];
X{6} = [phi_all(:,1:end-1); phi_dot(:,1:end-1); phi_sqr(:,1:end-1); phi_dot_sqr(:,1:end-1);...
        phi_crs(:,1:end-1); phi_crs_sqr(:,1:end-1)];
Z{6} = [phi_all(:,2:end); phi_dot(:,2:end); phi_sqr(:,2:end); phi_dot_sqr(:,2:end);...
        phi_crs(:,2:end); phi_crs_sqr(:,2:end)];

%% Konstruieren der kompletten Steuerungsmatrix Y // Construct the complete control matrix Y
% 3 Zustandskombinationen // 3 combinations of states
Y{1} = [s(1:end-1)];
Y{2} = [s(1:end-1); s_dot_dot(1:end-1)];
Y{3} = [s(1:end-1); s_dot_dot(1:end-1); s_sqr(1:end-1)];

%% Konstruieren der  Datenmatrix X & zeitversetzten Datenmatrix Z für ein best. Zeitinterval
% Construct the data matrix X & time-shifted data matrix Z for a specific time interval
int_time_train = length(time(1):sampletime:time_train);
length_S = int_time_train-1; %Zeitvektor für die DMDc-Anlernzeit // Time vector for DMDc training time

X_train{1} = [phi_all(:,1:(int_time_train-1))];
Z_train{1} = [phi_all(:,(2:int_time_train))];

X_train{2} = [phi_all(:,1:(int_time_train-1)); phi_dot(:,1:(int_time_train-1))];
Z_train{2} = [phi_all(:,(2:int_time_train)); phi_dot(:,(2:int_time_train))]; 

X_train{3} = [phi_all(:,1:(int_time_train-1)); phi_dot(:,1:(int_time_train-1));...
              phi_sqr(:,1:1:(int_time_train-1))];
Z_train{3} = [phi_all(:,(2:int_time_train)); phi_dot(:,(2:int_time_train));...
              phi_sqr(:,(2:int_time_train))];

X_train{4} = [phi_all(:,1:(int_time_train-1)); phi_dot(:,1:(int_time_train-1));...
              phi_sqr(:,1:1:(int_time_train-1)); phi_dot_sqr(:,1:1:(int_time_train-1))];
Z_train{4} = [phi_all(:,(2:int_time_train)); phi_dot(:,(2:int_time_train));...
              phi_sqr(:,(2:int_time_train)); phi_dot_sqr(:,(2:int_time_train))];

X_train{5} = [phi_all(:,1:(int_time_train-1)); phi_dot(:,1:(int_time_train-1));...
              phi_sqr(:,1:1:(int_time_train-1)); phi_dot_sqr(:,1:1:(int_time_train-1));...
              phi_crs(:,1:1:(int_time_train-1))];
Z_train{5} = [phi_all(:,(2:int_time_train)); phi_dot(:,(2:int_time_train));...
              phi_sqr(:,(2:int_time_train)); phi_dot_sqr(:,(2:int_time_train));...
              phi_crs(:,(2:int_time_train))];

X_train{6} = [phi_all(:,1:(int_time_train-1)); phi_dot(:,1:(int_time_train-1));...
              phi_sqr(:,1:1:(int_time_train-1)); phi_dot_sqr(:,1:1:(int_time_train-1));...
              phi_crs(:,1:1:(int_time_train-1)); phi_crs_sqr(:,1:1:(int_time_train-1))];
Z_train{6} = [phi_all(:,(2:int_time_train)); phi_dot(:,(2:int_time_train));...
              phi_sqr(:,(2:int_time_train)); phi_dot_sqr(:,(2:int_time_train));...
              phi_crs(:,(2:int_time_train)); phi_crs_sqr(:,(2:int_time_train))];

%% Konstruieren der Steuerungsmatrix Y für ein best. Zeitintervall       
% Construct the control matrix Y for a specific time interval
Y_train{1} = s(1:(int_time_train-1));

Y_train{2} = [s(1:(int_time_train-1)); s_dot_dot(1:(int_time_train-1))];

Y_train{3} = [s(1:(int_time_train-1)); s_dot_dot(1:(int_time_train-1));...
               s_sqr(1:(int_time_train-1))];

%% Zusammensetzen der Eingabematrix Omega // Composing the input matrix Omega
for i = 1:6                                %6 Zustandskombinationen für X, Z // 6 state combinations for X, Z
    for k = 1:3                            %3 Zustandskombinationen für Y // 3 state combinations for Y
        Omega{i,k} = [X_train{i}; Y_train{k}];     %Omega = [X, Y]^T

%% Anwenden des Algorithmus // Apply the algorithm
[U_t,S_t,V_t] = svd(Omega{i,k}, "econ");        %SVD / Singulärwertzerlegung für Omega // singular value decomposition for omega
[U_d,S_d,V_d] = svd(Z_train{i}, "econ");        %SVD / Singulärwertzerlegung für X'=Z // singular value decomposition for Z
U_tilde{i,k} = U_t;                             %Speichern von U, S, V aus der SVD // Saving U, S, V from the SVD
S_tilde{i,k} = S_t;
V_tilde{i,k} = V_t;
U_dach{i} = U_d;
S_dach{i} = S_d;
V_dach{i} = V_d;

U1_tilde{i,k} = U_tilde{i,k}(1:length(X_train{i}(:,1)),:);         %Aufsplitten von U_tilde auf U1_tilde & U2_tilde // Split U_tilde
U2_tilde{i,k} = U_tilde{i,k}(length(X_train{i}(:,1))+1:end,:);     

A_tilde{i,k} = Z_train{i} * V_tilde{i,k} * S_tilde{i,k}^(-1) * U1_tilde{i,k}';    %Berechnungsformel für Operator A // Calculation formula for A
B_tilde{i,k} = Z_train{i} * V_tilde{i,k} * S_tilde{i,k}^(-1) * U2_tilde{i,k}';    %Berechnungsformel für Operator B // Calculation formula for B

%% DMDc-Zustandsraummodell Z=AX+BY // DMDc state space model
Z_mod{i,k} = A_tilde{i,k} * X{i} + B_tilde{i,k} * Y{k};   

%% Zustandsschätzung // State estimation 

Z_prediction{i,k}(:,1:length_S) = Z{i}(:,1:length_S);   % Zustandsschätzung = Messung für DMDc-Anlernzeit 
                                                        % state estimation = measurement for DMDc training time
for j=(length_S+1):length(Y{k})
   Z_prediction{i,k}(:,j) = (A_tilde{i,k}*Z_prediction{i,k}(:,j-1)+B_tilde{i,k}*Y{k}(:,j-1));
end
end
end

%% Plot: Phi1/Phi2/Phi3-Messungen vs. DMDc-Zustandsschätzung // Plot: Measurements vs. DMDc state estimation
figure  %Phi 1
for i=1:6
% subplot(2,2,1)
plot(time(1:end-1),Z_prediction{i,1}(1,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1:end-1),Z{1}(1,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3)', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',12)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_1 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30); 

figure  %Phi 2
for i=1:6
% subplot(2,2,2)
plot(time(1:end-1),Z_prediction{i,1}(2,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1:end-1),Z{1}(2,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3)', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',12)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_2 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30); 

figure  %Phi 3
for i=1:6
% subplot(2,2,3)
plot(time(1:end-1),Z_prediction{i,1}(3,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1:end-1),Z{1}(3,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3)', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',12)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_3 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30);

figure  %s
% subplot(2,2,4)
plot(time(1:end-1),Y{1}(1,1:end),'LineWidth',2)
xlabel('Zeit [s]','fontsize',30); ylabel('Position s [m]','fontsize',30); grid on;
xlim([0 6.0])
set(gca,'Fontsize',30);

%% Plot: Phi1/Phi2/Phi3-Messungen vs. Zustandsraummodell // Plot: Measurements vs.state space model
figure  %Phi 1
for i=1:6
plot(time(1,1:end-1),Z_mod{i,2}(1,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1,1:end-1),Z{1}(1,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',14)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_1 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30); 

figure  %Phi 2
for i=1:6
plot(time(1,1:end-1),Z_mod{i,2}(2,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1,1:end-1),Z{1}(2,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3)', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',14)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_2 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30); 

figure  %Phi 3
for i=1:6
plot(time(1,1:end-1),Z_mod{i,2}(3,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time(1,1:end-1),Z{1}(3,1:end),'LineWidth',2)
legend('DMDc (Z_i i=1,...,3)', 'DMDc (Z_i i=1,...,6)','DMDc (Z_i i=1,...,9)', 'DMDc (Z_i i=1,...,12)', 'DMDc (Z_i i=1,...,15)',...
    'DMDc (Z_i i=1,...,18)', 'Messdaten', 'location', 'best','fontsize',14)
xlabel('Zeit [s]','fontsize',30); ylabel('\phi_3 [°]','fontsize',30); grid on;
ylim([-40 40])
xlim([0 6.0])
set(gca,'Fontsize',30); 