%  Example 6.10.  Generate a root locus for a multi-input LQR system as 
%    the control weighting in the cost function is varied.
clear
%  Define the plant model.
A=[  0   1   0
     0   0   1
   -170 -37 -12];
B=[0  0
   1  0
   0  1];
C=[7 2 4
   5 1 0];
D=zeros(2);
%  Define the state weighting matrix.
Qy=[100 0
     0  1];
Q=C'*Qy*C;
%  Compute the plant poles.
plant_poles=eig(A)
%  Compute the zeros of B'*inv(-sI-A')*Q*inv(sI-A)*B.  Note:  These zeros
%  are simply the plant zeros since the plant is square.
Plant=pck(A,B,C,D);
plant_zeros=szeros(Plant)
finite_zeros=plant_zeros(2)
%  The control weighting matrix is Rho*R0.
R0=eye(2);
%  Define the values to be used for the control weighting parameter Rho.
Rhov=logspace(-1,3,50);
%  Generate the closed loop pole locations for each Rho.
for i=1:50,
   [K,S,E]=lqr(A,B,Q,Rhov(i)*R0);
   pole_k(i,:)=E';
end
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
plot(pole_k,'x')
xlabel('Real axis')
ylabel('Imaginary axis')
grid
