%
%  This funtion implements the kalman
%  filter and simulates its use in determining
%  and AR model with (assumed) known covariance
%
%   Nsteps = the number of time steps for the
%            simulation
%
  Nsteps = 500;
%
%  We assume a state variable model
%
%  x(k+1) = Phi(k) x(k) + Gamma(k)u(k)
%  y(k) = H(k) x(k) + v(k)
%
%  where Q(k) is the autocovariance of u(k), 
%    and R(k) is the autocovariance of v(k)
%
%  for estimating the parameters of an AR 
%    model, H(k) = [y(k-1) y(k-2) ]
%           x(k) = [-a1 -a2]'
%
   R = 0.05;
   Qscale = 0.05;
   Q = Qscale*eye(2,2);
   v = sqrt(R)*randn(1,Nsteps);
%
%  loop through the time steps and estimate the
%  output at each time step.
%
%  y(k) = -a1y(k-1)-a2y(k-2)+ v(k)
%
%  to initialize, we will assume all previous y 
%    values are 0.0, the error matrix is diagonal,
%    and our initial guess for the parameters is 0
%
  x = [0; 0];
  H = [0 0];
  xhat = [0; 0];
  P = eye(2);
%
%  initially, assume Phi is diagonal;
%
  Phi = eye(2);
  Gamma = eye(2);
%
% initialize the output
%
  true_out = zeros(2,Nsteps);
  est_out = zeros(2,Nsteps);
%
  for i=1:Nsteps
%
%  get the new output at this time
%
%      a1 = (0.4+0.5*cos(3*pi*i/200));
%      a2 = (0.5+0.3*sin(2*pi*i/200));
%      a2 = (1.5+0.3*sin(2*pi*i/200));
     a1 = 0.7;
     a2 = 0.12;
    true_out(:,i) = [-a1; -a2];
    y = H*[-a1; -a2] + v(i);
%
%   Now update the estimate of the parameters
%
    K = P*H';
    Re = H*P*H'+R;
    x = Phi*xhat + Phi*K*inv(Re)*(y-H*xhat);
    P = Phi*P*Phi'+Gamma*Q*Gamma' - Phi*K*inv(Re)*K'*Phi';
    est_out(:,i) = x;
    xhat = x;
    H = [y H(1)];
  end;
%
%  now make the output plots
%
  figure;
  t = [1:Nsteps];
  hold on
  plot(t,true_out(1,:)','r--','LineWidth',3);
  plot(t,est_out(1,:)','r-')
  plot(t,true_out(2,:)','b--','LineWidth',3);
  plot(t,est_out(2,:)','b-');
  hold off
  legend('a_1','a_1 (est)','a_2','a_2 (est)');
  grid;
  xlabel('Sample (n)');
  ylabel('Amplitude of a_1 and a_2');
  title(['Nsteps = ',num2str(Nsteps),', R = ', num2str(R),', Q = ', num2str(Qscale)]);
%
  return;
%
%
  