clc
clear all
close all

%% inverted pendulum mounted on motorized cart
        % extra assigment

%% declaration of constants
    mass_c = 0.8;
    mass_p = 0.6;
    b = 0.12;
    l = 0.6;
    g = 9.8;
    
    initial = [0; 0; 0.3; 0];
    
    states = {'p' 'p_dot' 'theta' 'theta_dot'};
    inputs = {'F'};
    outputs = {'p' 'theta'};  

%% definition of the open loop plant model
    % definition of the state-space matrices
        A = [0 1 0 0; 0 -b/mass_c (mass_p/mass_c)*g 0; 0 0 0 1; 0 -b/(mass_c*l) (g/l)*(1+(mass_p/mass_c)) 0];
        B = [0; 1/mass_c; 0; 1/(mass_c*l)];
        C = [1 0 0 0; 0 0 1 0];
        D = [0; 0];
    
    % convert into transfer function
        plant_model_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);
        plant_model = tf(plant_model_ss)
        
%% analysis of the plant_model
    % plot the step response
        figure
        hold on
        step(plant_model(1))
        step(plant_model(2))
        legend('position of the cart','angular position of the pendulum')
        hold off
        
    % plot the impulse response
        figure
        hold on
        impulse(plant_model(1))
        impulse(plant_model(2))
        legend('position of the cart','angular position of the pendulum')
        hold off
    % poles and zeros
        poles_p = pole(plant_model(1))
        poles_theta = pole(plant_model(2))
        zeros_p = zero(plant_model(1))
        zeros_theta = zero(plant_model(2))
        % To show it graphically use the root locus
        figure
        hold on
        subplot(1,2,1)
        rlocus(plant_model(1))
        title('position of the cart')
        subplot(1,2,2)
        rlocus(plant_model(2))
        title('angular position of the pendulum')
        hold off

%% Design of the controller
    % Proposed structure of wightin matrix Q assumg, that both output
    % states have an equal importance.
        Q = C'*C

    % Test, if the LQR could work. The contrability matrix has to have full
    % rank.
        co = ctrb(plant_model_ss);
        controllability = rank(co)
        
    % Design the controller to meet the design requirements
        Q(1,1) = 55;
        Q(3,3) = 60;
        R = 1;
        K = lqr(plant_model_ss,Q,R)

        Ac = [(A-B*K)];
        Bc = [B];
        Cc = [C];
        Dc = [D];

        sys_LQR_ss = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);
        sys_LQR = tf(sys_LQR_ss);
        
        info_p = stepinfo(sys_LQR(1))
        info_theta = stepinfo(sys_LQR(2))
        
        t = 0:0.1:2;
        u = 0.2 * ones(size(t));      
        figure
        lsim(sys_LQR_ss,u,t,initial)
%         initialplot(sys_LQR_ss,initial)
        
        poles_LQR = pole(sys_LQR_ss)
        
%% Observer
    % Is a observer possible?
        ob =obsv(plant_model_ss);
        observability = rank(ob)