matrixoperators.hpp

一个gps小工具包

      int i3 = (i2+1) % 3;
      toReturn(i1,i1) = 1.0;
      toReturn(i2,i2) = toReturn(i3,i3) = ::cos(angle);
      toReturn(i3,i2) = -(toReturn(i2,i3) = ::sin(angle));

      return toReturn;
   }

/**
 * Inverts the matrix M by Gaussian elimination. Throws on non-square
 * and singular matricies.
 */
   template <class T, class BaseClass>
   inline Matrix<T> inverse(const ConstMatrixBase<T, BaseClass>& m)
      throw (MatrixException)
   {
      if ((m.rows() != m.cols()) || (m.cols() == 0))
      {
         MatrixException e("inverse() requires non-trivial square matrix");
         GPSTK_THROW(e);
      }

      Matrix<T> toReturn(m.rows(), m.cols() * 2);

      size_t r, t, j;
      T temp;

         // set the left half to m
      {
         MatrixSlice<T> ms(toReturn, 0, 0, m.rows(), m.cols());
         ms = m;
      }

         // set the right half to identity
      {
         MatrixSlice<T> ms(toReturn, 0, m.cols(), m.rows(), m.cols());
         ident(ms);
      }

      for (r = 0; r < m.rows(); r++)
      {
            // if m(r,r) is zero, find another row
            // to add to it...
         if (toReturn(r,r) == 0)
         {
            t = r+1;
            while ( (t < m.rows()) && (toReturn(t,r) == 0) )
               t++;

            if (t == m.rows())
            {
               SingularMatrixException e("Singular matrix");
               GPSTK_THROW(e);
            }

            for (j = r; j < toReturn.cols(); j++)
               toReturn(r,j) += (toReturn(t,j) / toReturn(t,r));
         }

            // scale this row's (r,r)'th element to 1
         temp = toReturn(r,r);
         for (j = r; j < toReturn.cols(); j++)
            toReturn(r,j) /= temp;

            // do the elimination
         for (t = 0; t < m.rows(); t++)
         {
            if (t != r)
            {
               temp = toReturn(t,r);
               for (j = r; j < toReturn.cols(); j++)
                  toReturn(t,j) -= temp/toReturn(r,r) * toReturn(r,j);
            }
         }
      }
         // return the right hand side square matrix
      return Matrix<T>(toReturn, 0, m.cols(), m.rows(), m.cols());

   }  // end inverse

/**
 * Inverts the matrix M by LU decomposition. Throws on non-square
 * and singular matricies.
 */
   template <class T, class BaseClass>
   inline Matrix<T> inverseLUD(const ConstMatrixBase<T, BaseClass>& m)
      throw (MatrixException)
   {
      if ((m.rows() != m.cols()) || (m.cols() == 0)) {
         MatrixException e("inverseLUD() requires non-trivial square matrix");
         GPSTK_THROW(e);
      }

      size_t i,j,N=m.rows();
      Matrix<T> inv(m);
      Vector<T> V(N);
      LUDecomp<T> LU;
      LU(m);
      for(j=0; j<N; j++) {    // loop over columns
         V = T(0);
         V(j) = T(1);
         LU.backSub(V);
         for(i=0; i<N; i++) inv(i,j)=V(i);
      }
      return inv;

   }  // end inverseLUD

