%                             raimst.m
%  Scope:   This MATLAB macro determines availability and failure (detection)
%           flags for the Receiver Autonomous Integrity Monitoring (RAIM)
%           baseline standard algorithm (constant alarm rate algorithm) [1].
%  Remark:  It is assumed that at least five line-of-sight unit vectors 
%           are specified as input.
%  Usage:   [avail,testst] = raimst(g,threshold,arpceil,epsvec)
%  Description of parameters:
%           g         - input, line-of-sight unit vectors, each vector is
%                       stored in another row (it should be 3 columns)
%           threshold - input, threshold value
%           arpceil   - input, arp ceiling value; arp = approximate radial
%                       error protected
%           epsvec    - input, error vector in true range
%           avail     - output, availability flag
%                       =  0  admissible/available
%                       =  1  inadmissible/unavailable
%           testst    - output, failure (detection) flag
%                       =  0  no failure detection
%                       =  1  failure detection
%  Reference: 
%           [1] Brown, R. G., A baseline RAIM scheme and a note on the
%               equivalence of three RAIM methods. Proceedings of the National
%               Technical Meeting, Institute of Navigation, San Diego, CA,
%               Jan. 27-29, 1992, pp. 127-137.
%  Last update:  08/02/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [avail,testst] = raimst(g,threshold,arpceil,epsvec)

[nrow,ncol] = size(g);
if (ncol ~= 3) | (nrow < 5)
   error('Error -  RAIMST; check the unit line of sight vectors');
end
unitvec = ones(nrow,1);

h = [g unitvec];                              %  form the matrix  H
a = (inv(h'* h)) * h';
b = h * a;

for k = 1:nrow
   slope(k) = sqrt((nrow - 4) * ( a(1,k)*a(1,k) + a(2,k)*a(2,k) ) / ...
              (1. - b(k,k)));
end
slmax = max(slope);
arp = slmax * threshold;
if  arp > arpceil                             %  availability flag
   avail = 1;                                 %  inadmissible/unavailable
else
   avail = 0;                                 %  admissible/available
end

w = ( eye(nrow) - b ) * epsvec';
sse = w' * w;
s = sqrt(sse / (nrow - 4));
if  s > threshold                             %  test statistic
   testst = 1;                                %  failure detection
else
   testst = 0;                                %  no failure detection
end