mreast.m

GPS software toolbox for GPS receiver development

%                             mreast.m
%  Scope:   This MATLAB macro executes measurement reasonableness test for a 
%           given scalar measurement.
%  Usage:   alpha = mreast(n,ud,h,r,dz,test)
%  Description of parameters:
%           n     - input, integer scalar, dimension of state vector,
%                   n is greater than or equal to 2
%           ud    - input, real array with  n*(n+1)/2  elements, containing
%                   the  U-D  factors of the covariance matrix; U is unit
%                   upper triangular and  D  elements are stored on the dia-
%                   gonal
%           h     - input, real array with  n  elements, storing the observa-
%                   tion coefficients (measurement matrix) for one measurement
%           r     - input, real scalar, measurement variance
%           dz    - input, real scalar, measurement residual
%           test  - input, real scalar, constant used to test the validity 
%                   of the measurement, e.g.  test = 25  corresponds to  5
%                   sigma  verification
%           alpha - output, real scalar, innovations variance; if the meas-
%                   urement residual fails the test then the output is nega-
%                   tive  alpha
%  Constraint:  n >= 2 
%  Last update:  06/29/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  alpha = mreast(n,ud,h,r,dz,test)

alpha = ud(1) * h(1) * h(1) + r;
if (n >= 2) 
   jj = 1;
   for j = 2:n
      temp = h(j);
      jm1 = j - 1;
      for k = 1:jm1
         temp = h(k) * ud(jj+k) + temp;
      end   
      jj = jj + j;
      alpha = ud(jj) * temp * temp + alpha;
   end
end 

if (dz * dz) > (test * alpha) 
   alpha = - alpha;
end