RealQZ.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2012 Alexey Korepanov <kaikaikai@yandex.ru>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_REAL_QZ_H
00011 #define EIGEN_REAL_QZ_H
00012 
00013 namespace Eigen {
00014 
00057   template<typename _MatrixType> class RealQZ
00058   {
00059     public:
00060       typedef _MatrixType MatrixType;
00061       enum {
00062         RowsAtCompileTime = MatrixType::RowsAtCompileTime,
00063         ColsAtCompileTime = MatrixType::ColsAtCompileTime,
00064         Options = MatrixType::Options,
00065         MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
00066         MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
00067       };
00068       typedef typename MatrixType::Scalar Scalar;
00069       typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
00070       typedef typename MatrixType::Index Index;
00071 
00072       typedef Matrix<ComplexScalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> EigenvalueType;
00073       typedef Matrix<Scalar, ColsAtCompileTime, 1, Options & ~RowMajor, MaxColsAtCompileTime, 1> ColumnVectorType;
00074 
00086       RealQZ(Index size = RowsAtCompileTime==Dynamic ? 1 : RowsAtCompileTime) : 
00087         m_S(size, size),
00088         m_T(size, size),
00089         m_Q(size, size),
00090         m_Z(size, size),
00091         m_workspace(size*2),
00092         m_maxIters(400),
00093         m_isInitialized(false)
00094         { }
00095 
00104       RealQZ(const MatrixType& A, const MatrixType& B, bool computeQZ = true) :
00105         m_S(A.rows(),A.cols()),
00106         m_T(A.rows(),A.cols()),
00107         m_Q(A.rows(),A.cols()),
00108         m_Z(A.rows(),A.cols()),
00109         m_workspace(A.rows()*2),
00110         m_maxIters(400),
00111         m_isInitialized(false) {
00112           compute(A, B, computeQZ);
00113         }
00114 
00119       const MatrixType& matrixQ() const {
00120         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00121         eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
00122         return m_Q;
00123       }
00124 
00129       const MatrixType& matrixZ() const {
00130         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00131         eigen_assert(m_computeQZ && "The matrices Q and Z have not been computed during the QZ decomposition.");
00132         return m_Z;
00133       }
00134 
00139       const MatrixType& matrixS() const {
00140         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00141         return m_S;
00142       }
00143 
00148       const MatrixType& matrixT() const {
00149         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00150         return m_T;
00151       }
00152 
00160       RealQZ& compute(const MatrixType& A, const MatrixType& B, bool computeQZ = true);
00161 
00166       ComputationInfo info() const
00167       {
00168         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00169         return m_info;
00170       }
00171 
00174       Index iterations() const
00175       {
00176         eigen_assert(m_isInitialized && "RealQZ is not initialized.");
00177         return m_global_iter;
00178       }
00179 
00183       RealQZ& setMaxIterations(Index maxIters)
00184       {
00185         m_maxIters = maxIters;
00186         return *this;
00187       }
00188 
00189     private:
00190 
00191       MatrixType m_S, m_T, m_Q, m_Z;
00192       Matrix<Scalar,Dynamic,1> m_workspace;
00193       ComputationInfo m_info;
00194       Index m_maxIters;
00195       bool m_isInitialized;
00196       bool m_computeQZ;
00197       Scalar m_normOfT, m_normOfS;
00198       Index m_global_iter;
00199 
00200       typedef Matrix<Scalar,3,1> Vector3s;
00201       typedef Matrix<Scalar,2,1> Vector2s;
00202       typedef Matrix<Scalar,2,2> Matrix2s;
00203       typedef JacobiRotation<Scalar> JRs;
00204 
00205       void hessenbergTriangular();
00206       void computeNorms();
00207       Index findSmallSubdiagEntry(Index iu);
00208       Index findSmallDiagEntry(Index f, Index l);
00209       void splitOffTwoRows(Index i);
00210       void pushDownZero(Index z, Index f, Index l);
00211       void step(Index f, Index l, Index iter);
00212 
00213   }; // RealQZ
00214 
00216   template<typename MatrixType>
00217     void RealQZ<MatrixType>::hessenbergTriangular()
00218     {
00219 
00220       const Index dim = m_S.cols();
00221 
00222       // perform QR decomposition of T, overwrite T with R, save Q
00223       HouseholderQR<MatrixType> qrT(m_T);
00224       m_T = qrT.matrixQR();
00225       m_T.template triangularView<StrictlyLower>().setZero();
00226       m_Q = qrT.householderQ();
00227       // overwrite S with Q* S
00228       m_S.applyOnTheLeft(m_Q.adjoint());
00229       // init Z as Identity
00230       if (m_computeQZ)
00231         m_Z = MatrixType::Identity(dim,dim);
00232       // reduce S to upper Hessenberg with Givens rotations
00233       for (Index j=0; j<=dim-3; j++) {
00234         for (Index i=dim-1; i>=j+2; i--) {
00235           JRs G;
00236           // kill S(i,j)
00237           if(m_S.coeff(i,j) != 0)
00238           {
00239             G.makeGivens(m_S.coeff(i-1,j), m_S.coeff(i,j), &m_S.coeffRef(i-1, j));
00240             m_S.coeffRef(i,j) = Scalar(0.0);
00241             m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint());
00242             m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint());
00243           }
00244           // update Q
00245           if (m_computeQZ)
00246             m_Q.applyOnTheRight(i-1,i,G);
00247           // kill T(i,i-1)
00248           if(m_T.coeff(i,i-1)!=Scalar(0))
00249           {
00250             G.makeGivens(m_T.coeff(i,i), m_T.coeff(i,i-1), &m_T.coeffRef(i,i));
00251             m_T.coeffRef(i,i-1) = Scalar(0.0);
00252             m_S.applyOnTheRight(i,i-1,G);
00253             m_T.topRows(i).applyOnTheRight(i,i-1,G);
00254           }
00255           // update Z
00256           if (m_computeQZ)
00257             m_Z.applyOnTheLeft(i,i-1,G.adjoint());
00258         }
00259       }
00260     }
00261 
00263   template<typename MatrixType>
00264     inline void RealQZ<MatrixType>::computeNorms()
00265     {
00266       const Index size = m_S.cols();
00267       m_normOfS = Scalar(0.0);
00268       m_normOfT = Scalar(0.0);
00269       for (Index j = 0; j < size; ++j)
00270       {
00271         m_normOfS += m_S.col(j).segment(0, (std::min)(size,j+2)).cwiseAbs().sum();
00272         m_normOfT += m_T.row(j).segment(j, size - j).cwiseAbs().sum();
00273       }
00274     }
00275 
00276 
00278   template<typename MatrixType>
00279     inline typename MatrixType::Index RealQZ<MatrixType>::findSmallSubdiagEntry(Index iu)
00280     {
00281       using std::abs;
00282       Index res = iu;
00283       while (res > 0)
00284       {
00285         Scalar s = abs(m_S.coeff(res-1,res-1)) + abs(m_S.coeff(res,res));
00286         if (s == Scalar(0.0))
00287           s = m_normOfS;
00288         if (abs(m_S.coeff(res,res-1)) < NumTraits<Scalar>::epsilon() * s)
00289           break;
00290         res--;
00291       }
00292       return res;
00293     }
00294 
00296   template<typename MatrixType>
00297     inline typename MatrixType::Index RealQZ<MatrixType>::findSmallDiagEntry(Index f, Index l)
00298     {
00299       using std::abs;
00300       Index res = l;
00301       while (res >= f) {
00302         if (abs(m_T.coeff(res,res)) <= NumTraits<Scalar>::epsilon() * m_normOfT)
00303           break;
00304         res--;
00305       }
00306       return res;
00307     }
00308 
00310   template<typename MatrixType>
00311     inline void RealQZ<MatrixType>::splitOffTwoRows(Index i)
00312     {
00313       using std::abs;
00314       using std::sqrt;
00315       const Index dim=m_S.cols();
00316       if (abs(m_S.coeff(i+1,i)==Scalar(0)))
00317         return;
00318       Index z = findSmallDiagEntry(i,i+1);
00319       if (z==i-1)
00320       {
00321         // block of (S T^{-1})
00322         Matrix2s STi = m_T.template block<2,2>(i,i).template triangularView<Upper>().
00323           template solve<OnTheRight>(m_S.template block<2,2>(i,i));
00324         Scalar p = Scalar(0.5)*(STi(0,0)-STi(1,1));
00325         Scalar q = p*p + STi(1,0)*STi(0,1);
00326         if (q>=0) {
00327           Scalar z = sqrt(q);
00328           // one QR-like iteration for ABi - lambda I
00329           // is enough - when we know exact eigenvalue in advance,
00330           // convergence is immediate
00331           JRs G;
00332           if (p>=0)
00333             G.makeGivens(p + z, STi(1,0));
00334           else
00335             G.makeGivens(p - z, STi(1,0));
00336           m_S.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
00337           m_T.rightCols(dim-i).applyOnTheLeft(i,i+1,G.adjoint());
00338           // update Q
00339           if (m_computeQZ)
00340             m_Q.applyOnTheRight(i,i+1,G);
00341 
00342           G.makeGivens(m_T.coeff(i+1,i+1), m_T.coeff(i+1,i));
00343           m_S.topRows(i+2).applyOnTheRight(i+1,i,G);
00344           m_T.topRows(i+2).applyOnTheRight(i+1,i,G);
00345           // update Z
00346           if (m_computeQZ)
00347             m_Z.applyOnTheLeft(i+1,i,G.adjoint());
00348 
00349           m_S.coeffRef(i+1,i) = Scalar(0.0);
00350           m_T.coeffRef(i+1,i) = Scalar(0.0);
00351         }
00352       }
00353       else
00354       {
00355         pushDownZero(z,i,i+1);
00356       }
00357     }
00358 
00360   template<typename MatrixType>
00361     inline void RealQZ<MatrixType>::pushDownZero(Index z, Index f, Index l)
00362     {
00363       JRs G;
00364       const Index dim = m_S.cols();
00365       for (Index zz=z; zz<l; zz++)
00366       {
00367         // push 0 down
00368         Index firstColS = zz>f ? (zz-1) : zz;
00369         G.makeGivens(m_T.coeff(zz, zz+1), m_T.coeff(zz+1, zz+1));
00370         m_S.rightCols(dim-firstColS).applyOnTheLeft(zz,zz+1,G.adjoint());
00371         m_T.rightCols(dim-zz).applyOnTheLeft(zz,zz+1,G.adjoint());
00372         m_T.coeffRef(zz+1,zz+1) = Scalar(0.0);
00373         // update Q
00374         if (m_computeQZ)
00375           m_Q.applyOnTheRight(zz,zz+1,G);
00376         // kill S(zz+1, zz-1)
00377         if (zz>f)
00378         {
00379           G.makeGivens(m_S.coeff(zz+1, zz), m_S.coeff(zz+1,zz-1));
00380           m_S.topRows(zz+2).applyOnTheRight(zz, zz-1,G);
00381           m_T.topRows(zz+1).applyOnTheRight(zz, zz-1,G);
00382           m_S.coeffRef(zz+1,zz-1) = Scalar(0.0);
00383           // update Z
00384           if (m_computeQZ)
00385             m_Z.applyOnTheLeft(zz,zz-1,G.adjoint());
00386         }
00387       }
00388       // finally kill S(l,l-1)
00389       G.makeGivens(m_S.coeff(l,l), m_S.coeff(l,l-1));
00390       m_S.applyOnTheRight(l,l-1,G);
00391       m_T.applyOnTheRight(l,l-1,G);
00392       m_S.coeffRef(l,l-1)=Scalar(0.0);
00393       // update Z
00394       if (m_computeQZ)
00395         m_Z.applyOnTheLeft(l,l-1,G.adjoint());
00396     }
00397 
00399   template<typename MatrixType>
00400     inline void RealQZ<MatrixType>::step(Index f, Index l, Index iter)
00401     {
00402       using std::abs;
00403       const Index dim = m_S.cols();
00404 
00405       // x, y, z
00406       Scalar x, y, z;
00407       if (iter==10)
00408       {
00409         // Wilkinson ad hoc shift
00410         const Scalar
00411           a11=m_S.coeff(f+0,f+0), a12=m_S.coeff(f+0,f+1),
00412           a21=m_S.coeff(f+1,f+0), a22=m_S.coeff(f+1,f+1), a32=m_S.coeff(f+2,f+1),
00413           b12=m_T.coeff(f+0,f+1),
00414           b11i=Scalar(1.0)/m_T.coeff(f+0,f+0),
00415           b22i=Scalar(1.0)/m_T.coeff(f+1,f+1),
00416           a87=m_S.coeff(l-1,l-2),
00417           a98=m_S.coeff(l-0,l-1),
00418           b77i=Scalar(1.0)/m_T.coeff(l-2,l-2),
00419           b88i=Scalar(1.0)/m_T.coeff(l-1,l-1);
00420         Scalar ss = abs(a87*b77i) + abs(a98*b88i),
00421                lpl = Scalar(1.5)*ss,
00422                ll = ss*ss;
00423         x = ll + a11*a11*b11i*b11i - lpl*a11*b11i + a12*a21*b11i*b22i
00424           - a11*a21*b12*b11i*b11i*b22i;
00425         y = a11*a21*b11i*b11i - lpl*a21*b11i + a21*a22*b11i*b22i 
00426           - a21*a21*b12*b11i*b11i*b22i;
00427         z = a21*a32*b11i*b22i;
00428       }
00429       else if (iter==16)
00430       {
00431         // another exceptional shift
00432         x = m_S.coeff(f,f)/m_T.coeff(f,f)-m_S.coeff(l,l)/m_T.coeff(l,l) + m_S.coeff(l,l-1)*m_T.coeff(l-1,l) /
00433           (m_T.coeff(l-1,l-1)*m_T.coeff(l,l));
00434         y = m_S.coeff(f+1,f)/m_T.coeff(f,f);
00435         z = 0;
00436       }
00437       else if (iter>23 && !(iter%8))
00438       {
00439         // extremely exceptional shift
00440         x = internal::random<Scalar>(-1.0,1.0);
00441         y = internal::random<Scalar>(-1.0,1.0);
00442         z = internal::random<Scalar>(-1.0,1.0);
00443       }
00444       else
00445       {
00446         // Compute the shifts: (x,y,z,0...) = (AB^-1 - l1 I) (AB^-1 - l2 I) e1
00447         // where l1 and l2 are the eigenvalues of the 2x2 matrix C = U V^-1 where
00448         // U and V are 2x2 bottom right sub matrices of A and B. Thus:
00449         //  = AB^-1AB^-1 + l1 l2 I - (l1+l2)(AB^-1)
00450         //  = AB^-1AB^-1 + det(M) - tr(M)(AB^-1)
00451         // Since we are only interested in having x, y, z with a correct ratio, we have:
00452         const Scalar
00453           a11 = m_S.coeff(f,f),     a12 = m_S.coeff(f,f+1),
00454           a21 = m_S.coeff(f+1,f),   a22 = m_S.coeff(f+1,f+1),
00455                                     a32 = m_S.coeff(f+2,f+1),
00456 
00457           a88 = m_S.coeff(l-1,l-1), a89 = m_S.coeff(l-1,l),
00458           a98 = m_S.coeff(l,l-1),   a99 = m_S.coeff(l,l),
00459 
00460           b11 = m_T.coeff(f,f),     b12 = m_T.coeff(f,f+1),
00461                                     b22 = m_T.coeff(f+1,f+1),
00462 
00463           b88 = m_T.coeff(l-1,l-1), b89 = m_T.coeff(l-1,l),
00464                                     b99 = m_T.coeff(l,l);
00465 
00466         x = ( (a88/b88 - a11/b11)*(a99/b99 - a11/b11) - (a89/b99)*(a98/b88) + (a98/b88)*(b89/b99)*(a11/b11) ) * (b11/a21)
00467           + a12/b22 - (a11/b11)*(b12/b22);
00468         y = (a22/b22-a11/b11) - (a21/b11)*(b12/b22) - (a88/b88-a11/b11) - (a99/b99-a11/b11) + (a98/b88)*(b89/b99);
00469         z = a32/b22;
00470       }
00471 
00472       JRs G;
00473 
00474       for (Index k=f; k<=l-2; k++)
00475       {
00476         // variables for Householder reflections
00477         Vector2s essential2;
00478         Scalar tau, beta;
00479 
00480         Vector3s hr(x,y,z);
00481 
00482         // Q_k to annihilate S(k+1,k-1) and S(k+2,k-1)
00483         hr.makeHouseholderInPlace(tau, beta);
00484         essential2 = hr.template bottomRows<2>();
00485         Index fc=(std::max)(k-1,Index(0));  // first col to update
00486         m_S.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
00487         m_T.template middleRows<3>(k).rightCols(dim-fc).applyHouseholderOnTheLeft(essential2, tau, m_workspace.data());
00488         if (m_computeQZ)
00489           m_Q.template middleCols<3>(k).applyHouseholderOnTheRight(essential2, tau, m_workspace.data());
00490         if (k>f)
00491           m_S.coeffRef(k+2,k-1) = m_S.coeffRef(k+1,k-1) = Scalar(0.0);
00492 
00493         // Z_{k1} to annihilate T(k+2,k+1) and T(k+2,k)
00494         hr << m_T.coeff(k+2,k+2),m_T.coeff(k+2,k),m_T.coeff(k+2,k+1);
00495         hr.makeHouseholderInPlace(tau, beta);
00496         essential2 = hr.template bottomRows<2>();
00497         {
00498           Index lr = (std::min)(k+4,dim); // last row to update
00499           Map<Matrix<Scalar,Dynamic,1> > tmp(m_workspace.data(),lr);
00500           // S
00501           tmp = m_S.template middleCols<2>(k).topRows(lr) * essential2;
00502           tmp += m_S.col(k+2).head(lr);
00503           m_S.col(k+2).head(lr) -= tau*tmp;
00504           m_S.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
00505           // T
00506           tmp = m_T.template middleCols<2>(k).topRows(lr) * essential2;
00507           tmp += m_T.col(k+2).head(lr);
00508           m_T.col(k+2).head(lr) -= tau*tmp;
00509           m_T.template middleCols<2>(k).topRows(lr) -= (tau*tmp) * essential2.adjoint();
00510         }
00511         if (m_computeQZ)
00512         {
00513           // Z
00514           Map<Matrix<Scalar,1,Dynamic> > tmp(m_workspace.data(),dim);
00515           tmp = essential2.adjoint()*(m_Z.template middleRows<2>(k));
00516           tmp += m_Z.row(k+2);
00517           m_Z.row(k+2) -= tau*tmp;
00518           m_Z.template middleRows<2>(k) -= essential2 * (tau*tmp);
00519         }
00520         m_T.coeffRef(k+2,k) = m_T.coeffRef(k+2,k+1) = Scalar(0.0);
00521 
00522         // Z_{k2} to annihilate T(k+1,k)
00523         G.makeGivens(m_T.coeff(k+1,k+1), m_T.coeff(k+1,k));
00524         m_S.applyOnTheRight(k+1,k,G);
00525         m_T.applyOnTheRight(k+1,k,G);
00526         // update Z
00527         if (m_computeQZ)
00528           m_Z.applyOnTheLeft(k+1,k,G.adjoint());
00529         m_T.coeffRef(k+1,k) = Scalar(0.0);
00530 
00531         // update x,y,z
00532         x = m_S.coeff(k+1,k);
00533         y = m_S.coeff(k+2,k);
00534         if (k < l-2)
00535           z = m_S.coeff(k+3,k);
00536       } // loop over k
00537 
00538       // Q_{n-1} to annihilate y = S(l,l-2)
00539       G.makeGivens(x,y);
00540       m_S.applyOnTheLeft(l-1,l,G.adjoint());
00541       m_T.applyOnTheLeft(l-1,l,G.adjoint());
00542       if (m_computeQZ)
00543         m_Q.applyOnTheRight(l-1,l,G);
00544       m_S.coeffRef(l,l-2) = Scalar(0.0);
00545 
00546       // Z_{n-1} to annihilate T(l,l-1)
00547       G.makeGivens(m_T.coeff(l,l),m_T.coeff(l,l-1));
00548       m_S.applyOnTheRight(l,l-1,G);
00549       m_T.applyOnTheRight(l,l-1,G);
00550       if (m_computeQZ)
00551         m_Z.applyOnTheLeft(l,l-1,G.adjoint());
00552       m_T.coeffRef(l,l-1) = Scalar(0.0);
00553     }
00554 
00555 
00556   template<typename MatrixType>
00557     RealQZ<MatrixType>& RealQZ<MatrixType>::compute(const MatrixType& A_in, const MatrixType& B_in, bool computeQZ)
00558     {
00559 
00560       const Index dim = A_in.cols();
00561 
00562       eigen_assert (A_in.rows()==dim && A_in.cols()==dim 
00563           && B_in.rows()==dim && B_in.cols()==dim 
00564           && "Need square matrices of the same dimension");
00565 
00566       m_isInitialized = true;
00567       m_computeQZ = computeQZ;
00568       m_S = A_in; m_T = B_in;
00569       m_workspace.resize(dim*2);
00570       m_global_iter = 0;
00571 
00572       // entrance point: hessenberg triangular decomposition
00573       hessenbergTriangular();
00574       // compute L1 vector norms of T, S into m_normOfS, m_normOfT
00575       computeNorms();
00576 
00577       Index l = dim-1, 
00578             f, 
00579             local_iter = 0;
00580 
00581       while (l>0 && local_iter<m_maxIters)
00582       {
00583         f = findSmallSubdiagEntry(l);
00584         // now rows and columns f..l (including) decouple from the rest of the problem
00585         if (f>0) m_S.coeffRef(f,f-1) = Scalar(0.0);
00586         if (f == l) // One root found
00587         {
00588           l--;
00589           local_iter = 0;
00590         }
00591         else if (f == l-1) // Two roots found
00592         {
00593           splitOffTwoRows(f);
00594           l -= 2;
00595           local_iter = 0;
00596         }
00597         else // No convergence yet
00598         {
00599           // if there's zero on diagonal of T, we can isolate an eigenvalue with Givens rotations
00600           Index z = findSmallDiagEntry(f,l);
00601           if (z>=f)
00602           {
00603             // zero found
00604             pushDownZero(z,f,l);
00605           }
00606           else
00607           {
00608             // We are sure now that S.block(f,f, l-f+1,l-f+1) is underuced upper-Hessenberg 
00609             // and T.block(f,f, l-f+1,l-f+1) is invertible uper-triangular, which allows to
00610             // apply a QR-like iteration to rows and columns f..l.
00611             step(f,l, local_iter);
00612             local_iter++;
00613             m_global_iter++;
00614           }
00615         }
00616       }
00617       // check if we converged before reaching iterations limit
00618       m_info = (local_iter<m_maxIters) ? Success : NoConvergence;
00619       return *this;
00620     } // end compute
00621 
00622 } // end namespace Eigen
00623 
00624 #endif //EIGEN_REAL_QZ


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:52