% Aufgabe 2.2
% GSM mit Propeller
clc;
clear;
close all;
%% 2.2.2
% Subskript Notation: GSM Subskript substituiert mit G
% Konstante aus Tabelle 2.1
LG  = 1.4e-3; % H
RG  = .46; % Ohm
kG  = .1; % N m A^-1
JG  = 12.4e-3; % kg m^2
dcG = .152; % N m
dvG = 1.8e-3; % N m s rad^-1
JP  = 32.5e-3; % kg m^2
dcP = .169; % N m
dvP = 2.7e-3; % N m s rad^-1
dqP = 1e-4; % N m s^2 rad^-2
cGP = .6822; % N m rad^-1
dGP = 1e-5; % N m s rad^-1

% Ruhelagenbedingungen (Abgabe)
Mext = 0;
uG = 5.6;
% Ruhelagengleichungen (erste Lösung aus Maple)
iGR1 = (-kG*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (2*uG*dqP + kG*(dvG + dvP))*RG + kG^3)/(2*RG^2*dqP);
pGPR1 = ((-RG*dvG - kG^2)*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-2*dcG*dqP + dvG^2 + dvG*dvP)*RG^2 + ((2*dvG + dvP)*kG^2 + 2*uG*dqP*kG)*RG + kG^4)/(2*RG^2*dqP*cGP);
wGR1 = (sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
wPR1 = (sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
% Ruhelagengleichungen (zweite Lösung aus Maple)
iGR2 = (kG*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (2*uG*dqP + kG*(dvG + dvP))*RG + kG^3)/(2*RG^2*dqP);
pGPR2 = ((-RG*dvG - kG^2)*-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-2*dcG*dqP + dvG^2 + dvG*dvP)*RG^2 + ((2*dvG + dvP)*kG^2 + 2*uG*dqP*kG)*RG + kG^4)/(2*RG^2*dqP*cGP);
wGR2 = (-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
wPR2 = (-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);

% x = [iG, phiG, wG, wP]
% erste Ruhelage aus Maple
xR1 = [iGR1; pGPR1; wGR1; wPR1]
% zweite Ruhelage aus Maple
xR2 = [iGR2; pGPR2; wGR2; wPR2]

iGR = iGR1;
pGPR = pGPR1;
wGR = wGR1;
wPR = wPR1;

Al = [
    -RG/LG,      0,          -kG/LG,                           0;
    0,           0,               1,                          -1;
    kG/JG, -cGP/JG, (-dvG - dGP)/JG,                      dGP/JG;
    0,      cGP/JP,          dGP/JP, (-2*dqP*wPR - dGP - dvP)/JP
];
Bl = [
    1/LG,  0;
    0,     0;
    0,     0;
    0, -1/JP
];
cl = [0, 0, 0, 1];
dl = [0, 0];

eigAl = eig(Al);