/**
 * Inverts the square matrix M by SVD. Throws only on input of the zero matrix
 */
   template <class T, class BaseClass>
   inline Matrix<T> inverseSVD(const ConstMatrixBase<T, BaseClass>& m)
      throw (MatrixException)
   {
      if ((m.rows() != m.cols()) || (m.cols() == 0)) {
         MatrixException e("inverseSVD() requires non-trivial square matrix");
         GPSTK_THROW(e);
      }

      size_t i,j,N=m.rows();
      Matrix<T> inv(m);
      SVD<T> svd;
      svd(m);
      // svd will sort the singular values in descending order
      if(svd.S(0) == T(0)) {
         MatrixException e("Input is the zero matrix");
         GPSTK_THROW(e);
      }
      // edit singular values TD input tolerance, output edited SVs
      for(i=1; i<N; i++) if(svd.S(i) < T(1.e-8)*svd.S(0)) svd.S(i)=T(0);

      // back substitution
      Vector<T> V(N);
      for(j=0; j<N; j++) {    //loop over columns
         V = T(0);
         V(j) = T(1);
         svd.backSub(V);
         for(i=0; i<N; i++) inv(i,j)=V(i);
      }
      return inv;

   }  // end inverseSVD


   /**
    * Inverts the square symetrix positive definite matrix M using Cholesky-Crout
    * algorithm. Very fast and useful when M comes from using a Least Mean-Square 
    * (LMS) or Weighted Least Mean-Square (WLMS) method.
    */
   template <class T, class BaseClass>
   inline Matrix<T> inverseChol(const ConstMatrixBase<T, BaseClass>& m)
       throw (MatrixException)
   {
       int N = m.rows(), i, j, k;
       double sum;
       Matrix<T> LI(N,N, 0.0);      // Here we will first store L^-1, and later m^-1

       // Let's call CholeskyCrout class to decompose matrix "m" in L*LT
       gpstk::CholeskyCrout<double> CC;
       CC(m);

       // Let's find the inverse of L (the LI from above)
       for(i=0; i<N; i++) {
           LI(i,i) = 1.0 / CC.L(i,i);
           for(j=0; j<i; j++) {
               sum = 0.0;
               for(k=i; k>=0; k-- ) sum += CC.L(i,k)*LI(k,j);
               LI(i,j) = -sum*LI(i,i);
           }
       }

       // Now, let's remember that m^-1 = transpose(LI)*LI
       LI = transpose(LI) * LI;
       return LI;

   }  // end inverseChol


/**
 *  Matrix * Matrix : row by column multiplication of two matricies.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator* (const ConstMatrixBase<T, BaseClass1>& l, 
                            const ConstMatrixBase<T, BaseClass2>& r)
      throw (MatrixException)
   {
      if (l.cols() != r.rows())
      {
         MatrixException e("Incompatible dimensions for Matrix * Matrix");
         GPSTK_THROW(e);
      }
   
      Matrix<T> toReturn(l.rows(), r.cols(), T(0));
      size_t i, j, k;
      for (i = 0; i < toReturn.rows(); i++)
         for (j = 0; j < toReturn.cols(); j++)
            for (k = 0; k < l.cols(); k++)
               toReturn(i,j) += l(i,k) * r(k,j);

      return toReturn;
   }

/**
 * Matrix times vector multiplication, returning a vector.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Vector<T> operator* (const ConstMatrixBase<T, BaseClass1>& m, 
                            const ConstVectorBase<T, BaseClass2>& v)
      throw (MatrixException)
   {
      if (v.size() != m.cols())
      {
         gpstk::MatrixException e("Incompatible dimensions for Vector * Matrix");
         GPSTK_THROW(e);
      }
   
      Vector<T> toReturn(m.rows());
      size_t i, j;
      for (i = 0; i < m.rows(); i++) 
      {
         toReturn[i] = 0;
         for (j = 0; j < m.cols(); j++)
            toReturn[i] += m(i, j) * v[j];
      }
      return toReturn;
   }
/**
 * Vector times matrix multiplication, returning a vector.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Vector<T> operator* (const ConstVectorBase<T, BaseClass1>& v, 
                            const ConstMatrixBase<T, BaseClass2>& m)
      throw (gpstk::MatrixException)
   {
      if (v.size() != m.rows())
      {
         gpstk::MatrixException e("Incompatible dimensions for Vector * Matrix");
         GPSTK_THROW(e);
      }
   
      Vector<T> toReturn(m.cols());
      size_t i, j;
      for (i = 0; i < m.cols(); i++) 
      {
         toReturn[i] = 0;
         for (j = 0; j < m.rows(); j++)
            toReturn[i] += m(j,i) * v[j];
      }
      return toReturn;
   }

/**
 * Compute sum of two matricies.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator+ (const ConstMatrixBase<T, BaseClass1>& l,
                            const ConstMatrixBase<T, BaseClass2>& r)
      throw (MatrixException)
   {
      if (l.cols() != r.cols() || l.rows() != r.rows())
      {
         MatrixException e("Incompatible dimensions for Matrix + Matrix");
         GPSTK_THROW(e);
      }

      Matrix<T> toReturn(l.rows(), r.cols(), T(0));
      size_t i, j;
      for (i = 0; i < toReturn.rows(); i++)
         for (j = 0; j < toReturn.cols(); j++)
            toReturn(i,j) = l(i,j) + r(i,j);

      return toReturn;
   }

/**
 * Compute difference of two matricies.
 */
   template <class T, class BaseClass1, class BaseClass2>
   inline Matrix<T> operator- (const ConstMatrixBase<T, BaseClass1>& l,
                            const ConstMatrixBase<T, BaseClass2>& r)
      throw (MatrixException)
   {
      if (l.cols() != r.cols() || l.rows() != r.rows())
      {
         MatrixException e("Incompatible dimensions for Matrix - Matrix");
         GPSTK_THROW(e);
      }

      Matrix<T> toReturn(l.rows(), r.cols(), T(0));
      size_t i, j;
      for (i = 0; i < toReturn.rows(); i++)
         for (j = 0; j < toReturn.cols(); j++)
            toReturn(i,j) = l(i,j) - r(i,j);

      return toReturn;
   }

