matrix exponentiation function for armadillo - calculate the matrix exponential exp(A) in matlab/octave: expm(A)

gistfile1.cpp

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
//
//  Reference:
//
//    Cleve Moler, Charles VanLoan,
//    Nineteen Dubious Ways to Compute the Exponential of a Matrix,
//    Twenty-Five Years Later,
//    SIAM Review,
//    Volume 45, Number 1, March 2003, pages 3-49.
//    This code is based on John Burkhard's matrix_exponential.cpp
//
#include <armadillo>

template<typename eT>
Mat<eT> expm(const Mat<eT> & a)
{
  typedef typename get_pod_type<eT>::result  T;
  int n = a.n_rows;
  T c;
  Mat<eT> a2, d, e, x;
  int ee;
  int k;
  int p;
  const int q = 6;
  int s;

  ee = (int) log2(norm(a, "inf")) + 1;
  s = MAX(0, ee + 1);
  a2 = a * (1.0 / pow(2.0, s));
  x = a2;
  c = 0.5;
  e = eye(n,n) + c * a2;
  d = eye(n,n) + -c * a2;
  p = 1;

  for (k = 2; k <= q; k++) {
    c = c * (T) (q - k + 1) / (T) (k * (2 * q - k + 1));
    x = a2 * x;
    e += c * x;
    if (p) {
      d += c * x;
    }
    else {
      d += -c * x;
    }
    p = !p;
  }
  e = solve(d, e);
  for (k = 1; k <= s; k++) {
    e = e * e;
  }

  return e;
}