load segway_vars sys_katie
more on


input('Convert CT system to DT system: ','s')
T = .1
sys_katie_dt = c2d(sys_katie,T,'zoh')

% extract A and B matrices for CT system:
Act = sys_katie.a;
Bct = sys_katie.b;

input('extract A and B matrices for DT system: ','s')
B = sys_katie_dt.b
A = sys_katie_dt.a

input('Controllability matrix: ')
Co = [B A*B A^2*B A^3*B]
% Find poles of the system:
pvals = eig(A)
% Build the characteristic polynomial (with roots at poles):
apoly = 1;
for n=1:length(pvals)
    apoly = conv(apoly,[1 -pvals(n)])
end

input('"Canonical form" for A and B matrices:','s')
Ac = [(-apoly(2:end)./apoly(1))
    [eye(3,3)], [zeros(3,1)]]
Bc = [1;0;0;0]
% Controllability matrix for canonical form system:
Coc = [Bc Ac*Bc Ac^2*Bc Ac^3*Bc]
% Transformation matrix, to convert between A and Ac:
Tmat = Co*inv(Coc)
Tmat_inv = Coc*inv(Co)

Ac_test = Tmat_inv * A * Tmat

input('Now, pick an arbitrary desired polynomial: ','s')
apoly_des = [1 2 3 4 5]
% Roots of this are:
poles_des = roots(apoly_des)
% We want "Ac - Bc*Kc" to have the negative of apoly_des(2:end) in top row:
BK_needed = (apoly_des - apoly)  % (double negative not shown...)
Kc = BK_needed(2:end)  % gains for "Ac, Bc" canonical system
K = Kc * Tmat_inv  % Convert to original system gains

input('Now, test poles locations for both systems: ','s')
eig(Ac - Bc*Kc)
eig(A - B*K)



