RobustStats.hpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 3.0 of the License, or
//  any later version.
//
//  The GNSSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================
 
#ifndef GNSSTK_ROBUSTSTATS_HPP
#define GNSSTK_ROBUSTSTATS_HPP
 
//------------------------------------------------------------------------------------
// system includes
#include <cmath>
#include <string>
 
// GNSSTk
#include "Exception.hpp"
 
namespace gnsstk
{
//------------------------------------------------------------------------------------
#define ABSOLUTE(x) ((x) < T() ? -(x) : (x))
 
#define RobustTuningT (1.5) // or 1.345
 
#define RobustTuningA (0.778) // or 0.67
 
#define RobustTuningE (0.6745) // so MAD of a std normal dist is StdDev
 
   //---------------------------------------------------------------------------------
 
 
   //--------------------------------------------------------------------------------
   // quick sort, for use by robust statistics routines
 
   template <typename T> int Qsort_compare(const T& a, const T& b)
   {
      if (a > b)
      {
         return 1;
      }
      else if (a < b)
      {
         return -1;
      }
      else
      {
         return 0;
      }
   }
 
   template <typename T>
   void insert(T *sa, int na,
               int (*comp)(const T& , const T& ) = gnsstk::Qsort_compare)
   {
      int i, j;
      T stemp;
      for (i = 1; i < na; i = i + 1)
      { // insert the i-th element into the sorted array
         stemp = sa[i];
         j     = i - 1; // find where it goes
         while ((j >= 0) && (comp(stemp, sa[j]) < 0))
         {
            sa[j + 1] = sa[j];
            j         = j - 1;
         }
         sa[j + 1] = stemp;
      }
   } // end insert sort
 
   template <typename T>
   void QSort(T *sa, int na,
              int (*comp)(const T& , const T& ) = gnsstk::Qsort_compare)
   {
      int i, j, nr;
      T stemp, spart;
      while (1)
      {
         if (na < 8)
         { // use insert sort for small arrays
            insert(sa, na, comp);
            return;
         }
         spart = sa[na / 2]; // pick middle element as pivot
         i     = -1;
         j     = na;
         while (1)
         {
            do
            { // find first element to move right
               i = i + 1;
            } while (comp(sa[i], spart) < 0);
            do
            { // find first element to move left
               j = j - 1;
            } while (comp(sa[j], spart) > 0);
            // if the boundaries have met,
            // through paritioning,
            if (i >= j)
            {
               break;
            }
            // swap i and j elements
            stemp = sa[i];
            sa[i] = sa[j];
            sa[j] = stemp;
         }
         nr = na - i;
         if (i < (na / 2))
         {                      // now sort each partition
            QSort(sa, i, comp); // sort left side
            sa = &sa[i];        // sort right side here
            na = nr;            // memsort(&(sa[i]),nr,comp);
         }
         else
         {
            QSort(&(sa[i]), nr, comp); // sort right side
            na = i;
         }
      }
   } // end QSort
 
   template <typename T, typename S>
   void insert(T *sa, S *pa, int na,
               int (*comp)(const T& , const T& ) = gnsstk::Qsort_compare)
   {
      int i, j;
      T stemp;
      S ptemp;
      for (i = 1; i < na; i = i + 1)
      { // insert the i-th element into the sorted array
         stemp = sa[i];
         ptemp = pa[i];
         j     = i - 1; // find where it goes
         while ((j >= 0) && (comp(stemp, sa[j]) < 0))
         {
            sa[j + 1] = sa[j];
            pa[j + 1] = pa[j];
            j         = j - 1;
         }
         sa[j + 1] = stemp;
         pa[j + 1] = ptemp;
      }
   } // end insert sort
 
   template <typename T, typename S>
   void QSort(T *sa, S *pa, int na,
              int (*comp)(const T& , const T& ) = gnsstk::Qsort_compare)
   {
      int i, j, nr;
      T stemp, spart;
      S ptemp;
      while (1)
      {
         if (na < 8)
         { // use insert sort for small arrays
            insert(sa, pa, na, comp);
            return;
         }
         spart = sa[na / 2]; // pick middle element as pivot
         i     = -1;
         j     = na;
         while (1)
         {
            do
            { // find first element to move right
               i = i + 1;
            } while (comp(sa[i], spart) < 0);
            do
            { // find first element to move left
               j = j - 1;
            } while (comp(sa[j], spart) > 0);
            // if the boundaries have met,
            // through paritioning,
            if (i >= j)
            {
               break;
            }
            // swap i and j elements
            stemp = sa[i];
            ptemp = pa[i];
            sa[i] = sa[j];
            pa[i] = pa[j];
            sa[j] = stemp;
            pa[j] = ptemp;
         }
         nr = na - i;
         if (i < (na / 2))
         {                          // now sort each partition
            QSort(sa, pa, i, comp); // sort left side
            sa = &sa[i];            // sort right side here
            pa = &pa[i];
            na = nr; // memsort(&(sa[i]),nr,comp);
         }
         else
         {
            QSort(&(sa[i]), &(pa[i]), nr, comp); // sort right side
            na = i;
         }
      }
   } // end QSort
 
   template <typename T> T errfc(T x)
   {
      T t, z, ret;
      z = ::fabs(x);
      t = T(1) / (T(1) + z / T(2));
      ret =
         t *
         ::exp(-z * z - 1.26551223 +
               t * (1.00002368 +
                    t * (0.37409196 +
                         t * (0.09678418 +
                              t * (-0.18628806 +
                                   t * (0.27886807 +
                                        t * (-1.13520398 +
                                             t * (1.48851587 +
                                                  t * (-0.82215223 +
                                                       t * 0.17087277)))))))));
      return (x >= T(0) ? ret : T(2) - ret);
   } // end errfc
 
