polyfit.hpp

一个gps小工具包

#pragma ident "$Id: PolyFit.hpp 96 2006-08-22 17:15:20Z btolman $"



/**
 * @file PolyFit.hpp
 * Least squares fit using a polynomial model.
 */
 
#ifndef GPSTK_POLYFIT_HPP
#define GPSTK_POLYFIT_HPP

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk 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 2.1 of the License, or
//  any later version.
//
//  The GPSTk 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 GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software 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.
//
//=============================================================================






#include "Matrix.hpp"

namespace gpstk
{
   /** @addtogroup math */
   //@{

/**
 * Compute a polynomial fit of the form sum[X(i)*t**i] = d, that is solve for
 * coefficients X given a set of data pairs (t,d). The dimension of X is n, the
 * degree of the polynomial.
 * @code
 * unsigned int i,n=4;
 * double dat[17]={...}, times[17]={...};
 * PolyFit<double> PF(n);
 * for(i=0; i<17; i++)
 *    PF.Add(dat[i],times[i]);
 * 
 * cout << "Solution vector: " << PF.Solution() << endl;
 * cout << "Covariance matrix: " << PF.Covariance() << endl;
 * for(i=0; i<17; i++)
 *    cout << times[i] << " " << dat[i] << " " << PF.Evaluate(times[i]) << endl;
 * @endcode
 */
   template <class T>
   class PolyFit
   {
   public:
         /// Empty constructor
      PolyFit() : n_(0), Inverted(false), Singular(true), Npts(0) {}

         /// Constructor given an initial size.
      PolyFit(unsigned int n) : n_(n), Inverted(false), Singular(true), Npts(0)
      {
         InfData.resize(n_,T(0));
         X.resize(n_);
         InfMatrix.resize(n_,n_,T(0));
         Cov.resize(n_,n_);
      }

         /** Reset the estimation, i.e. restart with new data, with new dimension.
          * Default dimension is 0, meaning do not change dimension.
          */
      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;
      }

         /// Add a single (optional: weighted) datum to the estimation.
      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;
      }

         /// Add a gpstk::Vector of data to the estimation.
      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;
      }

         /// Add a std::vector of data to the estimation.
      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;
      }

         /// Evaluate the polynomial at the given time; singular problems return zero.
      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;
      }

         /// Evaluate the polynomial at a Vector of times;
         /// singular problems return an empty vector.
      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;
      }

         /// is the problem singular?
      inline bool isSingular(void) { Solve(); return Singular; }
         /// get the solution vector (coefficients)
      inline Vector<T> Solution(void) { Solve(); return X; }
         /// get the covariance matrix
      inline Matrix<T> Covariance(void) { Solve(); return Cov; }
         /// get the degree of the polynomial
      inline unsigned int Degree(void) const { return n_; }
         /// get the number of data points processed
      inline unsigned int N(void) const { return Npts; }

   private:
         /// Invert the equation
      inline void Solve(void)
      {
         if(Inverted) return;
         try { Cov=inverse(InfMatrix); }
         catch (gpstk::Exception& e) { Singular=true; return; }
         Singular = false;
         X = Cov * InfData;
         Inverted = true;
      }

         /// degree of polynomial to be fit (dimension of state).
      unsigned int n_;
         /// number of data points added to the estimation so far.
      unsigned int Npts;
         /// information matrix = inverse(Cov) = sum[transpose(P)*P], P=partials.
      Matrix<T> InfMatrix;
         /// information data = inverse(Cov)*X = sum[transpose(P)*data].
      Vector<T> InfData;
         /// flag indicating problem has been inverted.
      bool Inverted;
         /// flag indicating problem is singular.
      bool Singular;
         /// State vector (array of polynomial coefficients) of size n_.
      Vector<T> X;
         /// Covariance matrix
      Matrix<T> Cov;

   }; // end class PolyFit

   //@}

}  // namespace gpstk

#endif