%  Example 9.3.  Time-varying Hinf radar range tracking.
clear
%  Define the bounds on the disturbance input and the measurement error.
Wbound=4; Vbound=20;
%  Compute the inverse of the measurement error bound to simplify
%    future computations.
Vbinv=1/Vbound;
%  Define the plant model (with normalized inputs).
A=[0 1;0 0]; B=[0 0;Wbound 0]; D=[0 1];
Cm=[1/Vbound 0]; Cy=eye(2);
%  Initialize the state and the estimator state.
x(:,1)=[10000;-50]; xh(:,1)=[9500;0];
%  Initialize the Riccati solution.
Q=zeros(2);
%  Define the time vectors.  Note:  A short sampling time is used in 
%    the beginning, and a long sampling time is used subsequently.
dt1=0.001;
n1=1000;
dt2=0.1;
n2=240;
%  Define the disturbance input and the measurement error.
vn=2*Vbound*rand(1,n1+n2)-Vbound;
wn=2*Wbound*rand(1,n1+n2)-Wbound;
%  Define the performance bound.
gamma=22.5;
gm2=1/gamma/gamma;
%  Simulation loop for plant, Riccati equation, and estimator.
for i=1:n1,
  %  Plant simulation.
  m=[1 0]*x(:,i)+vn(i);
  x(:,i+1)=x(:,i)+dt1*(A*x(:,i)+B(:,1)*wn(i));
  %  Riccati solution.
  Q=Q+dt1*(Q*A'+A*Q+B*B'-Q*(Cm'*Cm-gm2*Cy'*Cy)*Q);
  %  Gain computation.
  G=Vbinv*Q*Cm';
  %  Save the estimator gains.
  Gs(:,i)=G;
  %  Estimator simulation.
  xh(:,i+1)=xh(:,i)+dt1*(A*xh(:,i)+G*(m-[1 0]*xh(:,i)));
  end
%  Use a different sampling time for the remainer of the simulation.
for i=n1+1:n1+n2,
  %  Plant simulation.
  m=[1 0]*x(:,i)+vn(i);
  x(:,i+1)=x(:,i)+dt2*(A*x(:,i)+B(:,1)*wn(i));
  %  Riccati solution.
  Q=Q+dt2*(Q*A'+A*Q+B*B'-Q*(Cm'*Cm-gm2*Cy'*Cy)*Q);
  %  Gain computation.
  G=Vbinv*Q*Cm';
  %  Save the estimator gains.
  Gs(:,i)=G;
  %  Estimator simulation.
  xh(:,i+1)=xh(:,i)+dt2*(A*xh(:,i)+G*(m-[1 0]*xh(:,i)));
end
%  Define the time vector for plotting.
t=[0:dt1:n1*dt1 n1*dt1+dt2:dt2:n1*dt1+n2*dt2];
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(2,2,1)
plot(t, x(1,:), 'r-', t, xh(1,:), 'r-.')
axis([0 25 8000 12000])
legend('Actual', 'Estimate')
xlabel('Time (sec)')
ylabel('Range')
grid
subplot(2,2,2)
plot(t, x(2,:), 'r-', t, xh(2,:), 'r-.')
axis([0 25 -100 100])
legend('Actual', 'Estimate')
xlabel('Time (sec)')
ylabel('Range rate')
grid
subplot(2,2,3)
t=[dt1:dt1:n1*dt1 n1*dt1+dt2:dt2:n1*dt1+n2*dt2];plot(t, Gs)
plot(t, Gs(1,:), 'r-', t, Gs(2,:), 'r-.')
axis([0 25 0 5])
legend('{\itG}_1', '{\itG}_2')
xlabel('Time (sec)')
ylabel('Gains')
grid
