%  Example 6.6.  Steady-state optimal control of a coast guard buoy tender.
clear
%  Define the state model.
A=[0 1 0 0 0 0
   0 0 0 0 0 0
   0 0 0 1 0 0
   0 0 0 0 0 0
   0 0 0 0 0 1
   0 0 0 0 0 0];
B=[  0       0      0
   1.4e-5    0      0
     0       0      0
     0    6.8e-6 6.8e-6
     0       0      0
     0      79     -81 ];
%  Note:  C and D refer to the reference output.
C=[1 0 0 0 0 0
   0 0 1 0 0 0
   0 0 0 0 1 0];
D=zeros(3);
%  Define the cost function parameters.
Q=diag([1e4 0 5e4 0 5e8 5e9]);
R=diag([1 4 4]);
%  Find the steady-state LQR gain. 
K=lqr(A,B,Q,R)
Acl=A-B*K;
%  Simulate the system.
x0=[100 0 50 0 20/57.3 0]';
t=0:0.5:150;
[y,x,t]=initial(Acl,B,C,D,x0,t);
%  Compute the control input (for all times).
u=K*x';
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(211),plot(t,y(:,1),'r-',t,y(:,2),'r--')
xlabel('Time (sec)')
ylabel('Displacement (ft)')
grid
legend('Surge','Sway')
subplot(212),plot(t,57.3*y(:,3))
xlabel('Time (sec)')
ylabel('Angle (deg)')
grid
legend('Yaw')
figure(2)
clf
plot(t,u(1,:),'r-',t,u(2,:),'r--',t,u(3,:),'r-.')
xlabel('Time (sec)')
ylabel('Thrust (lbs)')
grid
legend('{\itu}_{\itp}', '{\itu}_{\itb}', '{\itu}_{\its}')
