%% Optimale Regelung
%% Entwurf einer optimalen Zustandsregelung
%% für ein Labormodell (Inverses Pendel)
close all
clear all
clc

%Definieren der Konstanten
m=1*10^3;
M=10*10^3;
l=1;
g=9.80655;
beta1=M+m;
beta2=1/(M+0.5*m);
a23=(-0.5*g*m)/beta2;
a43=(g*beta1)/(2*l*beta2);
b2=1/beta2;
b4=(-1)/(2*l*beta2);
c1=0;
c2=0;
c3=1;%Winkel des Pendels
c4=0;
fuehrungsgroesse=1;

%Zustandsbeschreibung in Matrizenschreibweise (1-Größen-System)
%x1 := Position des Wagens
%x2 := Geschwindigkeit des Wagens
%x3 := Winkel des Pendels
%x4 := Geschwindigkeit des Pendels
%u := Beschleunigung  (aktive Beeinflussung des Pendels)
%Systemmatrix
A=[0,1,0,0;0,0,-5.1484*10^7,0;0,0,0,1;0,0,5.6633*10^8,0];
%Eingangsmatrix
b=[0;10500;0;-5250];
%Ausgangsmatrix
cT=[0,0,1,0];
%Durchgangsmatrix
d=0;
%Anfangszustand
x0=[1;1;1;1];

%Steuerbarkeit überprüfen
Qs=[b,A*b,A^2*b,A^3*b]%Steuerbarkeitsmatrix
r=rank(Qs);
n=4;
if r==n
    'System ist vollständig steuerbar und somit stabilisierbar!!!'
end

%Berechnung des Optimalreglers
e=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];%Einheitsmatrix
q=3;
Q=q*e;
R=1;
k=lqr(A,b,Q,R)

%Simulieren
sim('modell_invpen.mdl',9);

%Ausgabe
hold off
pendelwinkel=plot(t,regelgroesse,'r');
set(pendelwinkel,'LineStyle','-');
set(pendelwinkel,'LineWidth',1.75);
grid on
grid MINOR
title('Inverses Pendel\newline(linearisiertes Modell)\newline(mit Regelung)','FontSize',14,'FontWeight','Bold');
xlabel('Zeit in [sec] \rightarrow');
ylabel('Winkel in [°] \rightarrow');
legend('Winkel des Pendels','location','SouthEast');