%Christopher Lum
%AA547 Ly
%Sept. 15, 2004
%
%Matlab Tutorial (Itermediate)
%This m-file follows along with the matlab_tutorial_intermediate.doc
%document.

clear
clc
close all

%-------------------------Part 1--------------------------------
%Define the constants
k = 1;
m = 1;
c = 1;

%Enter the A, B, C, and D matrices  
A = [0 1;
    -k/m -c/m];

B = [0;
     1/m];
 
C = [1 0];

D = [0];

%-------------------------Part 2--------------------------------
%Check stability of this system
[eigenvectors_A,eigenvalues_A] = eig(A)



%-------------------------Part 3--------------------------------
%Check that the eigenvectors and eigenvalues satisfy Eq. 2
lambda1 = eigenvalues_A(1,1);
lambda2 = eigenvalues_A(2,2);

v1 = eigenvectors_A(:,1);
v2 = eigenvectors_A(:,2);

(A - lambda1*eye(2))*v1
(A - lambda2*eye(2))*v2



%-------------------------Part 4--------------------------------
%Characteristic equations is s^2 + (c/m)*s + (k/m)



%-------------------------Part 5--------------------------------
%Calculate a, b, and c, the coefficients of the characteristic equation
a = 1;
b = c/m;
c = k/m;

%Solve the roots of the characteristic equation using custom function
eigenvalues = quadratic_equation(a,b,c)



%-------------------------Part 6--------------------------------
%Perform a similarity transformation on the A matrix
T = eigenvectors_A;
A_tilde = inv(T)*A*T



%-------------------------Part 7--------------------------------
%Compute the controllablity matrix
Pc = [B A*B]

%Check the rank
rank_Pc = rank(Pc)

%Check the determinant is not near zero
det_Pc = det(Pc)



%-------------------------Part 8--------------------------------
%Use the columns of Pc as 1 set of basis vectors
v1 = Pc(:,1);
v2 = Pc(:,2);

%Calculate the orthonormal basis for this space
Q = orth(Pc);
q1 = Q(:,1);
q2 = Q(:,2);

%Plot these
figure
hold on                                         %turn on hold to plot multiple
                                                %sets of data on the same plot
                                                
plot([0,v1(1)],[0,v1(2)],'b-')                  %plot the 1st basis vector
plot([0,v2(1)],[0,v2(2)],'b-')                  %plot the 2bd basis vector
plot([0,q1(1)],[0,q1(2)],'r-','LineWidth',3)    %plot the 1st orthonormal vec
plot([0,q2(1)],[0,q2(2)],'r-','LineWidth',3)    %plot the 2nd orthonormal vec

title('Basis vectors for Controllable Space')
xlabel('x_1')
ylabel('x_2')
grid
legend('v_1','v_2','q_1 (orthonormal)','q_2 (orthonormal)')
axis([-1.5 1.5 -1.5 1.5])
hold off                                        %turn off hold mode