%
%     UD recursive least squares algorithm (Bierman)
%
%  [ th, d, u ] = rls( y, phi, th, d, u, npar, lam )
%
%   y    latest output y(t)
%   phi  [y(t-1) y(t-2) ...  u(t-k-1) u(t-k-2)...] ' col vector
%        -user must shift down  prior to each call to rls
%
%   th   parameter vector same dimension as phi - col vector
%   d    diagonal matrix (npar) elements
%   u    upper triangular matrix (npar)(npar-1)/2 elements
%
%   npar dimension of th, phi
%   lam  forgetting factor (.95 to .998)
%
%--------------------------------------------------------------
%
function [ th, d, u ] = rls( y, phi, th, d, u, npar, lam )
%
  if npar ~= length( phi ),
    error('no. of parameters must equal no. of elements in phi')
    return
  end
% set phi,th to be col vector
  th = th(:);
  phi = phi(:);
%   k    Kalman gain
  k = [ zeros( 1, npar ) ];
  e = y;
%   e    prediction error
  e = e - th' * phi;
  fj = phi(1);
  vj = d(1) * fj;
  k(1) = vj;
  alj = 1.0 + vj * fj;
  d(1) = d(1) / alj;
%
  if npar == 1
    th(1) = th(1) + e * k(1) / alj;
    return
  end
  kf = 0; ku = 0;
  for j = 2:npar
    fj = phi(j);
    for i = 1:j-1
      kf = kf + 1;
      fj = fj + phi(i) * u(kf);
    end
    vj = fj * d(j);
    k(j) = vj;
    ajlast = alj;
    alj = ajlast + vj * fj;
    d(j) = d(j) * ajlast / alj / lam;
    pj = - fj / ajlast;
    for i = 1:j-1
      ku = ku + 1;
      w = u(ku) + k(i) * pj;
      k(i) = k(i) + u(ku) * vj;
      u(ku) = w;
    end
  end
  th = th + k' * ( e / alj );
return
%-----------------------------------------------------------
