%% Ivan Edwards

%% CE 1.4

% Define Variables
syms L
syms J
syms Jb
syms r
syms t
syms m
syms p
syms pzero
syms pOneDot
syms pTwoDot
syms g
syms b %b = m/[(Jb/r^2)+m]
syms theta
syms thetaOneDot
syms thetaTwoDot
syms Tau

syms x1
syms x2
syms x3
syms x4

syms x1dot
syms x2dot
syms x3dot
syms x4dot

%input
syms u
%output
syms y

x1 = p;
x2 = pOneDot;
x3 = theta;
x4 = thetaOneDot;

u = Tau;
y = p;

% Derivation through Linearization using ODE

x1dot = x2;
x2dot = b*[(x1*x4^2)-g * sin(x3)];
x3dot = x4;

%% CE 2.4
% Numerical parameters

L=1;
J=10.0676;
m=0.9048;
r=0.03;
Jb=0.000326;
g=9.81;
pzero = 0.25;
b = m/[(Jb/r^2)+m];

% Define State Space Matrices

A = [0 1 0 0;0 0 -(b*g) 0;0 0 0 1;(-m*g)/(m*pzero^2+J+Jb) 0 0 0];
B = [0;0;0;1/(m*pzero^2+J+Jb)];
C = [1 0 0 0];
D = [0];

sys = ss(A, B, C, D)

% Simulation of SS
t = [0:5:50]
U = [zeros(size(t))];
%Open Loop Eigen Values (System is unstable)
E = eig(A)

%Plot open-loop state variables in response to Tao = " " and p(0) = 0.25
impulse(sys)

%% CE 3.4

% Determine if system is controllable

%% CE 6.4

%Use Lyapunov analysis to determine stability

%Use eigen value analysis to asses system stability

%Plot phase portraits

%% CE 7.4

%Design feedback Control Law (Calculate K)

% -1.27 + 3.79i

% -1.27 - 3.79i

% -1.88 + 1.24i

% -1.88 - 1.24i

%Plot and Compare Open vs Closed Loop

%% CE 4.4

%Deteremine is system is observable

%% CE 8.4

% Design Observer based compensator for the control law