11 #ifndef EIGEN_MATRIX_LOGARITHM 12 #define EIGEN_MATRIX_LOGARITHM 15 #define M_PI 3.141592653589793238462643383279503L 30 template <
typename MatrixType>
35 typedef typename MatrixType::Scalar
Scalar;
48 MatrixType
compute(
const MatrixType&
A);
52 void compute2x2(
const MatrixType& A, MatrixType& result);
53 void computeBig(
const MatrixType& A, MatrixType& result);
57 void computePade(MatrixType& result,
const MatrixType&
T,
int degree);
58 void computePade3(MatrixType& result,
const MatrixType& T);
59 void computePade4(MatrixType& result,
const MatrixType& T);
60 void computePade5(MatrixType& result,
const MatrixType& T);
61 void computePade6(MatrixType& result,
const MatrixType& T);
62 void computePade7(MatrixType& result,
const MatrixType& T);
63 void computePade8(MatrixType& result,
const MatrixType& T);
64 void computePade9(MatrixType& result,
const MatrixType& T);
69 static const int maxPadeDegree = std::numeric_limits<RealScalar>::digits<= 24? 5:
70 std::numeric_limits<RealScalar>::digits<= 53? 7:
71 std::numeric_limits<RealScalar>::digits<= 64? 8:
72 std::numeric_limits<RealScalar>::digits<=106? 10:
81 template <
typename MatrixType>
85 MatrixType result(A.rows(), A.rows());
87 result(0,0) =
log(
A(0,0));
88 else if (A.rows() == 2)
96 template <
typename MatrixType>
107 result(0,0) = logA00;
109 result(1,1) = logA11;
111 if (
A(0,0) ==
A(1,1)) {
112 result(0,1) =
A(0,1) /
A(0,0);
113 }
else if ((
abs(
A(0,0)) < 0.5*
abs(
A(1,1))) || (
abs(
A(0,0)) > 2*
abs(
A(1,1)))) {
114 result(0,1) =
A(0,1) * (logA11 - logA00) / (
A(1,1) -
A(0,0));
117 int unwindingNumber =
static_cast<int>(ceil((
imag(logA11 - logA00) -
M_PI) / (2*
M_PI)));
119 result(0,1) =
A(0,1) * (
Scalar(2) * numext::atanh2(y,x) +
Scalar(0,2*
M_PI*unwindingNumber)) / y;
125 template <
typename MatrixType>
129 int numberOfSquareRoots = 0;
130 int numberOfExtraSquareRoots = 0;
132 MatrixType
T =
A, sqrtT;
137 1.1880960220216759245467951592883642e-1
L;
140 RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
141 if (normTminusI < maxNormForPade) {
144 if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1))
146 ++numberOfExtraSquareRoots;
149 T = sqrtT.template triangularView<Upper>();
150 ++numberOfSquareRoots;
158 template <
typename MatrixType>
161 const float maxNormForPade[] = { 2.5111573934555054e-1 , 4.0535837411880493e-1,
162 5.3149729967117310e-1 };
171 template <
typename MatrixType>
174 const double maxNormForPade[] = { 1.6206284795015624e-2 , 5.3873532631381171e-2,
175 1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
184 template <
typename MatrixType>
187 #if LDBL_MANT_DIG == 53 // double precision 188 const long double maxNormForPade[] = { 1.6206284795015624e-2
L , 5.3873532631381171e-2
L,
189 1.1352802267628681e-1
L, 1.8662860613541288e-1
L, 2.642960831111435e-1
L };
190 #elif LDBL_MANT_DIG <= 64 // extended precision 191 const long double maxNormForPade[] = { 5.48256690357782863103e-3
L , 2.34559162387971167321e-2
L,
192 5.84603923897347449857e-2
L, 1.08486423756725170223e-1
L, 1.68385767881294446649e-1
L,
193 2.32777776523703892094e-1
L };
194 #elif LDBL_MANT_DIG <= 106 // double-double 195 const long double maxNormForPade[] = { 8.58970550342939562202529664318890e-5
L ,
196 9.34074328446359654039446552677759e-4
L, 4.26117194647672175773064114582860e-3
L,
197 1.21546224740281848743149666560464e-2
L, 2.61100544998339436713088248557444e-2
L,
198 4.66170074627052749243018566390567e-2
L, 7.32585144444135027565872014932387e-2
L,
199 1.05026503471351080481093652651105e-1
L };
200 #else // quadruple precision 201 const long double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5
L ,
202 5.8853168473544560470387769480192666e-4
L, 2.9216120366601315391789493628113520e-3
L,
203 8.8415758124319434347116734705174308e-3
L, 1.9850836029449446668518049562565291e-2
L,
204 3.6688019729653446926585242192447447e-2
L, 5.9290962294020186998954055264528393e-2
L,
205 8.6998436081634343903250580992127677e-2
L, 1.1880960220216759245467951592883642e-1
L };
215 template <
typename MatrixType>
232 template <
typename MatrixType>
235 const int degree = 3;
236 const RealScalar nodes[] = { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,
237 0.8872983346207416885179265399782400L };
238 const RealScalar weights[] = { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,
239 0.2777777777777777777777777777777778L };
241 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
242 result.setZero(T.rows(), T.rows());
243 for (
int k = 0; k < degree; ++k)
244 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
245 .template triangularView<Upper>().solve(TminusI);
248 template <
typename MatrixType>
251 const int degree = 4;
252 const RealScalar nodes[] = { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,
253 0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L };
254 const RealScalar weights[] = { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,
255 0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L };
257 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
258 result.setZero(T.rows(), T.rows());
259 for (
int k = 0; k < degree; ++k)
260 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
261 .template triangularView<Upper>().solve(TminusI);
264 template <
typename MatrixType>
267 const int degree = 5;
268 const RealScalar nodes[] = { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,
269 0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
270 0.9530899229693319963988134391496965L };
271 const RealScalar weights[] = { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,
272 0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
273 0.1184634425280945437571320203599587L };
275 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
276 result.setZero(T.rows(), T.rows());
277 for (
int k = 0; k < degree; ++k)
278 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
279 .template triangularView<Upper>().solve(TminusI);
282 template <
typename MatrixType>
285 const int degree = 6;
286 const RealScalar nodes[] = { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,
287 0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
288 0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L };
289 const RealScalar weights[] = { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,
290 0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
291 0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L };
293 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
294 result.setZero(T.rows(), T.rows());
295 for (
int k = 0; k < degree; ++k)
296 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
297 .template triangularView<Upper>().solve(TminusI);
300 template <
typename MatrixType>
303 const int degree = 7;
304 const RealScalar nodes[] = { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,
305 0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
306 0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
307 0.9745539561713792622630948420239256L };
308 const RealScalar weights[] = { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,
309 0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
310 0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
311 0.0647424830844348466353057163395410L };
313 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
314 result.setZero(T.rows(), T.rows());
315 for (
int k = 0; k < degree; ++k)
316 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
317 .template triangularView<Upper>().solve(TminusI);
320 template <
typename MatrixType>
323 const int degree = 8;
324 const RealScalar nodes[] = { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,
325 0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
326 0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
327 0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L };
328 const RealScalar weights[] = { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,
329 0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
330 0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
331 0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L };
333 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
334 result.setZero(T.rows(), T.rows());
335 for (
int k = 0; k < degree; ++k)
336 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
337 .template triangularView<Upper>().solve(TminusI);
340 template <
typename MatrixType>
343 const int degree = 9;
344 const RealScalar nodes[] = { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,
345 0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
346 0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
347 0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
348 0.9840801197538130449177881014518364L };
349 const RealScalar weights[] = { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,
350 0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
351 0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
352 0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
353 0.0406371941807872059859460790552618L };
355 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
356 result.setZero(T.rows(), T.rows());
357 for (
int k = 0; k < degree; ++k)
358 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
359 .template triangularView<Upper>().solve(TminusI);
362 template <
typename MatrixType>
365 const int degree = 10;
366 const RealScalar nodes[] = { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,
367 0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
368 0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
369 0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
370 0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L };
371 const RealScalar weights[] = { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,
372 0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
373 0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
374 0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
375 0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L };
377 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
378 result.setZero(T.rows(), T.rows());
379 for (
int k = 0; k < degree; ++k)
380 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
381 .template triangularView<Upper>().solve(TminusI);
384 template <
typename MatrixType>
387 const int degree = 11;
388 const RealScalar nodes[] = { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,
389 0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
390 0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
391 0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
392 0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
393 0.9891143290730284964019690005614287L };
394 const RealScalar weights[] = { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,
395 0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
396 0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
397 0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
398 0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
399 0.0278342835580868332413768602212743L };
401 MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
402 result.setZero(T.rows(), T.rows());
403 for (
int k = 0; k < degree; ++k)
404 result += weights[k] * (MatrixType::Identity(T.rows(), T.rows()) + nodes[k] * TminusI)
405 .template triangularView<Upper>().solve(TminusI);
426 typedef typename Derived::Index
Index;
438 template <
typename ResultType>
439 inline void evalTo(ResultType& result)
const 441 typedef typename Derived::PlainObject PlainObject;
443 static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
444 static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
445 static const int Options = PlainObject::Options;
446 typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
451 const PlainObject Aevaluated = m_A.eval();
456 Index
rows()
const {
return m_A.rows(); }
457 Index
cols()
const {
return m_A.cols(); }
466 template<
typename Derived>
477 template <
typename Derived>
486 #endif // EIGEN_MATRIX_LOGARITHM void computePade4(MatrixType &result, const MatrixType &T)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_pow_op< typename Derived::Scalar >, const Derived > pow(const Eigen::ArrayBase< Derived > &x, const typename Derived::Scalar &exponent)
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
void evalTo(ResultType &result) const
Compute the matrix logarithm.
static const int minPadeDegree
void computePade6(MatrixType &result, const MatrixType &T)
NumTraits< Scalar >::Real RealScalar
Derived::PlainObject ReturnType
MatrixType::Scalar Scalar
MatrixLogarithmReturnValue(const Derived &A)
Constructor.
void compute2x2(const MatrixType &A, MatrixType &result)
Compute logarithm of 2x2 triangular matrix.
const MatrixLogarithmReturnValue< Derived > log() const
iterative scaling algorithm to equilibrate rows and column norms in matrices
void computePade9(MatrixType &result, const MatrixType &T)
DerType::Scalar imag(const AutoDiffScalar< DerType > &)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
void computeBig(const MatrixType &A, MatrixType &result)
Compute logarithm of triangular matrices with size > 2.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
void compute(ResultType &result)
Compute the matrix function.
void computePade(MatrixType &result, const MatrixType &T, int degree)
Class for computing matrix functions.
MatrixLogarithmAtomic & operator=(const MatrixLogarithmAtomic &)
const ImagReturnType imag() const
void computePade8(MatrixType &result, const MatrixType &T)
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
MatrixType compute(const MatrixType &A)
Compute matrix logarithm of atomic matrix.
Provides a generic way to set and pass user-specified options.
void computePade11(MatrixType &result, const MatrixType &T)
Class for computing matrix square roots of upper triangular matrices.
void computePade5(MatrixType &result, const MatrixType &T)
int getPadeDegree(float normTminusI)
static const int maxPadeDegree
void computePade3(MatrixType &result, const MatrixType &T)
void computePade7(MatrixType &result, const MatrixType &T)
Helper class for computing matrix logarithm of atomic matrices.
void computePade10(MatrixType &result, const MatrixType &T)
The matrix class, also used for vectors and row-vectors.
MatrixLogarithmAtomic()
Constructor.
Proxy for the matrix logarithm of some matrix (expression).
internal::nested< Derived >::type m_A
IntermediateState log(const Expression &arg)