%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Systemidentifiktaion einfacher Modelle
% M.Geissmann 14.3.2025
% Matlab 2023b
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear; clc; close all;

%% Daten einlesen
Data = readtable("Schrittversuch.CSV");

% Zeit
Time = Data.Time;
Ts = Time(2)-Time(1); % Abtastzeit in [s]
% System Anregung
u = Data.u;
% System Antwort
y = Data.y;


figure
yyaxis left
plot(Time, u)
ylabel('Anregung u')
yyaxis right
plot(Time, y)
ylabel('Systemantwort y')
xlabel('Zeit [s]')

%% Preprocessing

% Inizialbedingungen
y_init = y(1);
y_fit = y-y_init;

u_init = 0;
u_fit = u-u_init;

%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % PT1
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %        Kp
% % G = --------------
% %      Tau*s + 1
yk = [];
X = [];
for k = 2:length(y_fit)
    yk = [yk; y_fit(k)];
    X = [X; -y_fit(k-1), u_fit(k-1)];
end
Theta = X\yk; % Kleinste Fehlerquadrate mit \
Kp = Theta(2)/(Theta(1)+1);
Tau = Ts/(Theta(1)+1);
PT1 = tf(Kp,[Tau,1])

% System Simulation
[y_sim, tout] = lsim(PT1,u,Time);
y_sim = y_sim+y_init;

figure 
hold on
plot(Time,y,'k:','LineWidth',2)
plot(tout,y_sim,'r-')
xlabel('Zeit [s]')
legend('Messung','Simulation')

%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % PT2
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %        Kp       
% % G = --------------
% %      As^2 + Bs + 1
yk = [];
X = [];
for k = 3:length(y_fit)
    yk = [yk; y_fit(k)];
    X = [X; -y_fit(k-1), -y_fit(k-2), u_fit(k-2)];
end
Theta = X\yk; % Kleinste Fehlerquadrate mit \
K = Theta(3)/(Theta(1) + Theta(2) + 1);
A = Ts^2/(Theta(1) + Theta(2) + 1);
B = (Ts*(Theta(1) + 2))/(Theta(1) + Theta(2) + 1);

PT2 = tf([K],[A B 1])

% System Simulation
[y_sim, tout] = lsim(PT2,u,Time);
y_sim = y_sim+y_init;

figure 
hold on
plot(Time,y,'k:','LineWidth',2)
plot(tout,y_sim,'r-')
xlabel('Zeit [s]')
legend('Messung','Simulation')


%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ARX
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     nB #Zeros --> (min 1)
% G = ---------
%     nA #Poles --> (>nB)
nB = 1;
nA = 2;
if nA<nB
    error('Error: nA muss grösser gleich nB sein.');
end
Y = y_fit; U = u_fit;
for i = 1:nA
    Y = [ Y, [Y(1,end); Y(1:end-1,end)] ];
    U = [ U, [U(1,end); U(1:end-1,end)] ];
end
X_fit = [-Y(:,2:end), U(:,2:nB+1)];
X_fit = X_fit(nA+1:end,:);

Theta = X_fit \ Y(nA+1:end,1); % Least Square mit \
Num = [0;Theta(nA+1:end)]'; % Kp
Den = [1;Theta(1:nA)]'; % Zeitkonstanten

Arx_dis = tf(Num, Den,Ts,'Variable','z^-1'); % Zeitdiskret
% Sys_cont = d2c(Sys_dis,'zoh') % Zeitkontinuierlich
Arx_cont = d2c(Arx_dis,'tustin') % Zeitkontinuierlich

% System Simulation
[y_sim, tout] = lsim(Arx_cont,u,Time);
y_sim = y_sim+y_init;

figure 
hold on
plot(Time,y,'k:','LineWidth',2)
plot(tout,y_sim,'r-')
xlabel('Zeit [s]')
legend('Messung','Simulation')






