autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
matrixfree_gmres.hpp
Go to the documentation of this file.
1#ifndef CGMRES__MATRIXFREE_GMRES_HPP_
2#define CGMRES__MATRIXFREE_GMRES_HPP_
3
4#include <iostream>
5#include <cmath>
6#include <limits>
7#include <stdexcept>
8
9#include "cgmres/types.hpp"
10
12
13
14namespace cgmres {
15namespace detail {
16
17template <typename LinearProblem, int _kmax>
19public:
20 static constexpr int dim = LinearProblem::dim;
21 static constexpr int kmax = std::min(dim, _kmax);
22
24 : hessenberg_mat_(Matrix<kmax+1, kmax+1>::Zero()),
25 basis_mat_(Matrix<dim, kmax+1>::Zero()),
26 b_vec_(Vector<dim>::Zero()),
27 givens_c_vec_(Vector<kmax+1>::Zero()),
28 givens_s_vec_(Vector<kmax+1>::Zero()),
29 g_vec_(Vector<kmax+1>::Zero()) {
30 static_assert(dim > 0);
31 static_assert(kmax > 0);
32 static_assert(dim >= kmax);
33 }
34
35 ~MatrixFreeGMRES() = default;
36
37 template <typename... LinearProblemArgs>
38 int solve(LinearProblem& linear_problem,
39 LinearProblemArgs... linear_problem_args,
40 Vector<dim>& linear_problem_solution) {
41 // Initializes vectors for QR factrization by Givens rotation.
42 givens_c_vec_.setZero();
43 givens_s_vec_.setZero();
44 g_vec_.setZero();
45 // Generates the initial basis of the Krylov subspace.
46 linear_problem.eval_b(linear_problem_args..., linear_problem_solution, b_vec_);
47 g_vec_.coeffRef(0) = b_vec_.template lpNorm<2>();
48 basis_mat_.col(0) = b_vec_ / g_vec_.coeff(0);
49 // k : the dimension of the Krylov subspace at the current iteration.
50 int k = 0;
51 for (; k<kmax; ++k) {
52 linear_problem.eval_Ax(linear_problem_args..., basis_mat_.col(k),
53 basis_mat_.col(k+1));
54 for (int j=0; j<=k; ++j) {
55 hessenberg_mat_.coeffRef(k, j) = basis_mat_.col(k+1).dot(basis_mat_.col(j));
56 basis_mat_.col(k+1).noalias() -= hessenberg_mat_.coeff(k, j) * basis_mat_.col(j);
57 }
58 hessenberg_mat_.coeffRef(k, k+1) = basis_mat_.col(k+1).template lpNorm<2>();
59 if (std::abs(hessenberg_mat_.coeff(k, k+1)) < std::numeric_limits<double>::epsilon()) {
60 break;
61 }
62 else {
63 basis_mat_.col(k+1).array() /= hessenberg_mat_.coeff(k, k+1);
64 }
65 // Givens Rotation for QR factrization of the least squares problem.
66 for (int j=0; j<k; ++j) {
67 givensRotation(hessenberg_mat_.row(k), j);
68 }
69 const Scalar nu = std::sqrt(hessenberg_mat_.coeff(k, k)*hessenberg_mat_.coeff(k, k)
70 +hessenberg_mat_.coeff(k, k+1)*hessenberg_mat_.coeff(k, k+1));
71 if (nu) {
72 givens_c_vec_.coeffRef(k) = hessenberg_mat_.coeff(k, k) / nu;
73 givens_s_vec_.coeffRef(k) = - hessenberg_mat_.coeff(k, k+1) / nu;
74 hessenberg_mat_.coeffRef(k, k) = givens_c_vec_.coeff(k) * hessenberg_mat_.coeff(k, k)
75 - givens_s_vec_.coeff(k) * hessenberg_mat_.coeff(k, k+1);
76 hessenberg_mat_.coeffRef(k, k+1) = 0.0;
77 givensRotation(g_vec_, k);
78 }
79 else {
80 throw std::runtime_error("Lose orthogonality of the basis of the Krylov subspace");
81 }
82 }
83 // Computes solution_vec by solving hessenberg_mat_ * y = g_vec.
84 for (int i=k-1; i>=0; --i) {
85 Scalar tmp = g_vec_.coeff(i);
86 for (int j=i+1; j<k; ++j) {
87 tmp -= hessenberg_mat_.coeff(j, i) * givens_c_vec_.coeff(j);
88 }
89 givens_c_vec_.coeffRef(i) = tmp / hessenberg_mat_.coeff(i, i);
90 }
91 for (int i=0; i<dim; ++i) {
92 Scalar tmp = 0.0;
93 for (int j=0; j<k; ++j) {
94 tmp += basis_mat_.coeff(i, j) * givens_c_vec_.coeff(j);
95 }
96 linear_problem_solution.coeffRef(i) += tmp;
97 }
98 return k;
99 }
100
101 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
102
103private:
104 Matrix<kmax+1, kmax+1> hessenberg_mat_;
105 Matrix<dim, kmax+1> basis_mat_;
106 Vector<dim> b_vec_;
107 Vector<kmax+1> givens_c_vec_, givens_s_vec_, g_vec_;
108
109 template <typename VectorType>
110 inline void givensRotation(const MatrixBase<VectorType>& column_vec,
111 const int i_column) const {
112 const Scalar tmp1 = givens_c_vec_.coeff(i_column) * column_vec.coeff(i_column)
113 - givens_s_vec_.coeff(i_column) * column_vec.coeff(i_column+1);
114 const Scalar tmp2 = givens_s_vec_.coeff(i_column) * column_vec.coeff(i_column)
115 + givens_c_vec_.coeff(i_column) * column_vec.coeff(i_column+1);
116 CGMRES_EIGEN_CONST_CAST(VectorType, column_vec).coeffRef(i_column) = tmp1;
117 CGMRES_EIGEN_CONST_CAST(VectorType, column_vec).coeffRef(i_column+1) = tmp2;
118 }
119
120};
121
122} // namespace detail
123} // namespace cgmres
124
125#endif // CGMRES__MATRIXFREE_GMRES_HPP_
Definition: matrixfree_gmres.hpp:18
static constexpr int kmax
Definition: matrixfree_gmres.hpp:21
MatrixFreeGMRES()
Definition: matrixfree_gmres.hpp:23
int solve(LinearProblem &linear_problem, LinearProblemArgs... linear_problem_args, Vector< dim > &linear_problem_solution)
Definition: matrixfree_gmres.hpp:38
static constexpr int dim
Definition: matrixfree_gmres.hpp:20
#define CGMRES_EIGEN_CONST_CAST(TYPE, OBJ)
Definition: macros.hpp:7
Definition: continuation_gmres.hpp:11
Eigen::Matrix< Scalar, rows, cols > Matrix
Alias of Eigen::Matrix.
Definition: types.hpp:17
Eigen::Matrix< Scalar, size, 1 > Vector
Alias of Eigen::Vector.
Definition: types.hpp:23
double Scalar
Alias of double.
Definition: types.hpp:11
Eigen::MatrixBase< MatrixType > MatrixBase
Alias of Eigen::MatrixBase.
Definition: types.hpp:29