% 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 alle Versuche (bis auf einen*) // DMDc for all trajectories (except for one*)
% *auf diesen wird DMD angewandt // *DMD is applied to these 
clear
clc

%% Setup
nv = 23; %Anzahl der Experimente // number of experiments 
sampletime = 1/150; %Schrittweite h = delta_t // Step size
Versuch = 16; %Auswahl des Versuches* // 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
for i = 1:nv  %Schleife für alle durchgeführten Experimente // %loop for all experiments 
    [phi1{i},phi2{i},phi3{i},s_s{i},time{i}] = Auswertung_Messdaten(i,sampletime,length_V);
end
for i = 1:(Versuch-1)
     phi_all{i} = [phi1{i}; phi2{i}; phi3{i}];      %Winkelkoordinaten / Drehwinkel // Rotation angle
     phi_dot{i} = gradient(phi_all{i})/sampletime;  %Ableitungen der Drehwinkel // Derivations
     phi_sqr{i} = phi_all{i}.^2;                    %Quadrate der Drehwinkel // Squares
     phi_dot_sqr{i} = phi_dot{i}.^2;                %(Ableitungen der Drehwinkel)^2 // (Derivations)^2
     phi_crs{i} = phi_all{i} .* phi_dot{i};         %Drehwinkel * Ableitungen // Rotation angle * derivations
     phi_crs_sqr{i} = phi_crs{i}.^2;                %(Drehwinkel * Ableitungen)^2 // (Rotation angle * derivations)^2 

     s{i} = s_s{i};                                 %Position des Pendelwagens // Position of the shuttle
     s_dot{i} = gradient(s{i})/sampletime;          %Geschwindigkeit des Pendelwagens // Velocity of the shuttle
     s_dot_dot{i} = gradient(s_dot{i})/sampletime;  %Beschleunigung des Pendelwagens // Acceleration of the shuttle
     s_sqr{i} = s{i}.^2;                            %s^2
end
for i = Versuch:(nv-1)
    phi_all{i} = [phi1{i+1}; phi2{i+1}; phi3{i+1}];
    phi_dot{i} = gradient(phi_all{i})/sampletime;
    phi_sqr{i} = phi_all{i}.^2;
    phi_dot_sqr{i} = phi_dot{i}.^2;
    phi_crs{i} = phi_all{i} .* phi_dot{i};
    phi_crs_sqr{i} = phi_crs{i}.^2;
    s{i} = s_s{i+1};
    s_dot{i} = gradient(s{i})/sampletime;
    s_dot_dot{i} = gradient(s_dot{i})/sampletime;
    s_sqr{i} = s{i}.^2;
end

%% Konstruieren der Vektoren für die Datenmatrix X, die zeitversetzte Datenmatrix X'=Z und die Steuerungsmatrix Y
%  Construct the vectors for the data matrix X, the time-shifted data matrix X'=Z, and the control matrix Y.
tmp2 = 0;
for i = 1:(Versuch-1)
    tmp1 = length(phi1{i}(1:end-1));
    PHI_ALL(:,(tmp2+1):(tmp2+tmp1)) = phi_all{i}(:,1:end-1);
    PHI_DOT(:,(tmp2+1):(tmp2+tmp1)) = phi_dot{i}(:,1:end-1);
    PHI_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_sqr{i}(:,1:end-1);
    PHI_DOT_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_dot_sqr{i}(:,1:end-1);
    PHI_CRS(:,(tmp2+1):(tmp2+tmp1)) = phi_crs{i}(:,1:end-1);
    PHI_CRS_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_crs_sqr{i}(:,1:end-1);
    
    S(:,(tmp2+1):(tmp2+tmp1)) = (s{i}(:,1:end-1));
    S_DOT(:,(tmp2+1):(tmp2+tmp1)) = (s_dot{i}(:,1:end-1));
    S_DOT_DOT(:,(tmp2+1):(tmp2+tmp1)) = (s_dot_dot{i}(:,1:end-1));
    S_SQR(:,(tmp2+1):(tmp2+tmp1)) = (s_sqr{i}(:,1:end-1));
    
    tmp2 = length(PHI_ALL);