/**
 * Compute the outer product of two vectors.
 */
   template <class T, class BaseClass>
   inline Matrix<T> outer(const ConstVectorBase<T, BaseClass>& v,
                        const ConstVectorBase<T, BaseClass>& w)
      throw (MatrixException)
   {
      if(v.size()*w.size() == 0) {
         MatrixException e("Zero length vector(s)");
         GPSTK_THROW(e);
      }
      Matrix<T> M(v.size(),w.size(),T(0));
      for(size_t i=0; i<v.size(); i++)
         for(size_t j=0; j<w.size(); j++)
            M(i,j) = v(i)*w(j);
      return M;
   }

/// Multiplies all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator* (const ConstMatrixBase<T, BaseClass>& m, const T d)
   {
      Matrix<T> temp(m);
      return temp *= d;
   }

/// Multiplies all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator* (const T d, const ConstMatrixBase<T, BaseClass>& m)
   {
      Matrix<T> temp(m);
      return temp *= d;
   }

/// Divides all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator/ (const ConstMatrixBase<T, BaseClass>& m, const T d)
   {
      Matrix<T> temp(m);
      return temp /= d;
   }

/// Divides all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator/ (const T d, const ConstMatrixBase<T, BaseClass>& m)
   {
      Matrix<T> temp(m);
      return temp /= d;
   }

/// Adds all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator+ (const ConstMatrixBase<T, BaseClass>& m, const T d)
   {
      Matrix<T> temp(m);
      return temp += d;
   }

/// Adds all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator+ (const T d, const ConstMatrixBase<T, BaseClass>& m)
   {
      Matrix<T> temp(m);
      return temp += d;
   }

/// Subtracts all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator- (const ConstMatrixBase<T, BaseClass>& m, const T d)
   {
      Matrix<T> temp(m);
      return temp -= d;
   }

/// Subtracts all the elements of m by d.
   template <class T, class BaseClass>
   inline Matrix<T> operator- (const T d, const ConstMatrixBase<T, BaseClass>& m)
   {
      Matrix<T> temp(m);
      return temp -= d;
   }

   //@}
 
}  // namespace

#endif