WPILibC++ 2024.3.2
MatrixExponential.h
Go to the documentation of this file.
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009, 2010, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
5// Copyright (C) 2011, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
6//
7// This Source Code Form is subject to the terms of the Mozilla
8// Public License v. 2.0. If a copy of the MPL was not distributed
9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10
11#ifndef EIGEN_MATRIX_EXPONENTIAL
12#define EIGEN_MATRIX_EXPONENTIAL
13
14#include "StemFunction.h"
15
16// IWYU pragma: private
18
19namespace Eigen {
20namespace internal {
21
22/** \brief Scaling operator.
23 *
24 * This struct is used by CwiseUnaryOp to scale a matrix by \f$2^{-s} \f$.
25 */
26template <typename RealScalar>
28 /** \brief Constructor.
29 *
30 * \param[in] squarings The integer \f$s \f$ in this document.
31 */
32 MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) {}
33
34 /** \brief Scale a matrix coefficient.
35 *
36 * \param[in,out] x The scalar to be scaled, becoming \f$2^{-s} x \f$.
37 */
38 inline const RealScalar operator()(const RealScalar& x) const {
39 using std::ldexp;
40 return ldexp(x, -m_squarings);
41 }
42
43 typedef std::complex<RealScalar> ComplexScalar;
44
45 /** \brief Scale a matrix coefficient.
46 *
47 * \param[in,out] x The scalar to be scaled, becoming \f$2^{-s} x \f$.
48 */
49 inline const ComplexScalar operator()(const ComplexScalar& x) const {
50 using std::ldexp;
51 return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings));
52 }
53
54 private:
55 int m_squarings;
56};
57
58/** \brief Compute the (3,3)-Pad&eacute; approximant to the exponential.
59 *
60 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
61 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
62 */
63template <typename MatA, typename MatU, typename MatV>
64void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V) {
65 typedef typename MatA::PlainObject MatrixType;
66 typedef typename NumTraits<typename traits<MatA>::Scalar>::Real RealScalar;
67 const RealScalar b[] = {120.L, 60.L, 12.L, 1.L};
68 const MatrixType A2 = A * A;
69 const MatrixType tmp = b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
70 U.noalias() = A * tmp;
71 V = b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
72}
73
74/** \brief Compute the (5,5)-Pad&eacute; approximant to the exponential.
75 *
76 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
77 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
78 */
79template <typename MatA, typename MatU, typename MatV>
80void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V) {
81 typedef typename MatA::PlainObject MatrixType;
82 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
83 const RealScalar b[] = {30240.L, 15120.L, 3360.L, 420.L, 30.L, 1.L};
84 const MatrixType A2 = A * A;
85 const MatrixType A4 = A2 * A2;
86 const MatrixType tmp = b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
87 U.noalias() = A * tmp;
88 V = b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
89}
90
91/** \brief Compute the (7,7)-Pad&eacute; approximant to the exponential.
92 *
93 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
94 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
95 */
96template <typename MatA, typename MatU, typename MatV>
97void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V) {
98 typedef typename MatA::PlainObject MatrixType;
99 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
100 const RealScalar b[] = {17297280.L, 8648640.L, 1995840.L, 277200.L, 25200.L, 1512.L, 56.L, 1.L};
101 const MatrixType A2 = A * A;
102 const MatrixType A4 = A2 * A2;
103 const MatrixType A6 = A4 * A2;
104 const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
105 U.noalias() = A * tmp;
106 V = b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
107}
108
109/** \brief Compute the (9,9)-Pad&eacute; approximant to the exponential.
110 *
111 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
112 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
113 */
114template <typename MatA, typename MatU, typename MatV>
115void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V) {
116 typedef typename MatA::PlainObject MatrixType;
117 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
118 const RealScalar b[] = {17643225600.L, 8821612800.L, 2075673600.L, 302702400.L, 30270240.L,
119 2162160.L, 110880.L, 3960.L, 90.L, 1.L};
120 const MatrixType A2 = A * A;
121 const MatrixType A4 = A2 * A2;
122 const MatrixType A6 = A4 * A2;
123 const MatrixType A8 = A6 * A2;
124 const MatrixType tmp =
125 b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
126 U.noalias() = A * tmp;
127 V = b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
128}
129
130/** \brief Compute the (13,13)-Pad&eacute; approximant to the exponential.
131 *
132 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
133 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
134 */
135template <typename MatA, typename MatU, typename MatV>
136void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V) {
137 typedef typename MatA::PlainObject MatrixType;
138 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
139 const RealScalar b[] = {64764752532480000.L,
140 32382376266240000.L,
141 7771770303897600.L,
142 1187353796428800.L,
143 129060195264000.L,
144 10559470521600.L,
145 670442572800.L,
146 33522128640.L,
147 1323241920.L,
148 40840800.L,
149 960960.L,
150 16380.L,
151 182.L,
152 1.L};
153 const MatrixType A2 = A * A;
154 const MatrixType A4 = A2 * A2;
155 const MatrixType A6 = A4 * A2;
156 V = b[13] * A6 + b[11] * A4 + b[9] * A2; // used for temporary storage
157 MatrixType tmp = A6 * V;
158 tmp += b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
159 U.noalias() = A * tmp;
160 tmp = b[12] * A6 + b[10] * A4 + b[8] * A2;
161 V.noalias() = A6 * tmp;
162 V += b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
163}
164
165/** \brief Compute the (17,17)-Pad&eacute; approximant to the exponential.
166 *
167 * After exit, \f$(V+U)(V-U)^{-1} \f$ is the Pad&eacute;
168 * approximant of \f$\exp(A) \f$ around \f$A = 0 \f$.
169 *
170 * This function activates only if your long double is double-double or quadruple.
171 */
172#if LDBL_MANT_DIG > 64
173template <typename MatA, typename MatU, typename MatV>
174void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V) {
175 typedef typename MatA::PlainObject MatrixType;
176 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
177 const RealScalar b[] = {830034394580628357120000.L,
178 415017197290314178560000.L,
179 100610229646136770560000.L,
180 15720348382208870400000.L,
181 1774878043152614400000.L,
182 153822763739893248000.L,
183 10608466464820224000.L,
184 595373117923584000.L,
185 27563570274240000.L,
186 1060137318240000.L,
187 33924394183680.L,
188 899510451840.L,
189 19554575040.L,
190 341863200.L,
191 4651200.L,
192 46512.L,
193 306.L,
194 1.L};
195 const MatrixType A2 = A * A;
196 const MatrixType A4 = A2 * A2;
197 const MatrixType A6 = A4 * A2;
198 const MatrixType A8 = A4 * A4;
199 V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2; // used for temporary storage
200 MatrixType tmp = A8 * V;
201 tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
202 U.noalias() = A * tmp;
203 tmp = b[16] * A8 + b[14] * A6 + b[12] * A4 + b[10] * A2;
204 V.noalias() = tmp * A8;
205 V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
206}
207#endif
208
209template <typename MatrixType, typename RealScalar = typename NumTraits<typename traits<MatrixType>::Scalar>::Real>
211 /** \brief Compute Pad&eacute; approximant to the exponential.
212 *
213 * Computes \c U, \c V and \c squarings such that \f$(V+U)(V-U)^{-1} \f$ is a Pad&eacute;
214 * approximant of \f$\exp(2^{-\mbox{squarings}}M) \f$ around \f$M = 0 \f$, where \f$M \f$
215 * denotes the matrix \c arg. The degree of the Pad&eacute; approximant and the value of squarings
216 * are chosen such that the approximation error is no more than the round-off error.
217 */
218 static void run(const MatrixType& arg, MatrixType& U, MatrixType& V, int& squarings);
219};
220
221template <typename MatrixType>
222struct matrix_exp_computeUV<MatrixType, float> {
223 template <typename ArgType>
224 static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
225 using std::frexp;
226 using std::pow;
227 const float l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
228 squarings = 0;
229 if (l1norm < 4.258730016922831e-001f) {
231 } else if (l1norm < 1.880152677804762e+000f) {
233 } else {
234 const float maxnorm = 3.925724783138660f;
235 frexp(l1norm / maxnorm, &squarings);
236 if (squarings < 0) squarings = 0;
237 MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<float>(squarings));
239 }
240 }
241};
242
243template <typename MatrixType>
244struct matrix_exp_computeUV<MatrixType, double> {
245 typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
246 template <typename ArgType>
247 static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
248 using std::frexp;
249 using std::pow;
250 const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
251 squarings = 0;
252 if (l1norm < 1.495585217958292e-002) {
254 } else if (l1norm < 2.539398330063230e-001) {
256 } else if (l1norm < 9.504178996162932e-001) {
258 } else if (l1norm < 2.097847961257068e+000) {
260 } else {
261 const RealScalar maxnorm = 5.371920351148152;
262 frexp(l1norm / maxnorm, &squarings);
263 if (squarings < 0) squarings = 0;
264 MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<RealScalar>(squarings));
266 }
267 }
268};
269
270template <typename MatrixType>
271struct matrix_exp_computeUV<MatrixType, long double> {
272 template <typename ArgType>
273 static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings) {
274#if LDBL_MANT_DIG == 53 // double precision
276
277#else
278
279 using std::frexp;
280 using std::pow;
281 const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
282 squarings = 0;
283
284#if LDBL_MANT_DIG <= 64 // extended precision
285
286 if (l1norm < 4.1968497232266989671e-003L) {
288 } else if (l1norm < 1.1848116734693823091e-001L) {
290 } else if (l1norm < 5.5170388480686700274e-001L) {
292 } else if (l1norm < 1.3759868875587845383e+000L) {
294 } else {
295 const long double maxnorm = 4.0246098906697353063L;
296 frexp(l1norm / maxnorm, &squarings);
297 if (squarings < 0) squarings = 0;
298 MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
300 }
301
302#elif LDBL_MANT_DIG <= 106 // double-double
303
304 if (l1norm < 3.2787892205607026992947488108213e-005L) {
306 } else if (l1norm < 6.4467025060072760084130906076332e-003L) {
308 } else if (l1norm < 6.8988028496595374751374122881143e-002L) {
310 } else if (l1norm < 2.7339737518502231741495857201670e-001L) {
312 } else if (l1norm < 1.3203382096514474905666448850278e+000L) {
314 } else {
315 const long double maxnorm = 3.2579440895405400856599663723517L;
316 frexp(l1norm / maxnorm, &squarings);
317 if (squarings < 0) squarings = 0;
318 MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
320 }
321
322#elif LDBL_MANT_DIG <= 113 // quadruple precision
323
324 if (l1norm < 1.639394610288918690547467954466970e-005L) {
326 } else if (l1norm < 4.253237712165275566025884344433009e-003L) {
328 } else if (l1norm < 5.125804063165764409885122032933142e-002L) {
330 } else if (l1norm < 2.170000765161155195453205651889853e-001L) {
332 } else if (l1norm < 1.125358383453143065081397882891878e+000L) {
334 } else {
335 const long double maxnorm = 2.884233277829519311757165057717815L;
336 frexp(l1norm / maxnorm, &squarings);
337 if (squarings < 0) squarings = 0;
338 MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
340 }
341
342#else
343
344 // this case should be handled in compute()
345 eigen_assert(false && "Bug in MatrixExponential");
346
347#endif
348#endif // LDBL_MANT_DIG
349 }
350};
351
352template <typename T>
353struct is_exp_known_type : false_type {};
354template <>
355struct is_exp_known_type<float> : true_type {};
356template <>
357struct is_exp_known_type<double> : true_type {};
358#if LDBL_MANT_DIG <= 113
359template <>
360struct is_exp_known_type<long double> : true_type {};
361#endif
362
363template <typename ArgType, typename ResultType>
364void matrix_exp_compute(const ArgType& arg, ResultType& result, true_type) // natively supported scalar type
365{
366 typedef typename ArgType::PlainObject MatrixType;
367 MatrixType U, V;
368 int squarings;
369 matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
370 MatrixType numer = U + V;
371 MatrixType denom = -U + V;
372 result = denom.partialPivLu().solve(numer);
373 for (int i = 0; i < squarings; i++) result *= result; // undo scaling by repeated squaring
374}
375
376/* Computes the matrix exponential
377 *
378 * \param arg argument of matrix exponential (should be plain object)
379 * \param result variable in which result will be stored
380 */
381template <typename ArgType, typename ResultType>
382void matrix_exp_compute(const ArgType& arg, ResultType& result, false_type) // default
383{
384 typedef typename ArgType::PlainObject MatrixType;
385 typedef typename traits<MatrixType>::Scalar Scalar;
386 typedef typename NumTraits<Scalar>::Real RealScalar;
387 typedef typename std::complex<RealScalar> ComplexScalar;
388 result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
389}
390
391} // namespace internal
392
393/** \ingroup MatrixFunctions_Module
394 *
395 * \brief Proxy for the matrix exponential of some matrix (expression).
396 *
397 * \tparam Derived Type of the argument to the matrix exponential.
398 *
399 * This class holds the argument to the matrix exponential until it is assigned or evaluated for
400 * some other reason (so the argument should not be changed in the meantime). It is the return type
401 * of MatrixBase::exp() and most of the time this is the only way it is used.
402 */
403template <typename Derived>
404struct MatrixExponentialReturnValue : public ReturnByValue<MatrixExponentialReturnValue<Derived> > {
405 public:
406 /** \brief Constructor.
407 *
408 * \param src %Matrix (expression) forming the argument of the matrix exponential.
409 */
410 MatrixExponentialReturnValue(const Derived& src) : m_src(src) {}
411
412 /** \brief Compute the matrix exponential.
413 *
414 * \param result the matrix exponential of \p src in the constructor.
415 */
416 template <typename ResultType>
417 inline void evalTo(ResultType& result) const {
420 }
421
422 Index rows() const { return m_src.rows(); }
423 Index cols() const { return m_src.cols(); }
424
425 protected:
427};
428
429namespace internal {
430template <typename Derived>
431struct traits<MatrixExponentialReturnValue<Derived> > {
432 typedef typename Derived::PlainObject ReturnType;
433};
434} // namespace internal
435
436template <typename Derived>
438 eigen_assert(rows() == cols());
440}
441
442} // end namespace Eigen
443
444#endif // EIGEN_MATRIX_EXPONENTIAL
auto arg(const Char *name, const T &arg) -> detail::named_arg< Char, T >
\rst Returns a named argument to be used in a formatting function.
Definition: core.h:1841
dimensionless::scalar_t exp(const ScalarUnit x) noexcept
Compute exponential function.
Definition: math.h:332
void matrix_exp_pade7(const MatA &A, MatU &U, MatV &V)
Compute the (7,7)-Padé approximant to the exponential.
Definition: MatrixExponential.h:97
void matrix_exp_pade9(const MatA &A, MatU &U, MatV &V)
Compute the (9,9)-Padé approximant to the exponential.
Definition: MatrixExponential.h:115
void matrix_exp_compute(const ArgType &arg, ResultType &result, true_type)
Definition: MatrixExponential.h:364
void matrix_exp_pade3(const MatA &A, MatU &U, MatV &V)
Compute the (3,3)-Padé approximant to the exponential.
Definition: MatrixExponential.h:64
void matrix_exp_pade13(const MatA &A, MatU &U, MatV &V)
Compute the (13,13)-Padé approximant to the exponential.
Definition: MatrixExponential.h:136
void matrix_exp_pade5(const MatA &A, MatU &U, MatV &V)
Compute the (5,5)-Padé approximant to the exponential.
Definition: MatrixExponential.h:80
Definition: MatrixSquareRoot.h:16
type
Definition: core.h:556
auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition: base.h:2806
b
Definition: data.h:44
Proxy for the matrix exponential of some matrix (expression).
Definition: MatrixExponential.h:404
void evalTo(ResultType &result) const
Compute the matrix exponential.
Definition: MatrixExponential.h:417
MatrixExponentialReturnValue(const Derived &src)
Constructor.
Definition: MatrixExponential.h:410
Index rows() const
Definition: MatrixExponential.h:422
const internal::ref_selector< Derived >::type m_src
Definition: MatrixExponential.h:426
Index cols() const
Definition: MatrixExponential.h:423
Scaling operator.
Definition: MatrixExponential.h:27
const ComplexScalar operator()(const ComplexScalar &x) const
Scale a matrix coefficient.
Definition: MatrixExponential.h:49
std::complex< RealScalar > ComplexScalar
Definition: MatrixExponential.h:43
const RealScalar operator()(const RealScalar &x) const
Scale a matrix coefficient.
Definition: MatrixExponential.h:38
MatrixExponentialScalingOp(int squarings)
Constructor.
Definition: MatrixExponential.h:32
Definition: MatrixExponential.h:353
static void run(const ArgType &arg, MatrixType &U, MatrixType &V, int &squarings)
Definition: MatrixExponential.h:247
NumTraits< typenametraits< MatrixType >::Scalar >::Real RealScalar
Definition: MatrixExponential.h:245
static void run(const ArgType &arg, MatrixType &U, MatrixType &V, int &squarings)
Definition: MatrixExponential.h:224
static void run(const ArgType &arg, MatrixType &U, MatrixType &V, int &squarings)
Definition: MatrixExponential.h:273
Compute the (17,17)-Padé approximant to the exponential.
Definition: MatrixExponential.h:210
static void run(const MatrixType &arg, MatrixType &U, MatrixType &V, int &squarings)
Compute Padé approximant to the exponential.
Derived::PlainObject ReturnType
Definition: MatrixExponential.h:432