MatrixFunctionAtomic.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) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk>
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_MATRIX_FUNCTION_ATOMIC
00011 #define EIGEN_MATRIX_FUNCTION_ATOMIC
00012 
00013 namespace Eigen { 
00014 
00023 template <typename MatrixType>
00024 class MatrixFunctionAtomic
00025 {
00026   public:
00027 
00028     typedef typename MatrixType::Scalar Scalar;
00029     typedef typename MatrixType::Index Index;
00030     typedef typename NumTraits<Scalar>::Real RealScalar;
00031     typedef typename internal::stem_function<Scalar>::type StemFunction;
00032     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
00033 
00037     MatrixFunctionAtomic(StemFunction f) : m_f(f) { }
00038 
00043     MatrixType compute(const MatrixType& A);
00044 
00045   private:
00046 
00047     // Prevent copying
00048     MatrixFunctionAtomic(const MatrixFunctionAtomic&);
00049     MatrixFunctionAtomic& operator=(const MatrixFunctionAtomic&);
00050 
00051     void computeMu();
00052     bool taylorConverged(Index s, const MatrixType& F, const MatrixType& Fincr, const MatrixType& P);
00053 
00055     StemFunction* m_f;
00056 
00058     Index m_Arows;
00059 
00061     Scalar m_avgEival;
00062 
00064     MatrixType m_Ashifted;
00065 
00067     RealScalar m_mu;
00068 };
00069 
00070 template <typename MatrixType>
00071 MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A)
00072 {
00073   // TODO: Use that A is upper triangular
00074   m_Arows = A.rows();
00075   m_avgEival = A.trace() / Scalar(RealScalar(m_Arows));
00076   m_Ashifted = A - m_avgEival * MatrixType::Identity(m_Arows, m_Arows);
00077   computeMu();
00078   MatrixType F = m_f(m_avgEival, 0) * MatrixType::Identity(m_Arows, m_Arows);
00079   MatrixType P = m_Ashifted;
00080   MatrixType Fincr;
00081   for (Index s = 1; s < 1.1 * m_Arows + 10; s++) { // upper limit is fairly arbitrary
00082     Fincr = m_f(m_avgEival, static_cast<int>(s)) * P;
00083     F += Fincr;
00084     P = Scalar(RealScalar(1.0/(s + 1))) * P * m_Ashifted;
00085     if (taylorConverged(s, F, Fincr, P)) {
00086       return F;
00087     }
00088   }
00089   eigen_assert("Taylor series does not converge" && 0);
00090   return F;
00091 }
00092 
00094 template <typename MatrixType>
00095 void MatrixFunctionAtomic<MatrixType>::computeMu()
00096 {
00097   const MatrixType N = MatrixType::Identity(m_Arows, m_Arows) - m_Ashifted;
00098   VectorType e = VectorType::Ones(m_Arows);
00099   N.template triangularView<Upper>().solveInPlace(e);
00100   m_mu = e.cwiseAbs().maxCoeff();
00101 }
00102 
00104 template <typename MatrixType>
00105 bool MatrixFunctionAtomic<MatrixType>::taylorConverged(Index s, const MatrixType& F,
00106                                                        const MatrixType& Fincr, const MatrixType& P)
00107 {
00108   const Index n = F.rows();
00109   const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
00110   const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff();
00111   if (Fincr_norm < NumTraits<Scalar>::epsilon() * F_norm) {
00112     RealScalar delta = 0;
00113     RealScalar rfactorial = 1;
00114     for (Index r = 0; r < n; r++) {
00115       RealScalar mx = 0;
00116       for (Index i = 0; i < n; i++)
00117         mx = (std::max)(mx, std::abs(m_f(m_Ashifted(i, i) + m_avgEival, static_cast<int>(s+r))));
00118       if (r != 0)
00119         rfactorial *= RealScalar(r);
00120       delta = (std::max)(delta, mx / rfactorial);
00121     }
00122     const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
00123     if (m_mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm)
00124       return true;
00125   }
00126   return false;
00127 }
00128 
00129 } // end namespace Eigen
00130 
00131 #endif // EIGEN_MATRIX_FUNCTION_ATOMIC


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