controllability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #include <corbo-numerics/controllability.h>
26 
27 #include <corbo-numerics/matrix_utilities.h>
28 
29 #include <corbo-core/console.h>
30 
31 // #include <Eigen/LU>
32 #include <Eigen/QR>
33 
34 namespace corbo {
35 
36 bool Controllability::checkLinearTimeInvariantSystem(const Eigen::Ref<const Eigen::MatrixXd>& A, const Eigen::Ref<const Eigen::MatrixXd>& B,
37  int* rank)
38 {
39  assert(is_square(A));
40  assert(A.rows() == B.rows());
41  int n = A.rows();
42  int m = B.cols();
43  if (n < 1 || m < 1) return false;
44 
45  Eigen::MatrixXd ctrl_mat(n, n * m);
46 
47  ctrl_mat.leftCols(m) = B;
48  if (n > 1) ctrl_mat.middleCols(m, m) = A * B;
49 
50  if (n > 2)
51  {
52  Eigen::MatrixXd A_aux = A;
53  int current_col = 2 * m;
54  for (int i = 2; i < n; ++i)
55  {
56  A_aux *= A;
57  ctrl_mat.middleCols(current_col, m) = A_aux * B;
58  current_col += m;
59  }
60  }
61  // get rank using FullPivLU or ColPivHousehodler. Note, rank-revealing LU is the most reliable way with floating values
62  // see https://en.wikipedia.org/wiki/Rank_%28linear_algebra%29#Computing_the_rank_of_a_matrix
63 
64  // Eigen::FullPivLU<Eigen::MatrixXd> decomp(ctrl_mat);
65  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> decomp(ctrl_mat);
66  int ctrl_mat_rank = decomp.rank();
67  if (rank) *rank = ctrl_mat_rank;
68  return (ctrl_mat_rank == n);
69 }
70 
71 } // namespace corbo
corbo::Controllability::checkLinearTimeInvariantSystem
static bool checkLinearTimeInvariantSystem(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, int *rank=nullptr)
Check controllability of linear time invariant system.
Definition: controllability.cpp:58
corbo
Definition: communication/include/corbo-communication/utilities.h:37
console.h
corbo::is_square
bool is_square(const Eigen::MatrixBase< Derived > &matrix)
Determine if a given matrix is square.
Definition: matrix_utilities.h:65
B
MatrixType B(b, *n, *nrhs, *ldb)
matrix_utilities.h
Eigen::ColPivHouseholderQR
Householder rank-revealing QR decomposition of a matrix with column-pivoting.
Definition: ForwardDeclarations.h:255
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
controllability.h
n
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
A
MatrixType A(a, *n, *n, *lda)

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Wed Mar 2 2022 00:05:41