   template <typename T> T normalCDF(T m, T s, T x)
   {
      if (s == T(0))
      {
         return T(0);
      }
      T arg = (x - m) / (::sqrt(T(2.0)) * s);
      return (T(1) - errfc<T>(arg) / T(2));
   } // end normal CDF
 
   double ADtest(double *xd, const int nd, double m, double s,
                 bool save_flag = true);
 
   //--------------------------------------------------------------------------------
   namespace Robust
   {
      template <typename T> T Median(T *xd, const int nd, bool save_flag = true)
      {
         if (!xd || nd < 2)
         {
            Exception e("Invalid input");
            GNSSTK_THROW(e);
         }
 
         try
         {
            int i;
            T med, *save = NULL;
 
            if (save_flag)
            {
               save = new T[nd];
               if (!save)
               {
                  Exception e("Could not allocate temporary array");
                  GNSSTK_THROW(e);
               }
               for (i = 0; i < nd; i++)
                  save[i] = xd[i];
            }
 
            QSort(xd, nd);
 
            if (nd % 2)
            {
               med = xd[(nd + 1) / 2 - 1];
            }
            else
            {
               med = (xd[nd / 2 - 1] + xd[nd / 2]) / T(2);
            }
 
            // restore original data from temporary
            if (save_flag)
            {
               for (i = 0; i < nd; i++)
                  xd[i] = save[i];
               delete[] save;
            }
 
            return med;
         }
         catch (Exception &e)
         {
            GNSSTK_RETHROW(e);
         }
 
      } // end Median
 
      template <typename T>
      void Quartiles(const T *xd, const int nd, T& Q1, T& Q3)
      {
         if (!xd || nd < 2)
         {
            Exception e("Invalid input");
            GNSSTK_THROW(e);
         }
 
         int q;
         if (nd % 2)
         {
            q = (nd + 1) / 2;
         }
         else
         {
            q = nd / 2;
         }
 
         if (q % 2)
         {
            Q1 = xd[(q + 1) / 2 - 1];
            Q3 = xd[nd - (q + 1) / 2];
         }
         else
         {
            Q1 = (xd[q / 2 - 1] + xd[q / 2]) / T(2);
            Q3 = (xd[nd - q / 2] + xd[nd - q / 2 - 1]) / T(2);
         }
      } // end Quartiles
 
      template <typename T>
      T MedianAbsoluteDeviation(T *xd, int nd, T& M, bool save_flag = true)
      {
         int i;
         T mad, *save;
 
         if (!xd || nd < 2)
         {
            Exception e("Invalid input");
            GNSSTK_THROW(e);
         }
 
         // store data in a temporary array
         if (save_flag)
         {
            save = new T[nd];
            if (!save)
            {
               Exception e("Could not allocate temporary array");
               GNSSTK_THROW(e);
            }
            for (i = 0; i < nd; i++)
               save[i] = xd[i];
         }
 
         // get the median (don't care if xd gets sorted...)
         M = Median(xd, nd, false);
 
         // compute xd=abs(xd-M)
         for (i = 0; i < nd; i++)
            xd[i] = ABSOLUTE(xd[i] - M);
 
         // sort xd in ascending order
         QSort(xd, nd);
 
         // find median and normalize to get mad
         mad = Median(xd, nd, false) / T(RobustTuningE);
 
         // restore original data from temporary
         if (save_flag)
         {
            for (i = 0; i < nd; i++)
               xd[i] = save[i];
            delete[] save;
         }
 
         return mad;
 
      } // end MedianAbsoluteDeviation
 
      template <typename T> T MAD(T *xd, int nd, T& M, bool save_flag = true)
      {
         return MedianAbsoluteDeviation(xd, nd, M, save_flag);
      }
 
      // TD return a sigma - just form RMS(xd-mest)
      template <typename T>
      T MEstimate(const T *xd, int nd, const T& M, const T& MAD, T *w = NULL)
      {
         try
         {
            T tv, m, mold, sum, sumw, *wt, weight;
            T tol = 0.000001;
            int i, n, N = 10; // N is the max number of iterations
 
            if (!xd || nd < 2)
            {
               Exception e("Invalid input");
               GNSSTK_THROW(e);
            }
 
            tv = T(RobustTuningT) * MAD;
            n  = 0;
            m  = M;
            do
            {
               mold = m;
               n++;
               sum = sumw = T();
               for (i = 0; i < nd; i++)
               {
                  if (w)
                  {
                     wt = &w[i];
                  }
                  else
                  {
                     wt = &weight;
                  }
                  *wt = T(1);
                  if (xd[i] < m - tv)
                  {
                     *wt = -tv / (xd[i] - m);
                  }
                  else if (xd[i] > m + tv)
                  {
                     *wt = tv / (xd[i] - m);
                  }
                  sumw += (*wt);
                  sum += (*wt) * xd[i];
               }
               m = sum / sumw;
 
            } while (T(ABSOLUTE((m - mold) / m)) > tol && n < N);
 
            return m;
         }
         catch (Exception& e)
         {
            GNSSTK_RETHROW(e);
         }
 
      } // end MEstimate
 
      int RobustPolyFit(double *xd, const double *td, int nd, int n, double *c,
                        double *w = NULL);
 
      void StemLeafPlot(std::ostream& os, double *xd, long nd,
                        const std::string& msg = std::string(""));
 
      void Quantiles(double *xd, long nd);
 
   } // namespace Robust
 
 
} // end namespace gnsstk
 
#undef ABSOLUTE
 
#endif