test_algebraic_riccati_continuous.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-core/console.h>
26 #include <corbo-core/macros.h>
27 #include <corbo-numerics/algebraic_riccati_continuous.h>
28 
29 #include "gtest/gtest.h"
30 
31 using corbo::AlgebraicRiccatiContinuous;
32 
33 class TestAlgebraicRiccatiContinuous : public testing::Test
34 {
35  protected:
36  // You can do set-up work for each test here.
37  TestAlgebraicRiccatiContinuous() {}
38  // You can do clean-up work that doesn't throw exceptions here.
39  virtual ~TestAlgebraicRiccatiContinuous() {}
40  // If the constructor and destructor are not enough for setting up
41  // and cleaning up each test, you can define the following methods:
42 
43  // Code here will be called immediately after the constructor (right
44  // before each test).
45  // virtual void SetUp() {}
46  // Code here will be called immediately after each test (right
47  // before the destructor).
48  // virtual void TearDown();
49 };
50 
51 TEST_F(TestAlgebraicRiccatiContinuous, solve_feasible_2d_stable_real)
52 {
53  Eigen::Matrix2d A;
54  A << -3, 2, 1, 1; // only real eigenvalues (all stable)
55 
56  Eigen::Vector2d B;
57  B << 0, 1;
58 
59  Eigen::Matrix<double, 1, 2> C;
60  C << 1, -1;
61 
62  Eigen::MatrixXd Q = C.transpose() * C;
63  Eigen::MatrixXd R = Eigen::MatrixXd::Identity(1, 1);
64 
65  EXPECT_TRUE(AlgebraicRiccatiContinuous::isNumericallyStable(A, B, Q, R));
66 
67  Eigen::MatrixXd X;
68  Eigen::MatrixXd G;
69  bool solve_success = AlgebraicRiccatiContinuous::solve(A, B, Q, R, X, &G);
70  EXPECT_TRUE(solve_success);
71 
72  Eigen::Matrix2d X_sol;
73  X_sol << 0.2949, 0.5199, 0.5199, 3.0198;
74  EXPECT_EQ_MATRIX(X, X_sol, 1e-4);
75 
76  // check gain matrix
77  Eigen::MatrixXd G_sol(1, 2);
78  G_sol << 0.519862500040767, 3.019764837837087;
79  EXPECT_EQ_MATRIX(G, G_sol, 1e-4);
80 
81  // check closed-loop stability
82  EXPECT_TRUE(AlgebraicRiccatiContinuous::isClosedLoopStable(A, B, G));
83 }
84 
85 TEST_F(TestAlgebraicRiccatiContinuous, solve_feasible_4d_stableA_complex)
86 {
87  Eigen::Matrix4d A;
88  A << -8, -19, -22, -10, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0; // a complex pair of eigenvalues (all eigenvalues stable)
89 
90  Eigen::Vector4d B;
91  B << 1, 0, 0, 0;
92 
93  Eigen::Matrix<double, 1, 4> C;
94  C << 0, 0, 0, 1;
95 
96  Eigen::MatrixXd Q = C.transpose() * C;
97 
98  Eigen::MatrixXd R(1, 1);
99  R << 2;
100 
101  EXPECT_TRUE(AlgebraicRiccatiContinuous::isNumericallyStable(A, B, Q, R));
102 
103  Eigen::MatrixXd X;
104  Eigen::MatrixXd G;
105  bool solve_success = AlgebraicRiccatiContinuous::solve(A, B, Q, R, X, &G);
106  EXPECT_TRUE(solve_success);
107 
108  Eigen::Matrix4d X_sol;
109  X_sol << 0.0029, 0.0234, 0.0538, 0.0499, 0.0234, 0.1889, 0.4445, 0.4288, 0.0538, 0.4445, 1.1076, 1.1833, 0.0499, 0.4288, 1.1833, 1.6375;
110  EXPECT_EQ_MATRIX(X, X_sol, 1e-4);
111 
112  // check gain matrix
113  Eigen::MatrixXd G_sol(1, 4);
114  G_sol << 0.001461438418027, 0.011692575245343, 0.026875649174793, 0.024968827881711;
115  EXPECT_EQ_MATRIX(G, G_sol, 1e-4);
116 
117  // check closed-loop stability
118  EXPECT_TRUE(AlgebraicRiccatiContinuous::isClosedLoopStable(A, B, G));
119 }
120 
121 TEST_F(TestAlgebraicRiccatiContinuous, solve_feasible_4d_unstableA_complex)
122 {
123  Eigen::Matrix4d A;
124  A << 6, -5, -2, 10, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0; // a complex pair of eigenvalues (some eigenvalues unstable)
125 
126  Eigen::Vector4d B;
127  B << 1, 0, 0, 0;
128 
129  Eigen::Matrix<double, 1, 4> C;
130  C << 0, 0, 0, 1;
131 
132  Eigen::MatrixXd Q = C.transpose() * C;
133 
134  Eigen::MatrixXd R(1, 1);
135  R << 2;
136 
137  EXPECT_TRUE(AlgebraicRiccatiContinuous::isNumericallyStable(A, B, Q, R));
138 
139  Eigen::MatrixXd X;
140  Eigen::MatrixXd G;
141  bool solve_success = AlgebraicRiccatiContinuous::solve(A, B, Q, R, X, &G);
142  EXPECT_TRUE(solve_success);
143 
144  Eigen::Matrix4d X_sol;
145  X_sol << 28.0029, 28.0234, 40.0538, 40.0499, 28.0234, 324.1889, 336.4445, 40.4288, 40.0538, 336.4445, 777.1076, 481.1833, 40.0499, 40.4288,
146  481.1833, 481.6375;
147  EXPECT_EQ_MATRIX(X, X_sol, 1e-4);
148 
149  // check gain matrix
150  Eigen::MatrixXd G_sol(1, 4);
151  G_sol << 14.001461438418874, 14.011692575248027, 20.026875649177440, 20.024968827882546;
152  EXPECT_EQ_MATRIX(G, G_sol, 1e-4);
153 
154  // check closed-loop stability
155  EXPECT_TRUE(AlgebraicRiccatiContinuous::isClosedLoopStable(A, B, G));
156 }
157 
158 TEST_F(TestAlgebraicRiccatiContinuous, solve_linearized_pendulum_model)
159 {
160  Eigen::Matrix4d A;
161  A << 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 21.9524, -0.1348;
162 
163  Eigen::Vector4d B;
164  B << 0, 1.0, 0, 2.2385;
165 
166  Eigen::Matrix<double, 4, 4> C;
167  C << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
168 
169  Eigen::Matrix<double, 4, 4> Q;
170  Q << 35, 0, 0, 0, 0, 1, 0, 0, 0, 0, 5, 0, 0, 0, 0, 35;
171 
172  Eigen::MatrixXd R(1, 1);
173  R << 0.1;
174 
175  Eigen::MatrixXd X;
176  Eigen::MatrixXd G;
177  bool solve_success = AlgebraicRiccatiContinuous::solve(A, B, Q, R, X, &G);
178  EXPECT_TRUE(solve_success);
179 
180  Eigen::Matrix4d X_sol;
181  X_sol << 37.1092, 19.1727, -57.065, -9.4007, 19.1727, 14.5091, -50.7528, -7.3677, -57.065, -50.7528, 231.2142, 27.6002, -9.4007, -7.3677, 27.6002,
182  4.6237;
183 
184  EXPECT_EQ_MATRIX(X, X_sol, 1e-4);
185 
186  // check gain matrix
187  Eigen::MatrixXd G_sol(1, 4);
188  G_sol << -18.7083, -19.8357, 110.3077, 29.8251;
189  EXPECT_EQ_MATRIX(G, G_sol, 1e-4);
190 
191  // check closed-loop stability
192  EXPECT_TRUE(AlgebraicRiccatiContinuous::isClosedLoopStable(A, B, G));
193 }
194 
195 TEST_F(TestAlgebraicRiccatiContinuous, numerically_unstable)
196 {
197  Eigen::Matrix4d A;
198  A << 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 21.0524, 0, 0, 1, -0.1348;
199 
200  Eigen::Vector4d B;
201  B << 0, 1.0, 0, 2.2385;
202 
203  Eigen::Matrix<double, 4, 4> Q;
204  Q << 35, 0, 0, 0, 0, 1, 0, 0, 0, 0, 5, 0, 0, 0, 0, 35;
205 
206  Eigen::MatrixXd R(1, 1);
207  R << 0.1;
208 
209  EXPECT_FALSE(AlgebraicRiccatiContinuous::isNumericallyStable(A, B, Q, R));
210 }
A
MatrixType A(a, *n, *n, *lda)
B
MatrixType B(b, *n, *nrhs, *ldb)
EXPECT_EQ_MATRIX
#define EXPECT_EQ_MATRIX(A, B, tol)
Definition: macros.h:39
algebraic_riccati_continuous.h
X
#define X
Definition: icosphere.cpp:20
corbo::AlgebraicRiccatiContinuous
Methods for dealing with continuous-time algebraic Riccati equations.
Definition: algebraic_riccati_continuous.h:58
TEST_F
TEST_F(TestAlgebraicRiccatiContinuous, solve_feasible_2d_stable_real)
Definition: test_algebraic_riccati_continuous.cpp:51

control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:07:52