observability.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/observability.h>
26 
27 #include <corbo-core/console.h>
28 
29 #include <corbo-numerics/matrix_utilities.h>
30 
31 // #include <Eigen/LU>
32 #include <Eigen/QR>
33 
34 namespace corbo {
35 
36 bool Observability::checkLinearTimeInvariantSystem(const Eigen::Ref<const Eigen::MatrixXd>& A, const Eigen::Ref<const Eigen::MatrixXd>& C, int* rank)
37 {
38  assert(is_square(A));
39  assert(A.rows() == C.cols());
40  int n = A.rows();
41  int p = C.rows();
42  if (n < 1 || p < 1) return false;
43 
44  Eigen::MatrixXd observ_mat(n * p, n);
45 
46  observ_mat.topRows(p) = C;
47  if (n > 1) observ_mat.middleRows(p, p) = C * A;
48 
49  if (n > 2)
50  {
51  Eigen::MatrixXd A_aux = A;
52  int current_row = 2 * p;
53  for (int i = 2; i < n; ++i)
54  {
55  A_aux *= A;
56  observ_mat.middleRows(current_row, p) = C * A_aux;
57  current_row += p;
58  }
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(observ_mat);
66  int observ_mat_rank = decomp.rank();
67  if (rank) *rank = observ_mat_rank;
68  return (observ_mat_rank == n);
69 }
70 
71 } // namespace corbo
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
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
n
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
A
MatrixType A(a, *n, *n, *lda)
observability.h
corbo::Observability::checkLinearTimeInvariantSystem
static bool checkLinearTimeInvariantSystem(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &C, int *rank=nullptr)
Check observability of linear time invariant system.
Definition: observability.cpp:58

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