From 096199a2755494b970c0baca19baaffa12096b1b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Villemot?= <sebastien.villemot@ens.fr>
Date: Thu, 2 Aug 2012 16:05:31 +0200
Subject: [PATCH] Convert DecisionRules to Eigen

---
 mex/sources/estimation/DecisionRules.cc  | 174 ++++++++++-------------
 mex/sources/estimation/DecisionRules.hh  |  44 +++---
 mex/sources/estimation/tests/Makefile.am |   2 +-
 mex/sources/estimation/tests/test-dr.cc  |  54 +++----
 4 files changed, 120 insertions(+), 154 deletions(-)

diff --git a/mex/sources/estimation/DecisionRules.cc b/mex/sources/estimation/DecisionRules.cc
index f614f6090c..7f0234eb08 100644
--- a/mex/sources/estimation/DecisionRules.cc
+++ b/mex/sources/estimation/DecisionRules.cc
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Dynare Team
+ * Copyright (C) 2010-2012 Dynare Team
  *
  * This file is part of Dynare.
  *
@@ -23,11 +23,11 @@
 
 #include "DecisionRules.hh"
 
-DecisionRules::DecisionRules(size_t n_arg, size_t p_arg,
-                             const std::vector<size_t> &zeta_fwrd_arg,
-                             const std::vector<size_t> &zeta_back_arg,
-                             const std::vector<size_t> &zeta_mixed_arg,
-                             const std::vector<size_t> &zeta_static_arg,
+DecisionRules::DecisionRules(ptrdiff_t n_arg, ptrdiff_t p_arg,
+                             const std::vector<ptrdiff_t> &zeta_fwrd_arg,
+                             const std::vector<ptrdiff_t> &zeta_back_arg,
+                             const std::vector<ptrdiff_t> &zeta_mixed_arg,
+                             const std::vector<ptrdiff_t> &zeta_static_arg,
                              double qz_criterium) :
   n(n_arg), p(p_arg), zeta_fwrd(zeta_fwrd_arg), zeta_back(zeta_back_arg),
   zeta_mixed(zeta_mixed_arg), zeta_static(zeta_static_arg),
@@ -37,25 +37,24 @@ DecisionRules::DecisionRules(size_t n_arg, size_t p_arg,
   n_dynamic(n-n_static),
   S(n, n_static),
   A(n, n_back_mixed + n + n_fwrd_mixed),
-  D(n_fwrd + n_back + 2*n_mixed),
-  E(n_fwrd + n_back + 2*n_mixed),
-  Z_prime(n_fwrd + n_back + 2*n_mixed),
+  D(n_fwrd + n_back + 2*n_mixed, n_fwrd + n_back + 2*n_mixed),
+  E(n_fwrd + n_back + 2*n_mixed, n_fwrd + n_back + 2*n_mixed),
+  Z_prime(n_fwrd + n_back + 2*n_mixed, n_fwrd + n_back + 2*n_mixed),
   QR(n, n_static, n_back_mixed + n + n_fwrd_mixed),
   GSD(n_fwrd + n_back + 2*n_mixed, qz_criterium),
-  LU1(n_fwrd_mixed),
-  LU2(n_back_mixed),
-  LU3(n_static),
+  Solver1(n_fwrd_mixed, n_fwrd_mixed),
+  Solver2(n_back_mixed, n_back_mixed),
+  Solver3(n_static, n_static),
+  Solver4(n, n),
   Z21(n_fwrd_mixed, n_back_mixed),
-  g_y_back(n_back_mixed),
-  g_y_back_tmp(n_back_mixed),
+  g_y_back(n_back_mixed, n_back_mixed),
+  g_y_fwrd(n_fwrd_mixed, n_fwrd_mixed),
   g_y_static(n_static, n_back_mixed),
-  A0s(n_static),
+  A0s(n_static, n_static),
   A0d(n_static, n_dynamic),
   g_y_dynamic(n_dynamic, n_back_mixed),
-  g_y_static_tmp(n_fwrd_mixed, n_back_mixed),
   g_u_tmp1(n, n_back_mixed),
-  g_u_tmp2(n),
-  LU4(n)
+  g_u_tmp2(n, n)
 {
   assert(n == n_back + n_fwrd + n_mixed + n_static);
 
@@ -70,7 +69,7 @@ DecisionRules::DecisionRules(size_t n_arg, size_t p_arg,
             back_inserter(zeta_dynamic));
 
   // Compute beta_back and pi_back
-  for (size_t i = 0; i < n_back_mixed; i++)
+  for (ptrdiff_t i = 0; i < n_back_mixed; i++)
     if (find(zeta_mixed.begin(), zeta_mixed.end(), zeta_back_mixed[i])
         == zeta_mixed.end())
       pi_back.push_back(i);
@@ -78,7 +77,7 @@ DecisionRules::DecisionRules(size_t n_arg, size_t p_arg,
       beta_back.push_back(i);
 
   // Compute beta_fwrd and pi_fwrd
-  for (size_t i = 0; i < n_fwrd_mixed; i++)
+  for (ptrdiff_t i = 0; i < n_fwrd_mixed; i++)
     if (find(zeta_mixed.begin(), zeta_mixed.end(), zeta_fwrd_mixed[i])
         == zeta_mixed.end())
       pi_fwrd.push_back(i);
@@ -87,115 +86,92 @@ DecisionRules::DecisionRules(size_t n_arg, size_t p_arg,
 }
 
 void
-DecisionRules::compute(const Matrix &jacobian, Matrix &g_y, Matrix &g_u) throw (BlanchardKahnException, GeneralizedSchurDecomposition::GSDException)
+DecisionRules::compute(const MatrixXd &jacobian, MatrixXd &g_y, MatrixXd &g_u) throw (BlanchardKahnException, GeneralizedSchurDecomposition::GSDException)
 {
-  assert(jacobian.getRows() == n
-         && jacobian.getCols() == (n_back_mixed + n + n_fwrd_mixed + p));
-  assert(g_y.getRows() == n && g_y.getCols() == n_back_mixed);
-  assert(g_u.getRows() == n && g_u.getCols() == p);
+  assert(jacobian.rows() == n
+         && jacobian.cols() == (n_back_mixed + n + n_fwrd_mixed + p));
+  assert(g_y.rows() == n && g_y.cols() == n_back_mixed);
+  assert(g_u.rows() == n && g_u.cols() == p);
 
   // Construct S, perform QR decomposition and get A = Q*jacobian
-  for (size_t i = 0; i < n_static; i++)
-    mat::col_copy(jacobian, n_back_mixed+zeta_static[i], S, i);
+  for (ptrdiff_t i = 0; i < n_static; i++)
+    S.col(i) = jacobian.col(n_back_mixed+zeta_static[i]);
 
-  A = MatrixConstView(jacobian, 0, 0, n, n_back_mixed + n + n_fwrd_mixed);
+  A = jacobian.block(0, 0, n, n_back_mixed + n + n_fwrd_mixed);
   QR.computeAndLeftMultByQ(S, "T", A);
 
   // Construct matrix D
-  D.setAll(0.0);
-  for (size_t i = 0; i < n_mixed; i++)
+  D.setZero();
+  for (ptrdiff_t i = 0; i < n_mixed; i++)
     D(n - n_static + i, beta_back[i]) = 1.0;
-  for (size_t j = 0; j < n_back_mixed; j++)
-    mat::col_copy(A, n_back_mixed + zeta_back_mixed[j], n_static, n - n_static,
-                  D, j, 0);
-  MatrixView(D, 0, n_back_mixed, n - n_static, n_fwrd_mixed) = MatrixView(A, n_static, n_back_mixed + n, n - n_static, n_fwrd_mixed);
+  for (ptrdiff_t j = 0; j < n_back_mixed; j++)
+    D.col(j).head(n-n_static) = A.col(n_back_mixed + zeta_back_mixed[j]).tail(n-n_static);
+  
+  D.block(0, n_back_mixed, n - n_static, n_fwrd_mixed) = A.block(n_static, n_back_mixed + n, n - n_static, n_fwrd_mixed);
 
   // Construct matrix E
-  E.setAll(0.0);
-  for (size_t i = 0; i < n_mixed; i++)
+  E.setZero();
+  for (ptrdiff_t i = 0; i < n_mixed; i++)
     E(n - n_static + i, n_back_mixed + beta_fwrd[i]) = 1.0;
-  MatrixView(E, 0, 0, n - n_static, n_back_mixed) = MatrixView(A, n_static, 0, n - n_static, n_back_mixed);
-  for (size_t j = 0; j < n_fwrd; j++)
-    mat::col_copy(A, n_back_mixed + zeta_fwrd_mixed[pi_fwrd[j]], n_static, n - n_static,
-                  E, n_back_mixed + pi_fwrd[j], 0);
-  MatrixView E_tmp(E, 0, 0, n - n_static, n_fwrd + n_back + 2*n_mixed);
-  mat::negate(E_tmp); // Here we take the opposite of some of the zeros initialized in the constructor, but it is not a problem
+  E.block(0, 0, n - n_static, n_back_mixed) = -A.block(n_static, 0, n - n_static, n_back_mixed);
+  for (ptrdiff_t j = 0; j < n_fwrd; j++)
+    E.col(n_back_mixed + pi_fwrd[j]).head(n-n_static) = -A.col(n_back_mixed + zeta_fwrd_mixed[pi_fwrd[j]]).tail(n - n_static);
 
   // Perform the generalized Schur
-  size_t sdim;
+  ptrdiff_t sdim;
   GSD.compute(E, D, Z_prime, sdim);
 
   if (n_back_mixed != sdim)
     throw BlanchardKahnException(true, n_fwrd_mixed, n_fwrd + n_back + 2*n_mixed - sdim);
 
   // Compute DR for forward variables w.r. to endogenous
-  MatrixView Z21_prime(Z_prime, 0, n_back_mixed, n_back_mixed, n_fwrd_mixed),
-    Z22_prime(Z_prime, n_back_mixed, n_back_mixed, n_fwrd_mixed, n_fwrd_mixed);
+  Solver1.compute(Z_prime.block(n_back_mixed, n_back_mixed, n_fwrd_mixed, n_fwrd_mixed).transpose());
+  
+  if (!Solver1.isInvertible())
+    throw BlanchardKahnException(false, n_fwrd_mixed, n_fwrd + n_back + 2*n_mixed - sdim);
 
-  mat::transpose(Z21, Z21_prime);
+  g_y_fwrd = -Solver1.solve(Z_prime.block(0, n_back_mixed, n_back_mixed, n_fwrd_mixed).transpose());
 
-  try
-    {
-      LU1.invMult("T", Z22_prime, Z21);
-    }
-  catch (LUSolver::LUException &e)
-    {
-      throw BlanchardKahnException(false, n_fwrd_mixed, n_fwrd + n_back + 2*n_mixed - sdim);
-    }
-  mat::negate(Z21);
-  const Matrix &g_y_fwrd = Z21;
-
-  for (size_t i = 0; i < n_fwrd_mixed; i++)
-    mat::row_copy(g_y_fwrd, i, g_y, zeta_fwrd_mixed[i]);
+  for (ptrdiff_t i = 0; i < n_fwrd_mixed; i++)
+    g_y.row(zeta_fwrd_mixed[i]) = g_y_fwrd.row(i);
 
   // Compute DR for backward variables w.r. to endogenous
-  MatrixView Z11_prime(Z_prime, 0, 0, n_back_mixed, n_back_mixed),
-    T11(D, 0, 0, n_back_mixed, n_back_mixed),
-    S11(E, 0, 0, n_back_mixed, n_back_mixed);
-  mat::set_identity(g_y_back);
-  g_y_back_tmp = Z11_prime;
-  LU2.invMult("N", g_y_back_tmp, g_y_back);
-  g_y_back_tmp = g_y_back;
-  blas::gemm("N", "N", 1.0, S11, g_y_back_tmp, 0.0, g_y_back);
-  LU2.invMult("N", T11, g_y_back);
-  g_y_back_tmp = g_y_back;
-  blas::gemm("N", "N", 1.0, Z11_prime, g_y_back_tmp, 0.0, g_y_back);
-
+  Solver2.compute(Z_prime.block(0, 0, n_back_mixed, n_back_mixed));
+  g_y_back.noalias() = E.block(0, 0, n_back_mixed, n_back_mixed) * Solver2.solve(MatrixXd::Identity(n_back_mixed, n_back_mixed)); // S_{11} * Z'_{11}^{-1}
+  
+  Solver2.compute(D.block(0, 0, n_back_mixed, n_back_mixed));
+  g_y_back = Z_prime.block(0, 0, n_back_mixed, n_back_mixed) * Solver2.solve(g_y_back);
+  
   // TODO: avoid to copy mixed variables again, rather test it...
-  for (size_t i = 0; i < n_back_mixed; i++)
-    mat::row_copy(g_y_back, i, g_y, zeta_back_mixed[i]);
+  for (ptrdiff_t i = 0; i < n_back_mixed; i++)
+    g_y.row(zeta_back_mixed[i]) = g_y_back.row(i);
 
   // Compute DR for static variables w.r. to endogenous
-  g_y_static = MatrixView(A, 0, 0, n_static, n_back_mixed);
-  for (size_t i = 0; i < n_dynamic; i++)
+  for (ptrdiff_t i = 0; i < n_dynamic; i++)
     {
-      mat::row_copy(g_y, zeta_dynamic[i], g_y_dynamic, i);
-      mat::col_copy(A, n_back_mixed + zeta_dynamic[i], 0, n_static, A0d, i, 0);
+      g_y_dynamic.row(i) = g_y.row(zeta_dynamic[i]);
+      A0d.col(i) = A.col(n_back_mixed + zeta_dynamic[i]).head(n_static);
     }
-  blas::gemm("N", "N", 1.0, A0d, g_y_dynamic, 1.0, g_y_static);
-  blas::gemm("N", "N", 1.0, g_y_fwrd, g_y_back, 0.0, g_y_static_tmp);
-  blas::gemm("N", "N", 1.0, MatrixView(A, 0, n_back_mixed + n, n_static, n_fwrd_mixed),
-             g_y_static_tmp, 1.0, g_y_static);
-  for (size_t i = 0; i < n_static; i++)
-    mat::col_copy(A, n_back_mixed + zeta_static[i], 0, n_static, A0s, i, 0);
-  LU3.invMult("N", A0s, g_y_static);
-  mat::negate(g_y_static);
-
-  for (size_t i = 0; i < n_static; i++)
-    mat::row_copy(g_y_static, i, g_y, zeta_static[i]);
 
+  for (ptrdiff_t i = 0; i < n_static; i++)
+    A0s.col(i) = A.col(n_back_mixed + zeta_static[i]).head(n_static);
+  
+  Solver3.compute(A0s);
+  
+  g_y_static = -Solver3.solve(A.block(0, n_back_mixed + n, n_static, n_fwrd_mixed)*g_y_fwrd*g_y_back
+                              + A0d*g_y_dynamic + A.block(0, 0, n_static, n_back_mixed));
+
+  for (ptrdiff_t i = 0; i < n_static; i++)
+    g_y.row(zeta_static[i]) = g_y_static.row(i);
+  
   // Compute DR for all endogenous w.r. to shocks
-  blas::gemm("N", "N", 1.0, MatrixConstView(jacobian, 0, n_back_mixed + n, n, n_fwrd_mixed), g_y_fwrd, 0.0, g_u_tmp1);
-  g_u_tmp2 = MatrixConstView(jacobian, 0, n_back_mixed, n, n);
-  for (size_t i = 0; i < n_back_mixed; i++)
-    {
-      VectorView c1 = mat::get_col(g_u_tmp2, zeta_back_mixed[i]),
-        c2 = mat::get_col(g_u_tmp1, i);
-      vec::add(c1, c2);
-    }
-  g_u = MatrixConstView(jacobian, 0, n_back_mixed + n + n_fwrd_mixed, n, p);
-  LU4.invMult("N", g_u_tmp2, g_u);
-  mat::negate(g_u);
+  g_u_tmp1 = jacobian.block(0, n_back_mixed + n, n, n_fwrd_mixed) * g_y_fwrd;
+  
+  g_u_tmp2 = jacobian.block(0, n_back_mixed, n, n);
+  for (ptrdiff_t i = 0; i < n_back_mixed; i++)
+    g_u_tmp2.col(zeta_back_mixed[i]) += g_u_tmp1.col(i);
+  Solver4.compute(g_u_tmp2);
+  g_u = -Solver4.solve(jacobian.block(0, n_back_mixed + n + n_fwrd_mixed, n, p));
 }
 
 std::ostream &
diff --git a/mex/sources/estimation/DecisionRules.hh b/mex/sources/estimation/DecisionRules.hh
index 22e30bdf11..d053fbb531 100644
--- a/mex/sources/estimation/DecisionRules.hh
+++ b/mex/sources/estimation/DecisionRules.hh
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 Dynare Team
+ * Copyright (C) 2010-2012 Dynare Team
  *
  * This file is part of Dynare.
  *
@@ -20,28 +20,29 @@
 #include <cstdlib>
 #include <vector>
 
-#include "Vector.hh"
-#include "Matrix.hh"
+#include <Eigen/Core>
+#include <Eigen/QR>
+
 #include "QRDecomposition.hh"
 #include "GeneralizedSchurDecomposition.hh"
-#include "LUSolver.hh"
+
+using namespace Eigen;
 
 class DecisionRules
 {
 private:
-  const size_t n, p;
-  const std::vector<size_t> zeta_fwrd, zeta_back, zeta_mixed, zeta_static;
-  const size_t n_fwrd, n_back, n_mixed, n_static, n_back_mixed, n_fwrd_mixed, n_dynamic;
-  std::vector<size_t> zeta_fwrd_mixed, zeta_back_mixed, zeta_dynamic,
+  const ptrdiff_t n, p;
+  const std::vector<ptrdiff_t> zeta_fwrd, zeta_back, zeta_mixed, zeta_static;
+  const ptrdiff_t n_fwrd, n_back, n_mixed, n_static, n_back_mixed, n_fwrd_mixed, n_dynamic;
+  std::vector<ptrdiff_t> zeta_fwrd_mixed, zeta_back_mixed, zeta_dynamic,
     beta_back, beta_fwrd, pi_back, pi_fwrd;
-  Matrix S, A, D, E, Z_prime;
+  MatrixXd S, A, D, E, Z_prime;
   QRDecomposition QR;
   GeneralizedSchurDecomposition GSD;
-  LUSolver LU1, LU2, LU3;
-  Matrix Z21, g_y_back, g_y_back_tmp;
-  Matrix g_y_static, A0s, A0d, g_y_dynamic, g_y_static_tmp;
-  Matrix g_u_tmp1, g_u_tmp2;
-  LUSolver LU4;
+  ColPivHouseholderQR<MatrixXd> Solver1, Solver2, Solver3, Solver4;
+  MatrixXd Z21, g_y_back, g_y_fwrd;
+  MatrixXd g_y_static, A0s, A0d, g_y_dynamic;
+  MatrixXd g_u_tmp1, g_u_tmp2;
 public:
   class BlanchardKahnException
   {
@@ -54,24 +55,25 @@ public:
   /*!
     The zetas are supposed to follow C convention (first vector index is zero).
   */
-  DecisionRules(size_t n_arg, size_t p_arg, const std::vector<size_t> &zeta_fwrd_arg,
-                const std::vector<size_t> &zeta_back_arg, const std::vector<size_t> &zeta_mixed_arg,
-                const std::vector<size_t> &zeta_static_arg, double qz_criterium);
+  DecisionRules(ptrdiff_t n_arg, ptrdiff_t p_arg, const std::vector<ptrdiff_t> &zeta_fwrd_arg,
+                const std::vector<ptrdiff_t> &zeta_back_arg, const std::vector<ptrdiff_t> &zeta_mixed_arg,
+                const std::vector<ptrdiff_t> &zeta_static_arg, double qz_criterium);
   virtual ~DecisionRules(){};
 
   /*!
     \param jacobian First columns are backetermined vars at t-1 (in the order of zeta_back_mixed), then all vars at t (in the orig order), then forward vars at t+1 (in the order of zeta_fwrd_mixed), then exogenous vars.
   */
-  void compute(const Matrix &jacobian, Matrix &g_y, Matrix &g_u) throw (BlanchardKahnException, GeneralizedSchurDecomposition::GSDException);
-  template<class Vec1, class Vec2>
-  void getGeneralizedEigenvalues(Vec1 &eig_real, Vec2 &eig_cmplx);
+  void compute(const MatrixXd &jacobian, MatrixXd &g_y, MatrixXd &g_u) throw (BlanchardKahnException, GeneralizedSchurDecomposition::GSDException);
+  template<typename Vec1, typename Vec2>
+  void getGeneralizedEigenvalues(PlainObjectBase<Vec1> &eig_real, PlainObjectBase<Vec2> &eig_cmplx);
 };
 
 std::ostream &operator<<(std::ostream &out, const DecisionRules::BlanchardKahnException &e);
 
 template<class Vec1, class Vec2>
 void
-DecisionRules::getGeneralizedEigenvalues(Vec1 &eig_real, Vec2 &eig_cmplx)
+DecisionRules::getGeneralizedEigenvalues(PlainObjectBase<Vec1> &eig_real,
+                                         PlainObjectBase<Vec2> &eig_cmplx)
 {
   GSD.getGeneralizedEigenvalues(eig_real, eig_cmplx);
 }
diff --git a/mex/sources/estimation/tests/Makefile.am b/mex/sources/estimation/tests/Makefile.am
index 8ba8b7a84f..fed293826c 100644
--- a/mex/sources/estimation/tests/Makefile.am
+++ b/mex/sources/estimation/tests/Makefile.am
@@ -1,6 +1,6 @@
 check_PROGRAMS = test-dr testModelSolution testInitKalman testKalman testPDF
 
-test_dr_SOURCES = ../libmat/Matrix.cc ../libmat/Vector.cc ../libmat/QRDecomposition.cc ../libmat/GeneralizedSchurDecomposition.cc ../libmat/LUSolver.cc ../DecisionRules.cc test-dr.cc
+test_dr_SOURCES = ../libmat/GeneralizedSchurDecomposition.cc ../libmat/QRDecomposition.cc ../DecisionRules.cc test-dr.cc
 test_dr_LDADD = $(LAPACK_LIBS) $(BLAS_LIBS) $(LIBS) $(FLIBS)
 test_dr_CPPFLAGS = -I.. -I../libmat -I../../
 
diff --git a/mex/sources/estimation/tests/test-dr.cc b/mex/sources/estimation/tests/test-dr.cc
index 7d9ebd17c4..2a35fea5f8 100644
--- a/mex/sources/estimation/tests/test-dr.cc
+++ b/mex/sources/estimation/tests/test-dr.cc
@@ -3,7 +3,7 @@
  */
 
 /*
- * Copyright (C) 2010-2011 Dynare Team
+ * Copyright (C) 2010-2012 Dynare Team
  *
  * This file is part of Dynare.
  *
@@ -21,14 +21,16 @@
  * along with Dynare.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <iostream>
+
 #include "DecisionRules.hh"
 
 int
 main(int argc, char **argv)
 {
-  size_t endo_nbr = 6, exo_nbr = 2;
+  ptrdiff_t endo_nbr = 6, exo_nbr = 2;
 
-  std::vector<size_t> zeta_fwrd, zeta_back, zeta_mixed, zeta_static;
+  std::vector<ptrdiff_t> zeta_fwrd, zeta_back, zeta_mixed, zeta_static;
   // y and c are purely forward
   zeta_fwrd.push_back(0);
   zeta_fwrd.push_back(1);
@@ -132,10 +134,8 @@ main(int argc, char **argv)
     -1.000000000000000
   };
 
-  MatrixView jacob_tmp(jacob_data, 6, 14, 6);
-
-  Matrix jacobian(6, 14), g_y(6, 3), g_u(6, 2);
-  jacobian = jacob_tmp;
+  MatrixXd jacobian(6, 14), g_y(6, 3), g_u(6, 2);
+  jacobian = Map<Matrix<double,6,14> >(jacob_data);
 
   try
     {
@@ -150,44 +150,32 @@ main(int argc, char **argv)
       std::cerr << e << std::endl;
     }
 
-  Vector eig_real(6), eig_cmplx(6);
+  VectorXd eig_real(6), eig_cmplx(6);
   dr.getGeneralizedEigenvalues(eig_real, eig_cmplx);
-  std::cout << "Eigenvalues (real part): " << eig_real
-            << "Eigenvalues (complex part): " << eig_cmplx << std::endl
-            << "g_y = " << std::endl << g_y << std::endl
-            << "g_u = " << std::endl << g_u;
+  std::cout << "Eigenvalues (real part): " << std::endl << eig_real << std::endl << std::endl
+            << "Eigenvalues (complex part): " << std::endl << eig_cmplx << std::endl<< std::endl
+            << "g_y = " << std::endl << g_y << std::endl << std::endl
+            << "g_u = " << std::endl << g_u << std::endl;
 
   // Check the results for g_y
-  double real_g_y_data[] = {
-    0.005358267364601, 1.836717147430803, 0.837085806295838,
+  MatrixXd real_g_y(6, 3);
+  real_g_y << 0.005358267364601, 1.836717147430803, 0.837085806295838,
     0.038541607674354, 0.424582606909411, -0.318740381721598,
     0.941816659690247, 1.419061793291772, 1.419061793291773,
     -0.000000000000000, 0.950000000000000, 0.025000000000000,
     -0.012546516642830, 0.341714987626857, 0.341714987626861,
-    0.000000000000000, 0.025000000000000, 0.950000000000000
-  };
-
-  MatrixView real_g_y_prime(real_g_y_data, 3, 6, 3);
-  Matrix real_g_y(6, 3);
-  mat::transpose(real_g_y, real_g_y_prime);
-  mat::sub(real_g_y, g_y);
+    0.000000000000000, 0.025000000000000, 0.950000000000000;
 
-  assert(mat::nrminf(real_g_y) < 1e-13);
+  assert((real_g_y - g_y).lpNorm<Infinity>() < 1e-13);
 
   // Check the results for g_u
-  double real_g_u_data[] = {
-    1.911522267389459, 0.830839736432740,
+  MatrixXd real_g_u(6, 2);
+  real_g_u << 1.911522267389459, 0.830839736432740,
     0.456074274269694, -0.347518145871938,
     1.455447993119765, 1.455447993119767,
     1.000000000000000, 0,
     0.350476910386520, 0.350476910386525,
-    0, 1.000000000000000
-  };
-
-  MatrixView real_g_u_prime(real_g_u_data, 2, 6, 2);
-  Matrix real_g_u(6, 2);
-  mat::transpose(real_g_u, real_g_u_prime);
-  mat::sub(real_g_u, g_u);
-
-  assert(mat::nrminf(real_g_u) < 1e-13);
+    0, 1.000000000000000;
+  
+  assert((real_g_u - g_u).lpNorm<Infinity>() < 1e-13);
 }
-- 
GitLab