%  Example 7.2.  Generate the time-varying Kalman gains, simulate the
%    plant, and simulate the Kalman filter.
clear
%  Define the plant model.
A=[0 1
   0 0];
B=[0
   1];
C=[1 0];
D=0;
%  Define the plant noise and the measurement noise spectral densities.
Sw=4;
Sv=diag(1e4);
%  For efficiency, compute the inverse of the spectral density.
invSv=inv(Sv);
%  Allocate space for the arrays (increases efficiency).
x=zeros(2,1241);
xh=zeros(2,1241);
Sd=zeros(2,1240);
Gs=zeros(2,1240);
%  Initialize the state and the state estimate, and allocate space 
%    for these variables.
x(:,1)=[10000; -150];
xh(:,1)=[9000; 0];
%  Initialize the estimation error covariance matrix.
P=[1e6  0
    0  4e5];
% P=[0  0
%    0  0];
%  Compute the standard deviations of the state estimates. 
Sd(:,1)=sqrt(diag(P));
%  The simulation is performed using a short time step initially.  After
%    one second, the time step is increased.
%  Define the short sampling time and the number of time steps.
dt1=0.001;
n1=1000;
%  Define the standard deviation of the plant noise and the measurement
%    noise (with this time step) for use in the simulation.
sigw=sqrt(Sw/dt1);
sigv=sqrt(Sv/dt1);
%  Generate the plant and measurement noises for use in the simulation.
vn=sigv*randn(1,n1);
wn=sigw*randn(1,n1);
%  Perform the plant simulation, the gain computation, and the Kalman
%    filter simulation.
for i=1:n1,
  %  Plant simulation.
  y=C*x(:,i)+vn(i);
  x(:,i+1)=x(:,i)+dt1*(A*x(:,i)+B*wn(i));
  %  Riccati solution.
  P=P+dt1*(P*A'+A*P+B*Sw*B'-P*C'*invSv*C*P);
  %  Save the standard deviations of the state estimation errors.
  Sd(:,i)=sqrt(diag(P));
  %  Gain computation.
  G=P*C'*invSv;
  %  Save the gains.
  Gs(:,i)=G;
  %  Kalman filter simulation.
  xh(:,i+1)=xh(:,i)+dt1*(A*xh(:,i)+G*(y-C*xh(:,i)));
  end
%  Define the long sampling time and the number of time steps.
dt2=0.1;
n2=240;
%  Generate the plant and measurement noises for use in the simulation.
sigw=sqrt(Sw/dt2);
sigv=sqrt(Sv/dt2);
%  Perform the plant simulation, the gain computation, and the Kalman
%    filter simulation.
for i=n1+1:n1+n2,
  %  Plant simulation.
  y=C*x(:,i)+sigv*randn(1);
  x(:,i+1)=x(:,i)+dt2*(A*x(:,i)+B*sigw*randn(1));
  %  Riccati solution.
  P=P+dt2*(P*A'+A*P+B*Sw*B'-P*C'*invSv*C*P);
  %  Save the standard deviations of the state estimation errors.
  Sd(:,i)=sqrt(diag(P));
  %  Gain computation.
  G=P*C'*invSv;
  %  Save the gains.
  Gs(:,i)=G;
  %  Kalman filter simulation.
  xh(:,i+1)=xh(:,i)+dt2*(A*xh(:,i)+G*(y-C*xh(:,i)));
  end
%  Define the time vector for use in the state plots.
t=[0:dt1:n1*dt1 n1*dt1+dt2:dt2:n1*dt1+n2*dt2];
%  Plot the states and the estimated states.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',12)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(2,2,1)
plot(t, x(1,:), 'r-', t, xh(1,:), 'r--')
axis([0 25 0 15000])
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 -500 250])
legend('Actual', 'Estimate')
xlabel('Time (sec)')
ylabel('Range rate')
grid
subplot(2,2,3)
%  Define the time vector for use in the gain and standard deviation plots.
t=[dt1:dt1:n1*dt1 n1*dt1+dt2:dt2:n1*dt1+n2*dt2];plot(t, Gs)
%  Plot the gains and standard deviations of the state estimation errors.
plot(t, Gs(1,:), 'r-', t, Gs(2,:), 'r--')
axis([0 25 0 10])
legend('{\itG}_1', '{\itG}_2')
xlabel('Time (sec)')
ylabel('Gains')
grid
subplot(2,2,4)
plot(t, Sd(1,:), 'r-', t, Sd(2,:), 'r--')
axis([0 25 0 1000])
legend('Range', 'Range rate')
xlabel('Time (sec)')
ylabel('Standard deviation')
grid
