%% Reachable and observable directions in LTI systems

%% State-space model
% We start by defining the state space model. Either we can use the
% particular data below

A = [1 3; -1 -2];
B = [1 0]';
C = [0 1];
D = 0;
G = ss(A,B,C,D);

%%
% or we can generate a model randomly

G = rss(2);
[A,B,C,D] = ssdata(G)

%% Analyzing the modes
% We compute the modes by computing the eigenvalues and the corresponding 
% right and left eigenvectors 

[V,D,W] = eig(A)

%%
% For convenience we will relabel

v1 = V(:,1);
v2 = V(:,2);
w1 = W(:,1);
w2 = W(:,2);

%% 
% We can also form the following two matrices
A1 = v1*w1'
A2 = v2*w2'

%%
% These two matrices define the two modes in the following way
%
% $e^{\mathrm At}=\mathrm A_1e^{\lambda_1 t}+\mathrm A_2e^{\lambda_2 t}$
%
% But then the response to some nonzero initial state $\mathrm x(0) = \mathrm x_0$ can be computed as
%
% $x(t) = e^{\mathrm At}\mathrm x_0=\mathrm v_1e^{\lambda_1 t}\underbrace{\mathrm w_1^T\mathrm x_0}_{\alpha_1}+\mathrm v_2e^{\lambda_2 t}\underbrace{\mathrm w_2^T\mathrm x_0}_{\alpha_2}$
% 
% Obviously, the right eigenvectors $\mathrm v_i$ give the directions in
% which the individual poles exhibit themselves in the state plane most visibly and the
% left eigenvectors $\mathrm w_i$ give the directions in which the initial states must find themselves in order to stimulate the state
% responses mentioned previously most efficiently. We will now plot these. But since we are
% only interested in the directions, let's normalize the vectors first

v1n = v1/norm(v1);
v2n = v2/norm(v2);

w1n = w1/norm(w1);
w2n = w2/norm(w2);

%%
% Now we plot the right eigenvectors

plot([0 v1n(1)],[0 v1n(2)],'o-b',[0 v2n(1)],[0 v2n(2)],'o-r')
xlabel('x_1')
xlabel('x_2')
grid on
legend('v_1','v_2'), hold on, 
plot(exp(i*(0:0.01:2*pi)),'y.')
axis equal
hold off

%%
% and now the left eigenvectors

plot([0 w1n(1)],[0 w1n(2)],'o-b',[0 w2n(1)],[0 w2n(2)],'o-r')
xlabel('x_1')
xlabel('x_2')
grid on
legend('w_1','w_2'), hold on, 
plot(exp(i*(0:0.01:2*pi)),'y.')
axis equal
hold off

%%
% Note that the right and left eigenvectors coincide in the particular case
% of $\mathrm A=\mathrm A^T$, which seems to be the case for the randomly
% generated systems in Matlab. But generally this is not true.

%%
% If the modes are complex, we can rewrite them using just real numbers
% as
%
% $e^{\mathrm At}=2\Re(\mathrm A_1)\Re(e^{\lambda_1 t})-2\Im(\mathrm A_1)\Im(e^{\lambda_1 t}) = 2e^{\Re(\lambda_1)t}(\Re(\mathrm A_1)\cos(\Im(\lambda_1)t)-\Im(\mathrm A_1)\sin(\Im(\lambda_1)t)))$
%
% Ehmm... but then the analysis explained below becomes tricky because what
% is the point in reducing a system with just two poles to a system with
% just one pole using truncation? Which of the two modes should be
% discarded? Computation of Hankel singular values indeed shows that one
% mode is more significant then the other... Now what. Well, let's postpone
% the discussion of this case (two complex conjugate poles) for a while and
% let's just consider the cases with real poles.

%% Computing the reachability and observability gramians
% We will now compute the reachability and observability gramians by
% solving Lyapunov equations. Note that certain conditions need to be
% satisfied for the equation to have a solution. For randomly generated
% data the solver may occassionally fail to find a solution if those
% conditions are not satisfied. Consult the doc lyap.

P = lyap(A,B*B')
Q = lyap(A',C'*C)

%%
% Let's now analyze the easily reachable and the easily observable
% modes characterized by the corresponding "directions" by performing the
% eigendecomposition of the gramians

[Vp,Dp] = eig(P)
[Vq,Dq] = eig(Q)

%%
% Obviously, the mode corresponding to the second column of Vp enjoys 
% better reachability and the mode corresponding to second column of Vq 
% exhibits better observability. However, the two modes are not aligned, as 
% seen below.

%% Visualisation of the reachable and observable directions
%

theta=0:0.01:2*pi; 
x=[cos(theta);sin(theta)]; 

xp = sqrtm(P)*x;
xq = sqrtm(Q)*x;

plot(x(1,:),x(2,:),'y.')        % plotting a unit circle
hold on
plot(xp(1,:),xp(2,:),'r.')      % plotting a reachability ellipsoid
plot([0 Vp(1,2)],[0 Vp(2,2)],'r-o')      % plotting a strongly reachable direction
xlabel('x_1')
ylabel('x_2')
axis equal

plot(xq(1,:),xq(2,:),'b.')      % plotting an observability ellipsoid    
plot([0 Vq(1,2)],[0 Vq(2,2)],'b-o')      % plotting a strongly observable direction

hold off

%%
% Finally, let's put the visualisation of the reachable and observable
% directions into a single figure with the visualization of the left and
% right eigenvectors of A, that is, the modes

plot([0 v1n(1)],[0 v1n(2)],'o-b',[0 v2n(1)],[0 v2n(2)],'o-r')
xlabel('x_1')
xlabel('x_2')
grid on
hold on, 
plot([0 Vq(1,2)],[0 Vq(2,2)],'g-o') 
plot([0 Vp(1,2)],[0 Vp(2,2)],'m-o')
plot(exp(i*(0:0.01:2*pi)),'y.')
legend('v_1','v_2','v_q','v_p')
axis equal
hold off

%%
%

plot([0 w1n(1)],[0 w1n(2)],'o-b',[0 w2n(1)],[0 w2n(2)],'o-r')
xlabel('x_1')
xlabel('x_2')
grid on
hold on, 
plot([0 Vq(1,2)],[0 Vq(2,2)],'g-o') 
plot([0 Vp(1,2)],[0 Vp(2,2)],'m-o')
plot(exp(i*(0:0.01:2*pi)),'y.')
legend('w_1','w_2','v_q','v_p')
axis equal
hold off

%%
% We can now observe (for some randomly generated models this will be easier to see) 
% that some modes are easier to control while some (other) modes are easier to control. 
% Sometimes it can happen that the same mode will enjoy both the better controllability 
% and the observability, but in general not. In fact, we can see a bit of
% this if we now write down the transfer function of the original system in
% the factored form

zpk(G)

%%
% Is any of the poles shadowed by a zero? This could be seen even more
% easily after expressing the transfer function in the partial fraction
% expansion

Gtf = tf(G)
[r,p,k] = residue(Gtf.num{:},Gtf.den{:})


%%
% Are any of the residues (entries of the r vector) significantly smaller than the others? 
% In that case the corresponding poles (the entries of the p vector) will not contribute 
% significantly to the input-output response - they are either weakly
% controllable or observable or both.