%                                mudm.m  
%  Scope:   This MATLAB macro implements the discrete Kalman filter measurement
%           updating using Bierman's  U-D  measurement update algorithm. Only a
%           scalar measurement is processed, the  U-D  covariance factors and 
%           the state estimate (optional) are updated. The measurement is an 
%           input when the state is updated.
%  Usage:   [ud,f,g,alpha] = mudm(n,ud,r,h,index)
%           [ud,h,g,alpha] = mudm(n,ud,r,h,index) , when the input  h  
%                            is destroyed
%  Description of parameters:
%           n     - input, integer scalar, dimension of state vector,
%                   n is greater than or equal to 2
%           ud    - input/output, real array of length  (n+1)*(n+2)/2 .
%                   The first  n*(n+1)/2  elements store the input/output
%                   U-D  factors; the  D  elements are stored on diagonal.
%                   The last  n+1  elements store the input/output state
%                   vector  x  and the scalar value  z-h**(transpose)*
%                   x(a priori)  in  u((n+1)*(n+2)/2).
%                   If  index = 0  then the state  x  (and  z) need not
%                   be included and the dimension of the array should
%                   be  n*(n+1)/2 .
%           r     - input, real scalar, stores the measurement variance
%           h     - input, real array of length  (n+1) , stores the vector
%                   of measurement coefficients (first  n  elements) and
%                   h(n+1) = z  if  index = 0 . If  index = 0  the dimen-
%                   sion of the array  h  should be  n .
%           index - input, integer scalar, index of updating type
%                   = 0    then only the  U-D  factors are updated
%                          (the state is not updated)
%                   ~= 0   then  the  U-D  factors and state are updated
%           f     - output, real array of length  (n+1) ; the first  n
%                   elements contain the vector  u**(transpose)*h(input),
%                   and when index ~= 0  then  f(n+1)=(z-h**(transpose)*
%                   x(a priori))/alpha . If  index = 0  the dimension of
%                   the array  f  should be  n .
%           g     - output, real array of length  n , which contains
%                   the vector of unweighted Kalman gains; Kalman gain
%                   is  g/alpha.
%           alpha - output, real scalar, innovations variance of the
%                   measurement residual  (h**(transpose)*p(a priori)*h+r)
%  Remarks: The algorithm holds for  r = 0  (a perfect measurement) and the
%           code includes this case. One can use this algorithm with  r
%           negative to delete a previously processed data point. However,
%           one should note that data deletion is numerically unstable
%           and sometimes introduces numerical errors.
%  Constraint:   n >= 2 
%  Method:  The algorithm updates the columns of the  U-D  matrix, from
%           left to right, using  Bierman's algorithm; the state vector
%           is updated using the classical formula.
%  Reference:
%           [1] Bierman, G, J., Factorization methods for discrete sequential
%               estimation. Academic Press, New York, 1977.
%  Last update:  07/12/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [ud,f,g,alpha] = mudm(n,ud,r,h,index)

np1 = n + 1;
np2 = n + 2;
nn1 = n * np1 / 2;
     
if (index ~= 0)
   temp = h(np1);
   for k = 1:n
      temp = temp - h(k) * ud(nn1+k);
   end
   ud(nn1+np1) = temp;
end 

jjn = nn1;
for l = 2:n
   j = np2 - l;
   jj = jjn - j;
   temp = h(j);
   jm1 = j - 1;
   for k = 1:jm1
      temp = temp + ud(jj+k) * h(k);
   end
   f(j) = temp;
   g(j) = temp * ud(jjn);
   jjn = jj;
end

f(1) = h(1);
g(1) = ud(1) * f(1);
sum = r + g(1) * f(1);
gamma = 0.;
if (sum > 0.)
   gamma = 1. / sum;
end 
if (f(1) ~= 0.)
   ud(1) = ud(1) * r * gamma;
end

kj = 2;
for j = 2:n
   beta = sum;
   temp = g(j);
   sum = sum + temp * f(j);
   p = - f(j) * gamma;
   jm1 = j - 1;
   for k = 1:jm1
      s = ud(kj);
      ud(kj) = s + p * g(k);
      g(k) = g(k) + temp * s;
      kj = kj + 1;
   end
   if (temp ~= 0) 
      gamma = 1. / sum;
      ud(kj) = ud(kj) * beta * gamma;
   end 
   kj = kj + 1;
end
alpha = sum;

if (index ~= 0) 
   f(np1) = ud(nn1+np1) * gamma;
   for j = 1:n
      ud(nn1+j) = ud(nn1+j) + g(j) * f(np1);
   end
end