%  Example 7.3.  Time-varying Kalman gain computation.
clear
%  Define the time vector for use in generating samples of the gain.
dt=0.05;
tm=3;
t=0:dt:tm;
%  Compute the Kalman gain using the analytic formula given in Example 7.3.
G=(3*exp(-2*t)+9*exp(2*t))./(-exp(-2*t)+9*exp(2*t));
%  Plot the Kalman gain generated using the analytic formula.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
plot(t,G)
axis([0 tm 0 2])
xlabel('Time (sec)')
ylabel('Gain')
grid
%
%  The same gain can be computed using the Ricatti equation.
%  The Ricatti solution is generated as a check of the analytical 
%  gain solution.
%
%  Define the plant model.
A=-1; Bw=1; Bu=1; C=1;
%  Define the spectral densities of the plant and measurement noises.
Sw=3; Sv=1;
%  Allocate space for saving the Riccati solution and the Kalman gain.
P=zeros(1,61);
Gc=zeros(1,61);
%  Initialize the Riccati solution.
%  The estimation error covariance matrix is plant state variance.
%    Note:  Solving for this variance gives 1.5.
P0=lyap(A,Bw'*Sw*Bw);
P(1)=P0;
%  Compute the first Kalman gain.
Gc(1)=P(1)*C/Sv;
for k=1:tm/dt,
  %  Solve the Riccati equation.
  P(k+1)=P(k)+dt*(2*P(k)*A+Bw*Bw*Sw-P(k)*P(k)*C*C/Sv);
  %  Solve for the Kalman gain.
  Gc(k+1)=P(k+1)*C/Sv;
end
%  Plot the Kalman gain generated via Riccati equation solution.
figure(2)
clf
plot(t,Gc)
axis([0 tm 0 2])
xlabel('Time (sec)')
ylabel('Gain')
grid
