% Aufgabe 3.1
% GSM mit Propeller
clc;
clear;
close all;
%% Aufgabe 3.1.1

% altes File laden
run('GSM_Parameter.m');

Ac = Aredl;
bc = Bredl(:, 1);
cc = [0 0 1];
dc = 0;
sysc = ss(Ac, bc, cc, dc);
sysd = c2d(sysc, Ta, 'zoh');

assert((rank(ctrb(sysd)) == 3))
% == 3 => kein Rangverlust

lambda0 = -4.5;
lambda = [lambda0 lambda0 lambda0];

poles = exp(lambda * Ta)

[Ad, bd, cd, dd] = ssdata(sysd);
k = -acker(Ad, bd, poles);
g = 1/((cc+dd*k)*inv(eye(3)-Ad-bd*k)*bd+dd)

% Test
eig(Ad+bd*k) % == exp(lambda0*Ta)
log(eig(Ad+bd*k))/Ta % == lambda0

% struct to pass to matlab function in simulink model
parZus.k = k;
parZus.g = g;

%% Aufgabe 3.1.2
% Beim nichtlinearen Modell haben wir einen zusätzlichen Zustand (den wir
% beim reduzierten Modell eliminiert haben), welcher sich aus unserer
% Perspektive wie ein Fehler auf das System auswirkt. Dadurch können wir
% aber auch den Fehler nicht ausregeln. (ohne Integrierer)

%% Aufgabe 3.1.3
% Überprüfung
eig(Ad)
zpk(tf(sysd))

% angepasste Matrizen
AdI = [Ad, [0,0,0]'; -cc, 1];
bdI = [bd; 0];

% Pole gleich wählen, um 1 erweitern durch Integrator Zustand
lambdaI = [lambda0 lambda0 lambda0 lambda0];
polesI = exp(lambdaI*Ta)

% 8.51b
% u_k = [ k_^T  k_I ] * [ x_k; x_I,k ] + k_P * (r_k - c^T * x_k)
% Koeffizienten berechnen
ke = -acker(AdI, bdI, polesI);
kI = ke(4)
kP = 1/(cc*inv(eye(3)-Ad)*bd)
kx = kP*cc+ke(1:3)

% Test
eig(AdI+bdI*ke) % == exp(lambdaI*Ta)
log(eig(AdI+bdI*ke))/Ta % == lambdaI

parZR.x0 = [0 0 0];
parZR.xI0 = 0;
parZR.kx = kx;
parZR.kI = kI;
parZR.kP = kP;
parZR.cd = cd;

%% Aufgabe 3.1.4

load('GSM_2_5.mat', 'Rz', 'Rkomp', 'R', 'parGSM')

% Rz = (Vp + VI/(z - 1)) * Rkomp
RPI = minreal(R/Rkomp, 1e-7);
RzPI = c2d(RPI, Ta, 'tustin')
[Vi, one, Vp] = residue(RzPI.num{1}, RzPI.den{1})

% Ergebnis prüfen!
% Da nach der Zerlegung derselbe Regler rauskommen muss, können wir unser
% Ergebnis prüfen indem wir prüfen ob er ungefähr gleich dem ursprünglichen
% Regler im z-Bereich ist.
comp = minreal((Vp+Vi/(tf('z')-1))*c2d(Rkomp, Ta, 'tustin')/Rz, 1e-4);
assert(comp.num{1} == comp.den{1})