matrixfunctors.hpp

gps源代码

         /// Matrix U
      Matrix<T> U;
         /// Vector of singular values
      Vector<T> S;
         /// Matrix V (not transpose(V))
      Matrix<T> V;

   private:
      const size_t iterationMax;
   
      T SIGN(T a, T b)
         { 
            if (b >= T(0))
               return ABS(a);
            else
               return -ABS(a);
         }

   }; // end class SVD

/**
 * Performs the lower/upper triangular decomposition of a matrix PA = LU.
 * The results are put into the matricies L, U, and P (pivot), and sign
 * (representing even (positive) or odd (negative) row swaps.
 */
   template <class T>
   class LUDecomp
   {
   public:
      LUDecomp() {}        // why is there no constructor from ConstMatrixBase?

         /// Does the decomposition.
      template <class BaseClass>
      void operator() (const ConstMatrixBase<T, BaseClass>& m)
         throw (MatrixException)
         {
            if(!m.isSquare() || m.rows()<=1) {
               MatrixException e("LUDecomp requires a square, non-trivial matrix");
               GPSTK_THROW(e);
            }

            size_t N=m.rows(),i,j,k,n,imax;
            T big,t,d;
            Vector<T> V(N,T(0));

            LU = m;
            Pivot = Vector<int>(N);
            parity = 1;

            for(i=0; i<N; i++) {    // get scale of each row
               big = T(0);
               for(j=0; j<N; j++) {
                  t = ABS(LU(i,j));
                  if(t > big) big=t;
               }
               if(big <= T(0)) {    // m is singular
                  //LU *= T(0);
                  SingularMatrixException e("singular matrix!");
                  GPSTK_THROW(e);
               }
               V(i) = T(1)/big;
            }

            for(j=0; j<N; j++) {    // loop over columns
               for(i=0; i<j; i++) {
                  t = LU(i,j);
                  for(k=0; k<i; k++) t -= LU(i,k)*LU(k,j);
                  LU(i,j) = t;
               }
               big = T(0);          // find largest pivot
               for(i=j; i<N; i++) {
                  t = LU(i,j);
                  for(k=0; k<j; k++) t -= LU(i,k)*LU(k,j);
                  LU(i,j) = t;
                  d = V(i)*ABS(t);
                  if(d >= big) {
                     big = d;
                     imax = i;
                  }
               }
               if(j != imax) {
                  LU.swapRows(imax,j);
                  V(imax) = V(j);
                  parity = -parity;
               }
               Pivot(j) = imax;

               t = LU(j,j);
               if(t == 0.0) {       // m is singular
                  //LU *= T(0);
                  SingularMatrixException e("singular matrix!");
                  GPSTK_THROW(e);
               }
               if(j != N-1) {
                  d = T(1)/t;
                  for(i=j+1; i<N; i++) LU(i,j) *= d;
               }
            }
         }  // end LUDecomp()

         /** Compute inverse(m)*v, where *this is LUD(m), via back substitution
          * Solution overwrites input Vector v
          */
      template <class BaseClass2>
      void backSub(RefVectorBase<T, BaseClass2>& v) const
         throw (MatrixException)
      {
         if(LU.rows() != v.size()) {
            MatrixException e("Vector size does not match dimension of LUDecomp");
            GPSTK_THROW(e);
         }

         bool first=true;
         size_t N=LU.rows(),i,j,ii;
         T sum;

         // un-pivot
         for(i=0; i<N; i++) {
            sum = v(Pivot(i));
            v(Pivot(i)) = v(i);
            if(first && sum != T(0)) {
               ii = i;
               first = false;
            }
            else for(j=ii; j<i; j++) sum -= LU(i,j)*v(j);
            v(i) = sum;
         }
         // back substitution
         for(i=N-1; ; i--) {
            sum = v(i);
            for(j=i+1; j<N; j++) sum -= LU(i,j)*v(j);
            v(i) = sum / LU(i,i);
            if(i == 0) break;       // b/c i is unsigned
         }
      }  // end LUD::backSub

         /// compute determinant from LUD
      inline T det(void)
         throw(MatrixException)
      {
         T d(parity);
         for(size_t i=0; i<LU.rows(); i++) d *= LU(i,i);
         return d;
      }

         /** The matrix in LU-decomposed form: L and U together;
           * all diagonal elements of L are implied 1.
           */
         Matrix<T> LU;
         /// The pivot array
         Vector<int> Pivot;
         /// Parity
         int parity;

   }; // end class LUDecomp


   /**
    * Compute cholesky decomposition (upper triangular square root) of the
    * given matrix, which must be positive definite. Positive definite <=>
    * positive eigenvalues. Note that the UT sqrt is not unique, and that
    * m = U*transpose(U) (where U=UTsqrt(m)) only if m is symmetric.
    */
   template <class T>
   class Cholesky
   {
   public:
      Cholesky() {}

         /// @todo potential complex number problem!
      template <class BaseClass>
      void operator() (const ConstMatrixBase<T, BaseClass>& m)
         throw (MatrixException)
      {
         if(!m.isSquare()) {
            MatrixException e("Cholesky requires a square matrix");
            GPSTK_THROW(e);
         }

         size_t N=m.rows(),i,j,k;
         double d;
         Matrix<T> P(m);
         U = Matrix<T>(m.rows(),m.cols(),T(0));

         for(j=N-1; ; j--) {
            if(P(j,j) <= T(0)) {
               MatrixException e("Cholesky fails - eigenvalue <= 0");
               GPSTK_THROW(e);
            }
            U(j,j) = SQRT(P(j,j));
            d = T(1)/U(j,j);
            if(j > 0) {
               for(k=0; k<j; k++) U(k,j)=d*P(k,j);
               for(k=0; k<j; k++)
                  for(i=0; i<=k; i++)
                     P(i,k) -= U(k,j)*U(i,j);
            }
            if(j==0) break;      // since j is unsigned
         }

         // L does not = transpose(U);
         P = m;
         L = Matrix<T>(m.rows(),m.cols(),T(0));
         for(j=0; j<=N-1; j++) {
            if(P(j,j) <= T(0)) {
               MatrixException e("Cholesky fails - eigenvalue <= 0");
               GPSTK_THROW(e);
            }
            L(j,j) = SQRT(P(j,j));
            d = T(1)/L(j,j);
            if(j < N-1) {
               for(k=j+1; k<N; k++) L(k,j)=d*P(k,j);
               for(k=j+1; k<N; k++) {
                  for(i=k; i<N; i++) {
                     P(i,k) -= L(i,j)*L(k,j);
                  }
               }
            }
         }

      }  // end Cholesky::operator()

         /* Use backsubstition to solve the equation A*x=b where *this Cholesky
          * has been applied to A, i.e. A = L*transpose(L). The algorithm is in
          * two steps: since A*x=L*LT*x=b, first solve L*y=b for y, then solve
          * LT*x=y for x. x is returned as b.
          */
      template <class BaseClass2>
      void backSub(RefVectorBase<T, BaseClass2>& b) const
         throw (MatrixException)
      {
         if (L.rows() != b.size())
         {
            MatrixException e("Vector size does not match dimension of Cholesky");
            GPSTK_THROW(e);
         }
         size_t i,j,N=L.rows();
      
         Vector<T> y(b.size());
         y(0) = b(0)/L(0,0);
         for(i=1; i<N; i++) {
            y(i) = b(i);
            for(j=0; j<i; j++) y(i)-=L(i,j)*y(j);
            y(i) /= L(i,i);
         }
         // b is now x
         b(N-1) = y(N-1)/L(N-1,N-1);
         for(i=N-1; ; i--) {
            b(i) = y(i);
            for(j=i+1; j<N; j++) b(i)-=L(j,i)*b(j);
            b(i) /= L(i,i);
            if(i==0) break;
         }

      }  // end Cholesky::backSub

         /// Lower triangular and Upper triangular Cholesky decompositions
      Matrix<T> L, U;

   }; // end class Cholesky

   /**
    * Compute the Cholesky decomposition using the Cholesky-Crout algorithm,
    * which is very fast; if A is the given matrix we will get L, where A = L*LT. 
    * A must be symetric and positive definite. This is the usual case when
    * A comes from applying a Least Mean-Square (LMS) or Weighted Least 
    * Mean-Square (WLMS) method.
    */
   template <class T> 
   class CholeskyCrout : public Cholesky<T>
   {
   public:
       template <class BaseClass>
       void operator() (const ConstMatrixBase<T, BaseClass>& m) throw (MatrixException)
       {
           if(!m.isSquare()) {
               MatrixException e("CholeskyCrout requires a square matrix");
               GPSTK_THROW(e);
           }

           int N = m.rows(), i, j, k;
           double sum;
           (*this).L = Matrix<T>(N,N, 0.0);

           for(j=0; j<N; j++) {
               sum = m(j,j);
               for(k=0; k<j; k++ ) sum -= (*this).L(j,k)*(*this).L(j,k);
               if(sum > 0.0) {
                   (*this).L(j,j) = SQRT(sum);
               } else {
                   MatrixException e("CholeskyCrout fails - eigenvalue <= 0");
                   GPSTK_THROW(e);          
               }

               for(i=j+1; i<N; i++){
                   sum = m(i,j);
                   for(k=0; k<j; k++) sum -= (*this).L(i,k)*(*this).L(j,k);
                   (*this).L(i,j) = sum/(*this).L(j,j);
               }
           }

           (*this).U = transpose((*this).L);
       }

   }; // end class CholeskyCrout



