MatrixOperators.hpp

Go to the documentation of this file.
00001 #pragma ident "$Id: MatrixOperators.hpp 3320 2012-09-20 00:41:47Z yanweignss $"
00002 
00003 //============================================================================
00004 //
00005 //  This file is part of GPSTk, the GPS Toolkit.
00006 //
00007 //  The GPSTk is free software; you can redistribute it and/or modify
00008 //  it under the terms of the GNU Lesser General Public License as published
00009 //  by the Free Software Foundation; either version 2.1 of the License, or
00010 //  any later version.
00011 //
00012 //  The GPSTk is distributed in the hope that it will be useful,
00013 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 //  GNU Lesser General Public License for more details.
00016 //
00017 //  You should have received a copy of the GNU Lesser General Public
00018 //  License along with GPSTk; if not, write to the Free Software Foundation,
00019 //  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
00020 //  
00021 //  Copyright 2004, The University of Texas at Austin
00022 //
00023 //============================================================================
00024 
00030 #ifndef GPSTK_MATRIX_OPERATORS_HPP
00031 #define GPSTK_MATRIX_OPERATORS_HPP
00032 
00033 #include <limits>
00034 #include "MiscMath.hpp"
00035 #include "MatrixFunctors.hpp"
00036 
00037 namespace gpstk
00038 {
00041  
00046    template <class T, class BaseClass1, class BaseClass2>
00047    inline Matrix<T> operator&&(const ConstMatrixBase<T, BaseClass1>& l, 
00048                             const ConstMatrixBase<T, BaseClass2>& r) 
00049    throw(MatrixException)
00050    {
00051       if (l.cols() != r.cols())
00052       {
00053          MatrixException e("Incompatible dimensions for Matrix && Matrix");
00054          GPSTK_THROW(e);
00055       }
00056 
00057       size_t rows = l.rows() + r.rows();
00058       size_t cols = l.cols();
00059       Matrix<T> toReturn(rows, cols);
00060 
00061       for (rows = 0; rows < l.rows(); rows++)
00062          for (cols = 0; cols < l.cols(); cols++)
00063             toReturn(rows, cols) = l(rows, cols);
00064 
00065       for (rows = 0; rows < r.rows(); rows++)
00066          for (cols = 0; cols < l.cols(); cols++)
00067             toReturn(rows + l.rows(), cols) = r(rows, cols);
00068 
00069       return toReturn;
00070    }
00071 
00076    template <class T, class BaseClass1, class BaseClass2>
00077    inline Matrix<T> operator&&(const ConstMatrixBase<T, BaseClass1>& t, 
00078                             const ConstVectorBase<T, BaseClass2>& b) 
00079    throw(MatrixException)
00080    {
00081       if (t.cols() != b.size())
00082       {
00083          MatrixException e("Incompatible dimensions for Matrix && Vector");
00084          GPSTK_THROW(e);
00085       }
00086 
00087       size_t rows = t.rows() + 1;
00088       size_t cols = t.cols();
00089       Matrix<T> toReturn(rows, cols);
00090 
00091       for (rows = 0; rows < t.rows(); rows++)
00092          for (cols = 0; cols < t.cols(); cols++)
00093             toReturn(rows, cols) = t(rows, cols);
00094 
00095       for (cols = 0; cols < t.cols(); cols++)
00096          toReturn(t.rows(), cols) = b(cols);
00097 
00098       return toReturn;
00099    }
00100 
00105    template <class T, class BaseClass1, class BaseClass2>
00106    inline Matrix<T> operator&&(const ConstVectorBase<T, BaseClass1>& t, 
00107                             const ConstMatrixBase<T, BaseClass2>& b) 
00108    throw(MatrixException)
00109    {
00110       if (t.size() != b.cols())
00111       {
00112          MatrixException e("Incompatible dimensions for Vector && Matrix");
00113          GPSTK_THROW(e);
00114       }
00115 
00116       size_t rows = 1 + b.rows();
00117       size_t cols = b.cols();
00118       Matrix<T> toReturn(rows, cols);
00119 
00120       for (cols = 0; cols < b.cols(); cols++)
00121          toReturn(0, cols) = t(cols);
00122 
00123       for (rows = 1; rows < b.rows()+1; rows++)
00124          for (cols = 0; cols < b.cols(); cols++)
00125             toReturn(rows, cols) = b(rows, cols);
00126 
00127       return toReturn;
00128    }
00129 
00134    template <class T, class BaseClass1, class BaseClass2>
00135    inline Matrix<T> operator||(const ConstMatrixBase<T, BaseClass1>& l,
00136                             const ConstMatrixBase<T, BaseClass2>& r)  
00137       throw(MatrixException)
00138    {
00139       if (l.rows() != r.rows())
00140       {
00141          MatrixException e("Incompatible dimensions for Matrix || Matrix");
00142          GPSTK_THROW(e);
00143       }
00144 
00145       size_t rows = l.rows();
00146       size_t cols = l.cols() + r.cols();
00147       Matrix<T> toReturn(rows, cols);
00148 
00149       for (cols = 0; cols < l.cols(); cols++)
00150          for (rows = 0; rows < l.rows(); rows++)
00151             toReturn(rows, cols) = l(rows, cols);
00152 
00153       for (cols = 0; cols < r.cols(); cols++)
00154          for (rows = 0; rows < l.rows(); rows++)
00155             toReturn(rows, cols + l.cols()) = r(rows,cols);
00156 
00157       return toReturn;
00158    }
00159 
00164    template <class T, class BaseClass1, class BaseClass2>
00165    inline Matrix<T> operator||(const ConstMatrixBase<T, BaseClass1>& l,
00166                             const ConstVectorBase<T, BaseClass2>& r)
00167       throw(MatrixException)
00168    {
00169       if (l.rows() != r.size())
00170       {
00171          MatrixException e("Incompatible dimensions for Matrix || Vector");
00172          GPSTK_THROW(e);
00173       }
00174 
00175       size_t rows = l.rows();
00176       size_t cols = l.cols() + 1;
00177       Matrix<T> toReturn(rows, cols);
00178 
00179       for (cols = 0; cols < l.cols(); cols++)
00180          for (rows = 0; rows < l.rows(); rows++)
00181             toReturn(rows, cols) = l(rows, cols);
00182 
00183       for (rows = 0; rows < l.rows(); rows++)
00184          toReturn(rows, l.cols()) = r(rows);
00185 
00186       return toReturn;
00187    }
00188 
00193    template <class T, class BaseClass1, class BaseClass2>
00194    inline Matrix<T> operator||(const ConstVectorBase<T, BaseClass1>& l,
00195                             const ConstMatrixBase<T, BaseClass2>& r)
00196       throw(MatrixException)
00197    {
00198       if (l.size() != r.rows())
00199       {
00200          MatrixException e("Incompatible dimensions for Vector || Matrix");
00201          GPSTK_THROW(e);
00202       }
00203 
00204       size_t rows = r.rows();
00205       size_t cols = r.cols() + 1;
00206       Matrix<T> toReturn(rows, cols);
00207 
00208       for (rows = 0; rows < r.rows(); rows++)
00209          toReturn(rows, 0) = l(rows);
00210 
00211       for (cols = 1; cols < r.cols()+1; cols++)
00212          for (rows = 0; rows < r.rows(); rows++)
00213             toReturn(rows, cols) = r(rows, cols);
00214 
00215       return toReturn;
00216    }
00217 
00222    template <class T, class BaseClass1, class BaseClass2>
00223    inline Matrix<T> operator||(const ConstVectorBase<T, BaseClass1>& l,
00224                             const ConstVectorBase<T, BaseClass2>& r)
00225       throw(MatrixException)
00226    {
00227       if (l.size() != r.size())
00228       {
00229          MatrixException e("Incompatible dimensions for Vector || Vector");
00230          GPSTK_THROW(e);
00231       }
00232 
00233       size_t rows = r.size();
00234       Matrix<T> toReturn(rows, 2);
00235 
00236       for (rows = 0; rows < r.size(); rows++)
00237       {
00238         toReturn(rows, 0) = l(rows);
00239         toReturn(rows, 1) = r(rows);
00240       }
00241 
00242       return toReturn;
00243    }
00244 
00249    template <class T, class BaseClass>
00250    inline Matrix<T> minorMatrix(const ConstMatrixBase<T, BaseClass>& l,
00251                           size_t row, size_t col) 
00252       throw(MatrixException)
00253    {
00254       if ((row >= l.rows()) || (col >= l.cols()))
00255       {
00256          MatrixException e("Invalid row or column for minorMatrix()");
00257          GPSTK_THROW(e);
00258       }
00259          // handle special cases
00260       if (row == 0)
00261       {
00262          if (col == 0)
00263          {
00264             return Matrix<T>(l,1,1,l.rows()-1,l.cols()-1);  
00265          }
00266          else if (col == (l.cols() - 1))
00267          {
00268             return Matrix<T>(l,1,0,l.rows()-1,l.cols()-1);
00269          }
00270          else
00271          {
00272             return Matrix<T>(l,1,0,l.rows()-1,col) ||
00273                Matrix<T>(l,1,col+1,l.rows()-1,l.cols()-col-1);
00274          }
00275       }
00276       else if (row == (l.rows() - 1))
00277       {
00278          if (col == 0)
00279          {
00280             return Matrix<T>(l,0,1,l.rows()-1,l.cols()-1);
00281          }
00282          else if (col == (l.cols() - 1))
00283          {
00284             return Matrix<T>(l,0,0,l.rows()-1,l.cols()-1);
00285          }
00286          else
00287          {
00288             return Matrix<T>(l,0,0,l.rows()-1,col) ||
00289                Matrix<T>(l,0,col+1,l.rows()-1,l.cols()-col-1);
00290          }
00291       }
00292       else if (col == 0)
00293       {
00294          return Matrix<T>(l,0,1,row,l.cols()-1) &&
00295             Matrix<T>(l,row+1,1,l.rows()-row-1,l.cols()-1);
00296       }
00297       else if (col == (l.cols() - 1))
00298       {
00299          return Matrix<T>(l,0,0,row,l.cols()-1) &&
00300             Matrix<T>(l,row+1,0,l.rows()-row-1,l.cols()-1);
00301       }
00302       else
00303       {
00304          return (Matrix<T>(l, 0, 0, row, col) || 
00305                  Matrix<T>(l, 0, col + 1, row, l.cols()-col-1)) &&
00306             (Matrix<T>(l, row + 1, 0, l.rows()-row-1, col) ||
00307              Matrix<T>(l, row + 1, col + 1, l.rows()-row-1, l.cols()-col-1));
00308       }
00309    }
00310 
00314    template <class T, class BaseClass>
00315    inline Matrix<T> transpose(const ConstMatrixBase<T, BaseClass>& m)
00316    {
00317       Matrix<T> temp(m.cols(), m.rows());
00318       size_t i, j;
00319       for (i = 0; i < m.rows(); i++)
00320          for (j = 0; j < m.cols(); j++)
00321             temp(j,i) = m(i,j);
00322       return temp;
00323    }
00324  
00328    template <class T, class BaseClass>
00329    inline T det(const ConstMatrixBase<T, BaseClass>& m) 
00330       throw(MatrixException)
00331    {
00332       try
00333       {
00334          LUDecomp<T> LU;
00335          LU(m);
00336          return LU.det();
00337       }
00338       catch(MatrixException& e)
00339       {
00340          e.addText("in det()");
00341          GPSTK_RETHROW(e);
00342       }
00343    }
00344 
00348    template <class T, class BaseClass>
00349    inline T condNum(const ConstMatrixBase<T, BaseClass>& m, T& big, T& small) 
00350       throw ()
00351    {
00352       SVD<T> svd;
00353       svd(m);
00354       // SVD will not always sort singular values in descending order
00355       svd.sort(true);
00356       big = svd.S(0);
00357       small = svd.S(svd.S.size()-1);
00358       if(small <= std::numeric_limits<T>::epsilon())
00359          return T(0);
00360       return big/small;
00361    }
00362 
00366    template <class T, class BaseClass>
00367    inline T condNum(const ConstMatrixBase<T, BaseClass>& m) 
00368       throw ()
00369    {
00370       T big, small;
00371       return condNum(m, big, small);
00372    }
00373 
00377    template <class T>
00378    inline Matrix<T> ident(size_t dim)
00379       throw(MatrixException)
00380    {
00381       if (dim == 0)
00382       {
00383          MatrixException e("Invalid (0) dimension for ident()");
00384          GPSTK_THROW(e);
00385       }
00386       Matrix<T> toReturn(dim, dim, T(0));
00387       size_t i;
00388       for (i = 0; i < toReturn.rows(); i++)
00389          toReturn(i,i) = T(1);
00390       return toReturn;
00391    }
00392 
00396    template <class T, class BaseClass>
00397    inline Matrix<T> diag(const ConstMatrixBase<T, BaseClass>& m)
00398       throw(MatrixException)
00399    {
00400       if ( (m.rows() != m.cols()) || (m.cols() < 1) )
00401       {
00402          MatrixException e("invalid matrix dimensions for ident()");
00403          GPSTK_THROW(e);
00404       }
00405 
00406       const size_t dim = m.rows();
00407 
00408       Matrix<T> temp(dim, dim, T(0));
00409       for (size_t i = 0; i < dim; i++)
00410          temp(i,i) = m(i,i);
00411 
00412       return temp;
00413    }
00414 
00418    template <class T, class BaseClass>
00419    inline Matrix<T> blkdiag(const ConstMatrixBase<T, BaseClass>& m1,
00420                             const ConstMatrixBase<T, BaseClass>& m2)
00421       throw(MatrixException)
00422    {
00423       if ( (m1.rows() != m1.cols()) || (m1.cols() < 1) ||
00424            (m2.rows() != m2.cols()) || (m2.cols() < 1) )
00425       {
00426          MatrixException e("Invalid matrix dimensions of input.");
00427          GPSTK_THROW(e);
00428       }
00429 
00430       const size_t dim1 = m1.rows();
00431       const size_t dim2 = m2.rows();
00432 
00433       Matrix<T> temp(dim1+dim2, dim1+dim2, T(0));
00434       for (size_t i = 0; i < dim1; i++)
00435       {
00436          for(size_t j = 0; j < dim1; j++)
00437          {
00438             temp(i,j) = m1(i,j);
00439          }
00440       }
00441       for (size_t i = 0; i < dim2; i++)
00442       {
00443          for(size_t j = 0; j < dim2; j++)
00444          {
00445             temp(i+dim1,j+dim1) = m2(i,j);
00446          }
00447       }
00448 
00449       return temp;
00450    }
00451 
00452    template <class T, class BaseClass>
00453    inline Matrix<T> blkdiag(const ConstMatrixBase<T, BaseClass>& m1,
00454                             const ConstMatrixBase<T, BaseClass>& m2,
00455                             const ConstMatrixBase<T, BaseClass>& m3)
00456       throw(MatrixException)
00457    { return blkdiag( blkdiag(m1,m2), m3 ); }
00458 
00459    template <class T, class BaseClass>
00460    inline Matrix<T> blkdiag(const ConstMatrixBase<T, BaseClass>& m1,
00461                             const ConstMatrixBase<T, BaseClass>& m2,
00462                             const ConstMatrixBase<T, BaseClass>& m3,
00463                             const ConstMatrixBase<T, BaseClass>& m4)
00464       throw(MatrixException)
00465    { return blkdiag( blkdiag(m1,m2,m3), m4 ); }
00466 
00467 
00472    template <class T>
00473    inline Matrix<T> rotation(T angle, int axis)
00474       throw(MatrixException)
00475    {
00476       if (axis < 1 || axis > 3)
00477       {
00478          MatrixException e("Invalid axis (must be 1,2, or 3)");
00479          GPSTK_THROW(e);
00480       }
00481       Matrix<T> toReturn(3,3,T(0));
00482       int i1 = axis-1;
00483       int i2 = (i1+1) % 3;
00484       int i3 = (i2+1) % 3;
00485       toReturn(i1,i1) = 1.0;
00486       toReturn(i2,i2) = toReturn(i3,i3) = ::cos(angle);
00487       toReturn(i3,i2) = -(toReturn(i2,i3) = ::sin(angle));
00488 
00489       return toReturn;
00490    }
00491 
00496    template <class T, class BaseClass>
00497    inline Matrix<T> inverse(const ConstMatrixBase<T, BaseClass>& m)
00498       throw (MatrixException)
00499    {
00500       if ((m.rows() != m.cols()) || (m.cols() == 0))
00501       {
00502          MatrixException e("inverse() requires non-trivial square matrix");
00503          GPSTK_THROW(e);
00504       }
00505 
00506       Matrix<T> toReturn(m.rows(), m.cols() * 2);
00507 
00508       size_t r, t, j;
00509       T temp;
00510 
00511          // set the left half to m
00512       {
00513          MatrixSlice<T> ms(toReturn, 0, 0, m.rows(), m.cols());
00514          ms = m;
00515       }
00516 
00517          // set the right half to identity
00518       {
00519          MatrixSlice<T> ms(toReturn, 0, m.cols(), m.rows(), m.cols());
00520          ident(ms);
00521       }
00522 
00523       for (r = 0; r < m.rows(); r++)
00524       {
00525             // if m(r,r) is zero, find another row
00526             // to add to it...
00527          if (toReturn(r,r) == 0)
00528          {
00529             t = r+1;
00530             while ( (t < m.rows()) && (toReturn(t,r) == 0) )
00531                t++;
00532 
00533             if (t == m.rows())
00534             {
00535                SingularMatrixException e("Singular matrix");
00536                GPSTK_THROW(e);
00537             }
00538 
00539             for (j = r; j < toReturn.cols(); j++)
00540                toReturn(r,j) += (toReturn(t,j) / toReturn(t,r));
00541          }
00542 
00543             // scale this row's (r,r)'th element to 1
00544          temp = toReturn(r,r);
00545          for (j = r; j < toReturn.cols(); j++)
00546             toReturn(r,j) /= temp;
00547 
00548             // do the elimination
00549          for (t = 0; t < m.rows(); t++)
00550          {
00551             if (t != r)
00552             {
00553                temp = toReturn(t,r);
00554                for (j = r; j < toReturn.cols(); j++)
00555                   toReturn(t,j) -= temp/toReturn(r,r) * toReturn(r,j);
00556             }
00557          }
00558       }
00559          // return the right hand side square matrix
00560       return Matrix<T>(toReturn, 0, m.cols(), m.rows(), m.cols());
00561 
00562    }  // end inverse
00563 
00568    template <class T, class BaseClass>
00569    inline Matrix<T> inverseLUD(const ConstMatrixBase<T, BaseClass>& m)
00570       throw (MatrixException)
00571    {
00572       if ((m.rows() != m.cols()) || (m.cols() == 0)) {
00573          MatrixException e("inverseLUD() requires non-trivial square matrix");
00574          GPSTK_THROW(e);
00575       }
00576 
00577       size_t i,j,N=m.rows();
00578       Matrix<T> inv(m);
00579       Vector<T> V(N);
00580       LUDecomp<T> LU;
00581       LU(m);
00582       for(j=0; j<N; j++) {    // loop over columns
00583          V = T(0);
00584          V(j) = T(1);
00585          LU.backSub(V);
00586          for(i=0; i<N; i++) inv(i,j)=V(i);
00587       }
00588       return inv;
00589 
00590    }  // end inverseLUD
00591 
00596    template <class T, class BaseClass>
00597    inline Matrix<T> inverseLUD(const ConstMatrixBase<T, BaseClass>& m, T& determ)
00598       throw (MatrixException)
00599    {
00600       if ((m.rows() != m.cols()) || (m.cols() == 0)) {
00601          MatrixException e("inverseLUD() requires non-trivial square matrix");
00602          GPSTK_THROW(e);
00603       }
00604 
00605       size_t i,j,N=m.rows();
00606       Matrix<T> inv(m);
00607       Vector<T> V(N);
00608       LUDecomp<T> LU;
00609       LU(m);
00610       // compute determinant
00611       determ = T(LU.parity);
00612       for(i = 0; i < m.rows(); i++) determ *= LU.LU(i,i);
00613       // compute inverse
00614       for(j=0; j<N; j++) {    // loop over columns
00615          V = T(0);
00616          V(j) = T(1);
00617          LU.backSub(V);
00618          for(i=0; i<N; i++) inv(i,j)=V(i);
00619       }
00620       return inv;
00621 
00622    }  // end inverseLUD
00623 
00628    template <class T, class BaseClass>
00629    inline Matrix<T> inverseSVD(const ConstMatrixBase<T, BaseClass>& m,
00630          const T tol=T(1.e-8)) throw (MatrixException)
00631    {
00632       if ((m.rows() != m.cols()) || (m.cols() == 0)) {
00633          MatrixException e("inverseSVD() requires non-trivial square matrix");
00634          GPSTK_THROW(e);
00635       }
00636 
00637       size_t i,j,N=m.rows();
00638       Matrix<T> inv(m);
00639       SVD<T> svd;
00640       svd(m);
00641       // SVD will not always sort singular values in descending order
00642       svd.sort(true);
00643       if(svd.S(0) == T(0)) {
00644          MatrixException e("Input is the zero matrix");
00645          GPSTK_THROW(e);
00646       }
00647       // edit singular values TD input tolerance, output edited SVs
00648       for(i=1; i<N; i++) if(svd.S(i) < tol*svd.S(0)) svd.S(i)=T(0);
00649       // back substitution
00650       Vector<T> V(N);
00651       for(j=0; j<N; j++) {    //loop over columns
00652          V = T(0);
00653          V(j) = T(1);
00654          svd.backSub(V);
00655          for(i=0; i<N; i++) inv(i,j)=V(i);
00656       }
00657       return inv;
00658 
00659    }  // end inverseSVD
00660 
00666    template <class T, class BaseClass>
00667    inline Matrix<T> inverseSVD(const ConstMatrixBase<T, BaseClass>& m,
00668       T& big, T& small, const T tol=T(1.e-8)) throw (MatrixException)
00669    {
00670       if ((m.rows() != m.cols()) || (m.cols() == 0)) {
00671          MatrixException e("inverseSVD() requires non-trivial square matrix");
00672          GPSTK_THROW(e);
00673       }
00674 
00675       size_t i,j,N=m.rows();
00676       Matrix<T> inv(m);
00677       SVD<T> svd;
00678       svd(m);
00679       // SVD will not always sort singular values in descending order
00680       svd.sort(true);
00681       if(svd.S(0) == T(0)) {
00682          MatrixException e("Input is the zero matrix");
00683          GPSTK_THROW(e);
00684       }
00685 
00686       // compute condition number = big/small
00687       big = svd.S(0);
00688       small = svd.S(svd.S.size()-1);
00689 
00690       // edit singular values using input tolerance, output edited SVs
00691       for(i=1; i<N; i++) if(svd.S(i) < tol*svd.S(0)) svd.S(i)=T(0);
00692 
00693       // back substitution
00694       Vector<T> V(N);
00695       for(j=0; j<N; j++) {    //loop over columns
00696          V = T(0);
00697          V(j) = T(1);
00698          svd.backSub(V);
00699          for(i=0; i<N; i++) inv(i,j)=V(i);
00700       }
00701       return inv;
00702 
00703    }  // end inverseSVD
00704 
00710    template <class T, class BaseClass>
00711    inline Matrix<T> inverseSVD(const ConstMatrixBase<T, BaseClass>& m,
00712       Vector<T>& sv, const T tol=T(1.e-8)) throw (MatrixException)
00713    {
00714       if ((m.rows() != m.cols()) || (m.cols() == 0)) {
00715          MatrixException e("inverseSVD() requires non-trivial square matrix");
00716          GPSTK_THROW(e);
00717       }
00718 
00719       size_t i,j,N=m.rows();
00720       Matrix<T> inv(m);
00721       SVD<T> svd;
00722       svd(m);
00723       // SVD will not always sort singular values in descending order
00724       svd.sort(true);
00725       if(svd.S(0) == T(0)) {
00726          MatrixException e("Input is the zero matrix");
00727          GPSTK_THROW(e);
00728       }
00729 
00730       // save the singular values
00731       sv = Vector<T>(N);
00732       for(i=0; i<N; i++) sv(i) = svd.S(i);
00733 
00734       // edit singular values using input tolerance, output edited SVs
00735       for(i=1; i<N; i++) if(svd.S(i) < tol*svd.S(0)) svd.S(i)=T(0);
00736 
00737       // back substitution
00738       Vector<T> V(N);
00739       for(j=0; j<N; j++) {    //loop over columns
00740          V = T(0);
00741          V(j) = T(1);
00742          svd.backSub(V);
00743          for(i=0; i<N; i++) inv(i,j)=V(i);
00744       }
00745       return inv;
00746 
00747    }  // end inverseSVD
00748 
00754    template <class T, class BaseClass>
00755    inline Matrix<T> inverseChol(const ConstMatrixBase<T, BaseClass>& m)
00756        throw (MatrixException)
00757    {
00758        int N = m.rows(), i, j, k;
00759        double sum;
00760        Matrix<T> LI(N,N, 0.0);      // Here we will first store L^-1, and later m^-1
00761 
00762        // Let's call CholeskyCrout class to decompose matrix "m" in L*LT
00763        gpstk::CholeskyCrout<double> CC;
00764        CC(m);
00765 
00766        // Let's find the inverse of L (the LI from above)
00767        for(i=0; i<N; i++) {
00768            LI(i,i) = 1.0 / CC.L(i,i);
00769            for(j=0; j<i; j++) {
00770                sum = 0.0;
00771                for(k=i; k>=0; k-- ) sum += CC.L(i,k)*LI(k,j);
00772                LI(i,j) = -sum*LI(i,i);
00773            }
00774        }
00775 
00776        // Now, let's remember that m^-1 = transpose(LI)*LI
00777        LI = transpose(LI) * LI;
00778        return LI;
00779 
00780    }  // end inverseChol
00781 
00782 
00786    template <class T, class BaseClass1, class BaseClass2>
00787    inline Matrix<T> operator* (const ConstMatrixBase<T, BaseClass1>& l, 
00788                             const ConstMatrixBase<T, BaseClass2>& r)
00789       throw (MatrixException)
00790    {
00791       if (l.cols() != r.rows())
00792       {
00793          MatrixException e("Incompatible dimensions for Matrix * Matrix");
00794          GPSTK_THROW(e);
00795       }
00796    
00797       Matrix<T> toReturn(l.rows(), r.cols(), T(0));
00798       size_t i, j, k;
00799       for (i = 0; i < toReturn.rows(); i++)
00800          for (j = 0; j < toReturn.cols(); j++)
00801             for (k = 0; k < l.cols(); k++)
00802                toReturn(i,j) += l(i,k) * r(k,j);
00803 
00804       return toReturn;
00805    }
00806 
00810    template <class T, class BaseClass1, class BaseClass2>
00811    inline Vector<T> operator* (const ConstMatrixBase<T, BaseClass1>& m, 
00812                             const ConstVectorBase<T, BaseClass2>& v)
00813       throw (MatrixException)
00814    {
00815       if (v.size() != m.cols())
00816       {
00817          gpstk::MatrixException e("Incompatible dimensions for Vector * Matrix");
00818          GPSTK_THROW(e);
00819       }
00820    
00821       Vector<T> toReturn(m.rows());
00822       size_t i, j;
00823       for (i = 0; i < m.rows(); i++) 
00824       {
00825          toReturn[i] = 0;
00826          for (j = 0; j < m.cols(); j++)
00827             toReturn[i] += m(i, j) * v[j];
00828       }
00829       return toReturn;
00830    }
00834    template <class T, class BaseClass1, class BaseClass2>
00835    inline Vector<T> operator* (const ConstVectorBase<T, BaseClass1>& v, 
00836                             const ConstMatrixBase<T, BaseClass2>& m)
00837       throw (gpstk::MatrixException)
00838    {
00839       if (v.size() != m.rows())
00840       {
00841          gpstk::MatrixException e("Incompatible dimensions for Vector * Matrix");
00842          GPSTK_THROW(e);
00843       }
00844    
00845       Vector<T> toReturn(m.cols());
00846       size_t i, j;
00847       for (i = 0; i < m.cols(); i++) 
00848       {
00849          toReturn[i] = 0;
00850          for (j = 0; j < m.rows(); j++)
00851             toReturn[i] += m(j,i) * v[j];
00852       }
00853       return toReturn;
00854    }
00855 
00859    template <class T, class BaseClass1, class BaseClass2>
00860    inline Matrix<T> operator+ (const ConstMatrixBase<T, BaseClass1>& l,
00861                             const ConstMatrixBase<T, BaseClass2>& r)
00862       throw (MatrixException)
00863    {
00864       if (l.cols() != r.cols() || l.rows() != r.rows())
00865       {
00866          MatrixException e("Incompatible dimensions for Matrix + Matrix");
00867          GPSTK_THROW(e);
00868       }
00869 
00870       Matrix<T> toReturn(l.rows(), r.cols(), T(0));
00871       size_t i, j;
00872       for (i = 0; i < toReturn.rows(); i++)
00873          for (j = 0; j < toReturn.cols(); j++)
00874             toReturn(i,j) = l(i,j) + r(i,j);
00875 
00876       return toReturn;
00877    }
00878 
00882    template <class T, class BaseClass1, class BaseClass2>
00883    inline Matrix<T> operator- (const ConstMatrixBase<T, BaseClass1>& l,
00884                             const ConstMatrixBase<T, BaseClass2>& r)
00885       throw (MatrixException)
00886    {
00887       if (l.cols() != r.cols() || l.rows() != r.rows())
00888       {
00889          MatrixException e("Incompatible dimensions for Matrix - Matrix");
00890          GPSTK_THROW(e);
00891       }
00892 
00893       Matrix<T> toReturn(l.rows(), r.cols(), T(0));
00894       size_t i, j;
00895       for (i = 0; i < toReturn.rows(); i++)
00896          for (j = 0; j < toReturn.cols(); j++)
00897             toReturn(i,j) = l(i,j) - r(i,j);
00898 
00899       return toReturn;
00900    }
00901 
00905    template <class T, class BaseClass>
00906    inline Matrix<T> outer(const ConstVectorBase<T, BaseClass>& v,
00907                         const ConstVectorBase<T, BaseClass>& w)
00908       throw (MatrixException)
00909    {
00910       if(v.size()*w.size() == 0) {
00911          MatrixException e("Zero length vector(s)");
00912          GPSTK_THROW(e);
00913       }
00914       Matrix<T> M(v.size(),w.size(),T(0));
00915       for(size_t i=0; i<v.size(); i++)
00916          for(size_t j=0; j<w.size(); j++)
00917             M(i,j) = v(i)*w(j);
00918       return M;
00919    }
00920 
00922    template <class T, class BaseClass>
00923    inline Matrix<T> operator* (const ConstMatrixBase<T, BaseClass>& m, const T d)
00924    {
00925       Matrix<T> temp(m);
00926       return temp *= d;
00927    }
00928 
00930    template <class T, class BaseClass>
00931    inline Matrix<T> operator* (const T d, const ConstMatrixBase<T, BaseClass>& m)
00932    {
00933       Matrix<T> temp(m);
00934       return temp *= d;
00935    }
00936 
00938    template <class T, class BaseClass>
00939    inline Matrix<T> operator/ (const ConstMatrixBase<T, BaseClass>& m, const T d)
00940    {
00941       Matrix<T> temp(m);
00942       return temp /= d;
00943    }
00944 
00946    template <class T, class BaseClass>
00947    inline Matrix<T> operator/ (const T d, const ConstMatrixBase<T, BaseClass>& m)
00948    {
00949       Matrix<T> temp(m);
00950       return temp /= d;
00951    }
00952 
00954    template <class T, class BaseClass>
00955    inline Matrix<T> operator+ (const ConstMatrixBase<T, BaseClass>& m, const T d)
00956    {
00957       Matrix<T> temp(m);
00958       return temp += d;
00959    }
00960 
00962    template <class T, class BaseClass>
00963    inline Matrix<T> operator+ (const T d, const ConstMatrixBase<T, BaseClass>& m)
00964    {
00965       Matrix<T> temp(m);
00966       return temp += d;
00967    }
00968 
00970    template <class T, class BaseClass>
00971    inline Matrix<T> operator- (const ConstMatrixBase<T, BaseClass>& m, const T d)
00972    {
00973       Matrix<T> temp(m);
00974       return temp -= d;
00975    }
00976 
00978    template <class T, class BaseClass>
00979    inline Matrix<T> operator- (const T d, const ConstMatrixBase<T, BaseClass>& m)
00980    {
00981       Matrix<T> temp(m);
00982       return temp -= d;
00983    }
00984 
00986  
00987 }  // namespace
00988 
00989 #endif

Generated on Fri Dec 6 03:31:04 2013 for GPS ToolKit Software Library by  doxygen 1.3.9.1