% Skript LQR_1.m, in dem ein Servosystem mit 
% einer LQR-regelung untersucht wird. 
% Arbeitet mit Modell LQR1.mdl
clear;
% -------- Parameter des Systems
m1 = 10; m2 = 10; m3 = 5;
k1 = 10;   k2 = 1;    k3 = 10;
c1 = 0.5;  c2 = 0.5; 
n = 6;     % Ordnung des Systems
A = [0 1 0 0 0 0;
    -(k1+k3)/m1 -c1/m1 k1/m1 c1/m1 k3/m1 0;
    0 0 0 1 0 0;
    k1/m2 c1/m2 -(k1+k2)/m2 -(c1+c2)/m2 k2/m2 c2/m2;
    0 0 0 0 0 1;
    k3/m3 0 k2/m3 c2/m3 -(k2+k3)/m3 -c2/m3];
B = [0 1 0 0 0 0]';         C = [0 0 0 0 1 0];         D = 0;
randn('seed', 17935);
%x0 = randn(1,6);    % Anfangsbedingungen
x0 = 0;     
% ------- LQR-Regelung mit I-Anteil
An = [A, zeros(6,1); -C,0];    % Erweitertes Modell
Bn = [B;0];
Q = eye(7,7)*100;   % Kosten der Zielfunktion    
R = 1;
Kn = lqr(An,Bn,Q,R),      % LQR-Rückführungsmatrix
pole = eig(An-Bn*Kn),          % Eigenwerte des Systems
K = Kn(1:6),      ki = -Kn(7), % Rückführung und I-Anteil
% ------- Aufruf der Simulation
Tfinal = 50;      dt = 0.01;
my_options = simset('Solver','ode45','MaxStep',dt);
sim('LQR1', [0, Tfinal], my_options);

y = yx.signals.values(:,1);
x = yx.signals.values(:,2:7);
t = yx.time;
figure(1);    clf;
subplot(211), plot(t, y);
  title(['Sprungantwort für y(t) mit x0 = ',num2str(x0)]);
  xlabel('s');    grid on;
subplot(212), plot(t, x);
  title('Zustandsvariablen ');
  xlabel('s');    grid on;
  legend('x1g','v1g','x2g','v2g','x3g','v3g')


