% ECE 147B - Lecture 13, Katie Byl (2013)
format compact 
clear all

fprintf('\n\n')
input('Part 1: (hit enter)')
% Part 1: Controllability, general case
n=4;
A = rand(n,n)  % n states
B = rand(n,1)  % single input here...
Co = ctrb(A,B)  
RankCo = rank(Co)  % iff full rank -> Controllable (i.e.,invertible)!
sdes = [1 2 3 4 5] % desired characteristic eqn
pdes = roots(sdes) % desired poles

alpha_c = 0*A; % initialize
for ni = 1:length(sdes)
    alpha_c = alpha_c + sdes(ni)*A^(n+1-ni);
end
K = [zeros(1,n-1), 1] * inv(Co) * alpha_c

pact = eig(A-B*K)

fprintf('\n\n')
input('Part 2:')
% Part 2: A particular example
n=2;
A = [-2 -5; 1 0];   % control canonical form
B = [1; 0]; 
Co = ctrb(A,B)  % Co: CONTROLLABILITY matrix

RankCo = rank(Co)  % iff full rank -> Controllable (invertible)!
sdes = [1 20 100]  % sdes: desired char eqn coef?s
pdes = roots(sdes) % pdes: desired poles
alpha_c = 0*A;     % initialize
for ni = 1:length(sdes)
    alpha_c = alpha_c + sdes(ni)*A^(n+1-ni);
end

K = [zeros(1,n-1), 1] * inv(Co) * alpha_c  % Gain matrix
 
pact = eig(A-B*K) % pact: CL poles