/**
 * The Householder transformation is simply an orthogonal transformation
 * designed to make the elements below the diagonal zero. It applies to any
 * matrix.
 */
   template <class T>
   class Householder
   {
   public:
      Householder() {}

      /** Explicitly perform the transformation, one column at a time, without
      * actually constructing the transformation matrix. Let y be column k of the
      * input matrix. y can be zeroed below the diagonal as follows:
      * let sum=sign(y(k))*sqrt(y*y), and define vector u(k)=y(k)+sum,
      * u(j)=y(j) (j.gt.k). This defines the transformation matrix as (1-bu*u),
      * with b=2/u*u=1/sum*u(k). Redefine y(k)=u(k) and apply the transformation to
      * elements of the input matrix below and to the right of the (k,k) element.
      * This algorithm for each column k=0,n-1 in turn is equivalent to a single
      * orthogonal transformation which triangularizes the matrix.
      */
      template <class BaseClass>
      inline void operator() (const ConstMatrixBase<T, BaseClass>& m)
         throw (MatrixException)
         {
            size_t i,j;
            A = m;
            Matrix<T> P(A.rows(), A.rows());
            Matrix<T> colVector(A.rows(), 1),
               rowVector(1, A.rows());
            Vector<T> v(A.rows());
            for (j = 0; (j < A.cols()) && (j < (A.rows() - 1)); j++)
            {
               colVector.resize(A.rows() - j, 1);
               rowVector.resize(1, A.rows() - j);
               
                  // for each column c, form the vector v = 
                  // [c[0] + (sign(c[0]))abs(c), c[1], c[2], ...]
                  // then normalize v
               v = A.colCopy(j, j);
               v[0] += ((v[0] >= T(0)) ? T(1) : T(-1)) * norm(v);
               v = normalize(v);
                  // now make matrix P = 1 - 2* columnVector(v) * rowVector(v)
                  // (makes the lower right of P =
                  //   1 - 2* columnVector(v) * rowVector(v)
                  // and the remaining parts I)
                  // and perform A = P * A
               colVector = v;
               rowVector = v;
               MatrixSlice<T> Pslice(P, j, j, P.rows() - j, P.cols() - j);
               ident(P);
               //Pslice -= T(2) * colVector * rowVector;
               Pslice = T(2) * colVector * rowVector - Pslice;
               MatrixSlice<T> Aslice(A, j, j, A.rows() - j, A.cols() - j);
               Aslice = Pslice * Aslice;
                  // set the elements below the diagonal of this column to 0
               for(i = j+1; i < A.rows(); i++)
                  A(i,j) = T(0);
            }
         }  // end Householder::operator()
      
         /// The upper triangular transformed matrix.
      Matrix<T> A;

   }; // end class Householder

   //@}

}  // namespace

#endif