end
for i = Versuch:(nv-1)
    tmp1 = length(phi1{i+1}(1:end-1));
    PHI_ALL(:,(tmp2+1):(tmp2+tmp1)) = phi_all{i}(:,1:end-1);
    PHI_DOT(:,(tmp2+1):(tmp2+tmp1)) = phi_dot{i}(:,1:end-1);
    PHI_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_sqr{i}(:,1:end-1);
    PHI_DOT_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_dot_sqr{i}(:,1:end-1);
    PHI_CRS(:,(tmp2+1):(tmp2+tmp1)) = phi_crs{i}(:,1:end-1);
    PHI_CRS_SQR(:,(tmp2+1):(tmp2+tmp1)) = phi_crs_sqr{i}(:,1:end-1);
    
    S(:,(tmp2+1):(tmp2+tmp1)) = (s{i}(:,1:end-1));
    S_DOT(:,(tmp2+1):(tmp2+tmp1)) = (s_dot{i}(:,1:end-1));
    S_DOT_DOT(:,(tmp2+1):(tmp2+tmp1)) = (s_dot_dot{i}(:,1:end-1));
    S_SQR(:,(tmp2+1):(tmp2+tmp1)) = (s_sqr{i}(:,1:end-1));
    
    tmp2 = length(PHI_ALL);
end

tmp2 = 0;
for i = 1:(Versuch-1)
    tmp1 = length(phi1{i}(2:end));
    PHI_ALL_2(:,(tmp2+1):(tmp2+tmp1)) = phi_all{i}(:,2:end);
    PHI_DOT_2(:,(tmp2+1):(tmp2+tmp1)) = phi_dot{i}(:,2:end);
    PHI_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_sqr{i}(:,2:end);
    PHI_DOT_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_dot_sqr{i}(:,2:end);
    PHI_CRS_2(:,(tmp2+1):(tmp2+tmp1)) = phi_crs{i}(:,2:end);
    PHI_CRS_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_crs_sqr{i}(:,2:end);
    tmp2 = length(PHI_ALL_2);
end

for i = Versuch:(nv-1)
    tmp1 = length(phi1{i+1}(2:end));
    PHI_ALL_2(:,(tmp2+1):(tmp2+tmp1)) = phi_all{i}(:,2:end);
    PHI_DOT_2(:,(tmp2+1):(tmp2+tmp1)) = phi_dot{i}(:,2:end);
    PHI_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_sqr{i}(:,2:end);
    PHI_DOT_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_dot_sqr{i}(:,2:end);
    PHI_CRS_2(:,(tmp2+1):(tmp2+tmp1)) = phi_crs{i}(:,2:end);
    PHI_CRS_SQR_2(:,(tmp2+1):(tmp2+tmp1)) = phi_crs_sqr{i}(:,2:end);
    tmp2 = length(PHI_ALL_2);
end

% 6 Zustandskombinationen für X, Z // 6 combinations of states for X, Z
X{1} = [PHI_ALL]; 
Z{1} = [PHI_ALL_2];
X{2} = [PHI_ALL; PHI_DOT];
Z{2} = [PHI_ALL_2; PHI_DOT_2];
X{3} = [PHI_ALL; PHI_DOT; PHI_SQR]; 
Z{3} = [PHI_ALL_2; PHI_DOT_2; PHI_SQR_2]; 
X{4} = [PHI_ALL; PHI_DOT; PHI_SQR; PHI_DOT_SQR];
Z{4} = [PHI_ALL_2; PHI_DOT_2; PHI_SQR_2; PHI_DOT_SQR_2];
X{5} = [PHI_ALL; PHI_DOT; PHI_SQR; PHI_DOT_SQR; PHI_CRS];
Z{5} = [PHI_ALL_2; PHI_DOT_2; PHI_SQR_2; PHI_DOT_SQR_2; PHI_CRS_2];
X{6} = [PHI_ALL; PHI_DOT; PHI_SQR; PHI_DOT_SQR; PHI_CRS; PHI_CRS_SQR];
Z{6} = [PHI_ALL_2; PHI_DOT_2; PHI_SQR_2; PHI_DOT_SQR_2; PHI_CRS_2; PHI_CRS_SQR_2];

% 3 Zustandskombinationen für Y // 3 combinations of states for Y
Y{1} = [S];
Y{2} = [S; S_DOT_DOT];
Y{3} = [S; S_DOT_DOT; S_SQR];

%% 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{i}; Y{k}]; 

%% 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{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{i}(:,1)),:);       %Aufsplitten von U_tilde auf U1_tilde & U2_tilde // Split U_tilde
U2_tilde{i,k} = U_tilde{i,k}(length(X{i}(:,1))+1:end,:);

