%% SPLS/2e MATLAB
% Chapter 13
clear; close all;
%% Ex. 13.5
num = [2 10]; den = [1 8 19 12];
[A,B,C,D] = tf2ss(num,den)
[num,den] = ss2tf(A,B,C,D); H = tf(num,den)
%% Ex. 13.6
syms s
A = [-12 2/3;-36 -1]; B = [1/3; 1]; q0 = [2;1]; X = 1/s;
q = ilaplace(inv(s*eye(2)-A)*(q0+B*X))
t = (0:.01:2); q = subs(q); q1 = q(1,:); q2 = q(2,:);
plot(t,q1,'k',t,q2,'k--'); xlabel('t'); ylabel('Amplitude');
legend('q_1(t)','q_2(t)','Location','SE');
%% Ex. 13.7
A = [0 1;-2 -3]; B = [1 0;1 1];
C = [1 0;1 1;0 2]; D = [0 0;1 0;0 1];
syms s; H = collect(simplify(C*inv(s*eye(2)-A)*B+D))
[num,den] = ss2tf(A,B,C,D,2); H_32 = tf(num(3,:),den)
%% Ex. 13.10
A = [0 1;-2 -3]; B = [1; 2];
P = [1 1;1 -1];
Ahat = P*A*inv(P), Bhat = P*B
%% Ex. 13.11
A = [0 1;-2 -3]; B = [1; 2];
[V, Lambda] = eig(A);
P = inv(V), Lambda, Bhat = P*B
%% Ex. 13.12
A = [1 0;1 -1]; B = [1; 0]; C = [1 -2];
[V, Lambda] = eig(A); P=inv(V); Bhat = P*B, Chat = C*inv(P)
A = [-1 0;-2 1]; B = [1; 1]; C = [0 1];
[V, Lambda] = eig(A); P=inv(V); Bhat = P*B, Chat = C*inv(P)
%% Ex. 13.13
A = [0 1;-1/6 5/6]; B = [0; 1]; C = [-1 5]; D = 0;
N = 25; n = (0:N); x = ones(1,N+1); q0 = [2;3];
sys = ss(A,B,C,D,-1); % Discrete-time state space model
[y,q] = lsim(sys,x,n,q0); % Simulate output and state vector
clf; stem(n,y,'k.'); xlabel('n'); ylabel('y[n]'); 
axis([-.5 25.5 11.5 13.5]);
%% Section 13.8.1
A = [0 1;-1/6 5/6]; B = [0; 1]; C = [-1 5]; D = 0;
q_0 = [2;3];
z = sym('z');
X = ztrans(sym('1'))
Q = inv(eye(2)-z^(-1)*A)*q_0 + inv(z*eye(2)-A)*B*X
Q = simplify(Q)
Y = simplify(C*Q)
y = iztrans(Y)
y_zir = iztrans(simplify(C*inv(eye(2)-z^(-1)*A)*q_0))
y_zsr = y - y_zir
iztrans(simplify(C*inv(z*eye(2)-A)*B*X))
n = [0:25]; stem(n,subs(y,n),'k.'); 
xlabel('n'); ylabel('y[n]'); axis([-.5 25.5 11.5 13.5]);
%% Section 13.8.2
H = collect(simplify(C*inv(z*eye(2)-A)*B+D))
[num,den] = ss2tf(A,B,C,D)
syms gamma; char_poly = subs(det(z*eye(2)-A),z,gamma)
roots(sym2poly(char_poly))
h = iztrans(H)
iztrans(z^(-2)*X)
y_zsr = iztrans(H*X)
%% Section 13.8.3
A = [0 1;-1/6 -5/6]; B = [0; 1]; C = [-1/6 -1/3]; D = 1;
[V,Lambda] = eig(A)
P = inv(V);
Ahat = P*A*inv(P), Bhat = P*B, Chat = C*inv(P)
A = [0 -1/6;1 -5/6]; B = [-1/6; -1/3]; C = [0 1]; D = 1;
[V,Lambda] = eig(A)
P = inv(V);
Ahat = P*A*inv(P), Bhat = P*B, Chat = C*inv(P)
%% Section 13.8.3
A = [0 1;-1/6 5/6]; n = 3; A^n
A*A*A
syms z n; An = simplify(iztrans(inv(eye(2)-z^(-1)*A)))
double(subs(An,n,3))
syms t; A = [-12 2/3;-36 -1]; eAt = simplify(expm(A*t))
syms s; simplify(ilaplace(inv(s*eye(2)-A)))
double(subs(eAt,t,3))
expm(A*3)