PolyFit.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_POLYFIT_HPP
#define GNSSTK_POLYFIT_HPP
 
#include "Matrix.hpp"
 
namespace gnsstk
{
 
 
   template <class T>
   class PolyFit
   {
   public:
      PolyFit() : n_(0), Npts(0), Inverted(false), Singular(true) {}
      PolyFit(unsigned int n) : n_(n), Npts(0), Inverted(false), Singular(true)
      {
         InfData.resize(n_,T(0));
         X.resize(n_);
         InfMatrix.resize(n_,n_,T(0));
         Cov.resize(n_,n_);
      }
 
      void Reset(unsigned int n=0)
      {
         if(n != 0 && n_ != n) {
            InfData.resize(n,T(0));
            X.resize(n);
            InfMatrix.resize(n,n,T(0));
            Cov.resize(n,n);
            n_ = n;
         }
         Npts = 0;
         InfData = T(0);
         X = T(0);
         InfMatrix = T(0);
         Cov = T(0);
         Singular = true;
         Inverted = false;
      }
 
      void Add(T d, T t, T w=T(1))
      {
            //             n_-1
            // Equation is sum[t^i * X(i)] = d     OR    P * X = d
            //             i=0                          1xn_ n_  1
         Vector<T> P(n_);
         T tt=T(1);
         for(size_t i=0; i<n_; i++) { P(i)=tt; tt *= t; }
         Npts++;
         Matrix<T> PP;
         PP = outer(P,P);
         PP *= w;
         InfMatrix += PP;
         P *= d*w;
         InfData += P;
         Inverted = false;
      }
 
      void Add(const Vector<T>& d, const Vector<T>& t)
      {
         size_t m=d.size();
         if(t.size() < m) m=t.size();
 
         Matrix<T> P(m,n_);
         Vector<T> D(d);
         D.resize(m);
 
         for(size_t j=0; j<m; j++) {
            T tt=T(1);
            for(size_t i=0; i<n_; i++) { P(j,i)=tt; tt *= t(j); }
         }
         Npts += m;
         Matrix<T> PTP,PT;
         PT = transpose(P);
         PTP = PT*P;
         InfMatrix += PTP;
         InfData += PT*D;
         Inverted = false;
      }
 
      void Add(const std::vector<T>& d, const std::vector<T>& t)
      {
         size_t m=d.size();
         if(t.size() < m) m=t.size();
 
         Matrix<T> P(m,n_);
         Vector<T> D;
         D.resize(m);
         for(int i=0; i<m; i++) D(i)=d[i];
 
         for(size_t j=0; j<m; j++) {
            T tt=T(1);
            for(size_t i=0; i<n_; i++) { P(j,i)=tt; tt *= t[j]; }
         }
         Npts += m;
         Matrix<T> PTP,PT;
         PT = transpose(P);
         PTP = PT*P;
         InfMatrix += PTP;
         InfData += PT*D;
         Inverted = false;
      }
 
      T Evaluate(T t)
      {
         if(n_ <= 0) { Singular=true; return T(0); }
         Solve();
         if(Singular) return T(0);
 
         T d,tn;
         d = X(0);
         tn = t;
         for(size_t i=1; i<X.size(); i++) {
            d += X(i)*tn;
            tn *= t;
         }
         return d;
      }
 
      Vector<T> Evaluate(const Vector<T>& Vt)
      {
         Vector<T> R;
         if(n_ <= 0) { Singular=true; return R; }
         Solve();
         if(Singular) return R;
 
         T tn;
         R = Vector<T>(Vt.size());
         for(size_t j=0; j<Vt.size(); j++) {
            R(j) = X(0);
            tn = Vt(j);
            for(size_t i=1; i<X.size(); i++) {
               R(j) += X(i)*tn;
               tn *= Vt(j);
            }
         }
         return R;
      }
 
      inline bool isSingular(void) { Solve(); return Singular; }
      inline Vector<T> Solution(void) { Solve(); return X; }
      inline Matrix<T> Covariance(void) { Solve(); return Cov; }
      inline unsigned int Degree(void) const { return n_; }
      inline unsigned int N(void) const { return Npts; }
 
   private:
      inline void Solve(void)
      {
         if(Inverted) return;
         try { Cov=inverse(InfMatrix); }
         catch (gnsstk::Exception& e) { Singular=true; return; }
         Singular = false;
         X = Cov * InfData;
         Inverted = true;
      }
 
      unsigned int n_;
      unsigned int Npts;
      Matrix<T> InfMatrix;
      Vector<T> InfData;
      bool Inverted;
      bool Singular;
      Vector<T> X;
      Matrix<T> Cov;
 
   }; // end class PolyFit
 
 
}  // namespace gnsstk
 
#endif