A_tilde{i,k} = Z{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{i} * V_tilde{i,k} * S_tilde{i,k}^(-1) * U2_tilde{i,k}';    %Berechnungsformel für Operator B // Calculation formula for B

%% Konstruieren der Vektoren für X, X'=Z und Y für die ausgewählte Trajektorie*
%  Construct the vectors for X, X'=Z and Y for the chosen trajectory*
phi_all_V = [phi1{Versuch}; phi2{Versuch}; phi3{Versuch}];
phi_dot_V = gradient(phi_all_V)/sampletime;
phi_sqr_V = phi_all_V.^2;
phi_dot_sqr_V = phi_dot_V.^2;
phi_crs_V = phi_all_V .* phi_dot_V;
phi_crs_sqr_V = phi_crs_V.^2;
s_V = s_s{Versuch};
s_dot_V = gradient(s_V)/sampletime;
s_dot_dot_V = gradient(s_dot_V)/sampletime;
s_sqr_V = s_V.^2;

X_V{1} = [phi_all_V(:,1:end-1)];
Z_V{1} = [phi_all_V(:,2:end)];
X_V{2} = [phi_all_V(:,1:end-1); phi_dot_V(:,1:end-1)];
Z_V{2} = [phi_all_V(:,2:end); phi_dot_V(:,2:end)]; 
X_V{3} = [phi_all_V(:,1:end-1); phi_dot_V(:,1:end-1); phi_sqr_V(:,1:end-1)];
Z_V{3} = [phi_all_V(:,2:end); phi_dot_V(:,2:end); phi_sqr_V(:,2:end)];
X_V{4} = [phi_all_V(:,1:end-1); phi_dot_V(:,1:end-1); phi_sqr_V(:,1:end-1);...
       phi_dot_sqr_V(:,1:end-1)];
Z_V{4} = [phi_all_V(:,2:end); phi_dot_V(:,2:end); phi_sqr_V(:,2:end);... 
       phi_dot_sqr_V(:,2:end)];
X_V{5} = [phi_all_V(:,1:end-1); phi_dot_V(:,1:end-1); phi_sqr_V(:,1:end-1);...
       phi_dot_sqr_V(:,1:end-1); phi_crs_V(:,1:end-1)];
Z_V{5} = [phi_all_V(:,2:end); phi_dot_V(:,2:end); phi_sqr_V(:,2:end);... 
       phi_dot_sqr_V(:,2:end); phi_crs_V(:,2:end)];
X_V{6} = [phi_all_V(:,1:end-1); phi_dot_V(:,1:end-1); phi_sqr_V(:,1:end-1);...
       phi_dot_sqr_V(:,1:end-1); phi_crs_V(:,1:end-1); phi_crs_sqr_V(:,1:end-1)];
Z_V{6} = [phi_all_V(:,2:end); phi_dot_V(:,2:end); phi_sqr_V(:,2:end);... 
       phi_dot_sqr_V(:,2:end); phi_crs_V(:,2:end); phi_crs_sqr_V(:,2:end)];

Y_V{1} = [s_V(1,1:end-1)];
Y_V{2} = [s_V(1,1:end-1); s_dot_dot_V(1,1:end-1)]; 
Y_V{3} = [s_V(1,1:end-1); s_dot_dot_V(1,1:end-1); s_sqr_V(1,1:end-1)];


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

%% Zustandsschätzung // State estimation 
%  x_k+1 = A*x_k + B*u_k
int_time_train = length(time{Versuch}(1):sampletime:time_train);
length_S = int_time_train-1; %Zeitvektor für die DMDc-Anlernzeit // Time vector for DMDc training time

Z_prediction{i,k}(:,1:length_S) = Z_V{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_V{k})
   Z_prediction{i,k}(:,j) = (A_tilde{i,k}*Z_prediction{i,k}(:,j-1)+B_tilde{i,k}*Y_V{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,1,1)
plot(time{Versuch}(1,1:end-1),Z_prediction{i,2}(1,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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
% subplot(2,1,2)
plot(time{Versuch}(1,1:end-1),Z_prediction{i,2}(2,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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{Versuch}(1,1:end-1),Z_prediction{i,1}(3,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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',12); 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{Versuch}(1,1:end-1),Y_V{1}(1,:),'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{Versuch}(1,1:end-1),Z_mod{i,2}(1,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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{Versuch}(1,1:end-1),Z_mod{i,2}(2,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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{Versuch}(1,1:end-1),Z_mod{i,2}(3,1:end),'LineWidth',2)
hold on 
end
hold on 
plot(time{Versuch}(1,1:end-1),Z_V{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); 

