%  Example 4.3.  Quadratic cost computation via simulation.
clear
%  Define the  sampling time dt and the time vector t.
dt=0.1;
t=0:dt:150;
%  Define the test conditions.
r=180*atan2(320,(562.5-7.5*t))/pi;
Torque=cos(atan2(320,(562.5-7.5*t)));
v=zeros(size(t));
w=[r;Torque;v]';
x0=[0 0 25 0]';
%  Define the closed loop system:
p1=4; p2=0.1; k1=10; k2=0.01; k3=10; k4=150; a=0.4; b=4;
Acl=[-p1  0  -k1*k4    k1
      k2 -p2    0      0
	  0   1     0      0
	  0   0  k4*(b-a) -b ];
Bcl=[ k1*k4     0   -k1*k4
        0     k2*k3    0
        0       0      0
     k4*(a-b)   0   k4*(b-a)];
Ccl=[0 0 -1  0
     0 0 -k4 1];
Dcl=[1  0  0
     k4 0 -k4];
%  Simulate the system.
y=lsim(Acl,Bcl,Ccl,Dcl,w,t,x0);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(2,1,1),plot(t,y(:,1))
xlabel('Time (sec)')
ylabel('Error (deg)')
grid
subplot(2,1,2),plot(t,y(:,2))
xlabel('Time (sec)')
ylabel('Control (volts)')
grid
%  Compute the cost from the simulation results.
YY=[dt/300     0
       0   dt/300/15^2];
J=trace(YY*y'*y)
