%Regelungstechnik Serie 8 Aufgabe 3b)
%Marco Weber

clear all               % Clear all variables
close all
clc                     % Clear workspace

% Parameters

M=10;                    % Mass of cart [kg]
m=1;                    % Mass of sphere on pendulum [kg]
p_l=1;                  % Length of pendulum [m]
g=9.81;                 % Earth's gravity [m/s^2]

%Initial Conditions 

x0=[-0.7;0;0;0];
q=1000;                 %Gewichtung Matrix Q
r=1;               %Gewichtung Matrix R
gamma=1000;         %für Ctilde


%Define linear matrices

A=[0,1,0,0,;(M+m)*g/(M*p_l),0,0,0;0,0,0,1;-m*g/M,0,0,0];
B=[0;-1/(M*p_l);0;1/M];
C=[0,0,1,0];

% Define weighting matrices

Q=diag([1,1,q,1]);
R=[eye*r];

%solve Problem to determine K

Atilde=[A,zeros(4,1);-C,zeros(1,1)];
Btilde=[B;0];
Ctilde=[chol(Q),zeros(4,1);zeros(1,4),eye*gamma];

%LQRi
Ktilde=lqr(Atilde,Btilde,Ctilde'*Ctilde,R);

K=Ktilde(1,1:4);
KI=Ktilde(1,5:5);

%Run simulation

[t,x,z,dsoll,v]=sim('Ue8_simb',10);

%Plot results
figure (1)
title('mit Integrator')

subplot(1,3,1)
plot(t,z(:,1))
legend('\phi(t)')

subplot(1,3,2)
plot(t,z(:,3),t,dsoll)
legend('d(t)','d[soll](t)')
subplot(1,3,3)
plto(t,v)
legend('v(t)')


% figure (2)
% title('ohne Integrator')
% 
% subplot(1,3,1)
% plot(t_o,z_o(:,1))
% legend('\phi[ohne I](t)')
% 
% subplot(1,3,2)
% plot(t_o,z_o(:,3),t,dsoll)
% legend('d[ohne](t)','d[soll](t)')
% subplot(1,3,3)
% plto(t_o,v_o)
% legend('v[ohne](t)')