% Title   : Uebung8.m
% Author  : Charlie Boston
% Created : 01-May-2008
%Edited: 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.1;0;0;0];
q=1000;                 %Gewichtung Matrix Q
r=0.0001;               %Gewichtung Matrix R

% 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];

% Define weighting matrices
Q=diag([q,1,1,1]);
R=[eye*r];

% Solve problem to determine K

K=lqr(A,B,Q,R);

% Run simulation
u=-K*x0;
[t,x,z,v]=sim('Ue8_sim',30);

% Plot results

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

fiqure(2)
plot(t,z(:,3))
legend('d(t)')

figure(3)
plot(t,v)
legend('v(t)')

