raimfd.m

GPS TOOLBOX包含以下内容： 1、GPS相关常量和转换因子； 2、角度变换； 3、坐标系转换： &#61656 点变换； &#61656 矩阵变换； &#61656 向量变换

%                             raimfd.m
%  Scope:  This MATLAB macro determines the outcome of a fault detection 
%          algorithm for the Receiver Autonomous Integrity Monitoring (RAIM)
%          [1] by using parity vector method; only a time step is processed.
%          It is assumed that at least five line-of-sight unit vectors are 
%          specified as input.
%  Usage:  counts = raimfd(g,pfa,sigma,pmd)
%  Description of parameters:
%          g       - input, line-of-sight unit vectors, each vector is
%                    stored in another row (it should be 3 columns)
%          pfa     - input, probability of false alarm
%          sigma   - input, standard deviation of measurement noise
%                    (in meters)
%          pmd     - input, probability of missed detection
%          counts  - output, counters
%                    counts(1) =  false alarm counter
%                    counts(2) =  true alarm counter
%                    counts(3) =  missed detection counter
%                    counts(4) =  normal operation counter
%                    counts(5) =  unavailable counter
%  Reference: 
%          [1] Van Graas, F., Farrell, J. L., Baseline fault detection and
%              exclusion algorithm. Institute of Navigation, Proceedings of 
%              the 49th Annual Meeting, Cambridge, MA, June 21-23, 1993,
%              pp. 413-420.
%  Last update:  08/05/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function   counts = raimfd(g,pfa,sigma,pmd)

%  Initialization of the counters 

counts(1) = 0;                       %  false alarm counter
counts(2) = 0;                       %  true alarm counter
counts(3) = 0;                       %  missed detection counter
counts(4) = 0;                       %  normal operation counter
counts(5) = 0;                       %  unavailable counter

[nrow,ncol] = size(g);
if  ncol ~= 3
   error('Error -  RAIMFD; check the unit line of sight vectors');
end
nmeas = nrow;                        %  number of measurements
unitvec = ones(nmeas,1);
h = [g unitvec];                     %  form the matrix  H

%  Determine detection thresholds

if  nmeas == 5
   tdnmeas = sqrt(2) * sigma * erfinv(pfa);
elseif  nmeas > 5
   tdnmeas = sqrt(2) * sigma * erfinv(pfa/nmeas);
else
   counts(5) = 1;                    %  unavailable counter is set
   exit;
end

%  Determine minimum detectable biases

bias = sqrt(2) * sigma * erfinv(pmd+pmd);
mum = tdnmeas + bias;

%   Determine noise radius

invhth = inv(h'*h);
hdop = sqrt(invhth(1,1) + invhth(2,2));
rnoise = sqrt(2) * erfinv(1.e-3) * sigma * hdop;
h1 = invhth*h';

[q,r] = qr(h);
qxqp = q';                              %  matrix [qx  qp]'
qp = qxqp(5:nmeas,:);                   %  parity matrix  qp'

z = sigma * rand(nmeas,1);              %  generate measurement noise
x = h1 * z;                             %  true position error
poserror = norm(x(1:2));

if nmeas == 5

   for i = 1:nmeas                      %  calculate the worst bias error
      b = mum / qp(1,i);
      deltax = h1(:,i) * b;
      rh(i) = norm(deltax(1:2));
   end

   rbias = max(rh(1:nmeas));
   rp = rnoise + rbias;                 %  protection radius
   p = qp(1,1:nmeas) * z;               %  parity scalar

   if  abs(p) < tdnmeas & poserror > rp
       counts(3) = 1;                   %  missed detection
   elseif   abs(p) < tdnmeas & poserror < rp
       counts(4) = 1;                   %  normal operation
   elseif   abs(p) > tdnmeas & poserror < rp
       counts(1) = 1;                   %  false alarm
   else
       counts(2) = 1;                   %  true alarm
   end

else               %  for  nmeas > 5  

   flag = 0;       %  no alarm flag

   for i = 1:nmeas

      qptemp = qp;
      qptemp(1:nmeas-4,1) = qp(1:nmeas-4,i);
      qptemp(1:nmeas-4,i) = qp(1:nmeas-4,1);

      [qtemp,rstemp] = qr(qptemp);

      rtemp = rstemp;
      rtemp(1:nmeas-4,1) = rstemp(1:nmeas-4,i);
      rtemp(1:nmeas-4,i) = rstemp(1:nmeas-4,1);

      b = mum / rtemp(1,i);
      deltax = h1(:,i) * b;
      rh(i) = norm(deltax(1:2));
      p = rtemp(1,1:nmeas) * z;         %  parity scalar

      if abs(p) > tdnmeas
         flag = flag + 1;
      end

   end

   rbias = max(rh(1:nmeas));
   rp = rnoise + rbias;                 %  protection radius

   if flag >= 1
      if poserror > rp
         counts(2) = 1;                 %  true alarm
      else
         counts(1) = 1;                 %  false alarm
      end
   else
      if poserror > rp
         counts(3) = 1;                 %  missed detection
      else
         counts(4) = 1;                 %  normal operation
      end
   end

end