test_schur.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/schur.h>
26 
27 #include <corbo-core/console.h>
28 #include <corbo-core/macros.h>
29 #include <corbo-core/value_comparison.h>
30 
31 #include <Eigen/Eigenvalues>
32 
33 #include "gtest/gtest.h"
34 
35 using corbo::schur_decomposition_2d;
36 using corbo::swap_schur_blocks;
37 using corbo::reorder_schur_blocks;
38 using corbo::essentially_equal;
39 using corbo::approx_zero;
40 
41 class TestSchur : public testing::Test
42 {
43  protected:
44  // You can do set-up work for each test here.
45  TestSchur() {}
46  // You can do clean-up work that doesn't throw exceptions here.
47  virtual ~TestSchur() {}
48  // If the constructor and destructor are not enough for setting up
49  // and cleaning up each test, you can define the following methods:
50 
51  // Code here will be called immediately after the constructor (right
52  // before each test).
53  // virtual void SetUp() {}
54  // Code here will be called immediately after each test (right
55  // before the destructor).
56  // virtual void TearDown();
57 };
58 
59 TEST_F(TestSchur, schur_decomposition_2d1)
60 {
61  Eigen::Matrix2d T;
62  T << -5, -6, 1, 0; // only real eigenvalues
63 
64  Eigen::Matrix2d U;
65  schur_decomposition_2d(T, U);
66 
67  Eigen::Matrix2d Tsol;
68  Tsol << -3, -7, 0, -2;
69 
70  Eigen::Matrix2d Usol;
71  Usol << -0.9487, -0.3162, 0.3162, -0.9487;
72 
73  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
74  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
75 }
76 
77 TEST_F(TestSchur, schur_decomposition_2d2)
78 {
79  Eigen::Matrix2d T;
80  T << -3, -7, 0, -2; // already schur form
81 
82  Eigen::Matrix2d U;
83  schur_decomposition_2d(T, U);
84 
85  Eigen::Matrix2d Tsol;
86  Tsol << -3, -7, 0, -2;
87 
88  Eigen::Matrix2d Usol = Eigen::Matrix2d::Identity();
89 
90  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
91  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
92 }
93 
94 TEST_F(TestSchur, schur_decomposition_2d3)
95 {
96  Eigen::Matrix2d T;
97  T << -1, -1.25, 1, 0; // complex eigenvalues
98 
99  Eigen::Matrix2d U;
100  schur_decomposition_2d(T, U);
101 
102  Eigen::Matrix2d Tsol;
103  Tsol << -0.5, -1.6404, 0.6096, -0.5;
104 
105  Eigen::Matrix2d Usol;
106  Usol << 0.7882, 0.6154, -0.6154, 0.7882;
107 
108  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
109  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
110 }
111 
112 TEST_F(TestSchur, schur_decomposition_2d4)
113 {
114  Eigen::Matrix2d T;
115  T << -0.393678, -0.73017, 0.78598, -0.606322; // already schur form, but non-standard
116 
117  Eigen::Matrix2d U;
118  schur_decomposition_2d(T, U);
119 
120  Eigen::Matrix2d Tsol;
121  Tsol << -0.5000, -0.6482, 0.8680, -0.5000;
122 
123  Eigen::Matrix2d Usol;
124  Usol << 0.7918, 0.6108, -0.6108, 0.7918;
125 
126  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
127  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
128 }
129 
130 TEST_F(TestSchur, schur_decomposition_2d_b_zero)
131 {
132  // element T(0,1) = 0 (special implementation)
133  Eigen::Matrix2d T;
134  T << -5, 0, 1, 0; // only real eigenvalues
135 
136  Eigen::Matrix2d U;
137  schur_decomposition_2d(T, U);
138 
139  Eigen::Matrix2d Tsol;
140  Tsol << 0, -1, 0, -5;
141 
142  Eigen::Matrix2d Usol;
143  Usol << 0, -1, 1, 0;
144 
145  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
146  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
147 }
148 
149 TEST_F(TestSchur, schur_decomposition_2d_a_minus_d_zero1)
150 {
151  // special case: T(0,0) - T(1,1) = 0, but sign(1,b) != sign(1,c)
152  Eigen::Matrix2d T;
153  T << -5, -6, 1, -5; // only real eigenvalues
154 
155  Eigen::Matrix2d U;
156  schur_decomposition_2d(T, U);
157 
158  Eigen::Matrix2d Tsol;
159  Tsol << -5, -6, 1, -5;
160 
161  Eigen::Matrix2d Usol;
162  Usol << 1, 0, 0, 1;
163 
164  EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
165  EXPECT_EQ_MATRIX(U, Usol, 1e-4);
166 }
167 
168 TEST_F(TestSchur, schur_decomposition_2d_a_minus_d_zero2)
169 {
170  // special case: T(0,0) - T(1,1) = 0, but sign(1,b) == sign(1,c)
171  Eigen::Matrix2d T;
172  T << -5, -6, -1, -5; // only real eigenvalues
173 
174  Eigen::Matrix2d Rsol = T;
175 
176  Eigen::Matrix2d U;
177  schur_decomposition_2d(T, U);
178 
179  // in this example, our solution exhibits swapped eigenvalues compared to matlab.
180  // so we check validity by transforming the problem back
181 
182  Eigen::MatrixXd R = U * T * U.adjoint();
183  EXPECT_EQ_MATRIX(R, Rsol, 1e-4);
184 
185  // Eigen::Matrix2d Tsol;
186  // Tsol << -2.5505, -5.0, 0, -7.4495;
187 
188  // Eigen::Matrix2d Usol;
189  // Usol << 0.9258, 0.3780, -0.3780, 0.9258;
190 
191  // EXPECT_EQ_MATRIX(T, Tsol, 1e-4);
192  // EXPECT_EQ_MATRIX(U, Usol, 1e-4);
193 }
194 
195 TEST_F(TestSchur, swap_blocks_schur_form1)
196 {
197  Eigen::Matrix2d T;
198  T << -1.0000, 5.0000, 0, -2.0000;
199 
200  Eigen::Matrix2d Q;
201  Q.setIdentity();
202  swap_schur_blocks(T, 0, 1, 1, Q);
203  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix2d::Identity(), 1e-5);
204 
205  EXPECT_NEAR(T(0, 0), -2, 1e-10);
206  EXPECT_NEAR(T(1, 1), -1, 1e-10);
207 }
208 
209 TEST_F(TestSchur, swap_blocks_schur_form2)
210 {
211  // Eigen::Matrix3d A;
212  // A << -2, -1.8125, -0.8125, 1, 0, 0, 0, 1, 0;
213 
214  Eigen::Matrix3d T;
215  T << -1, -0.8378, 1.3184, 0, -0.5, 2.4397, 0, -0.2306, -0.5;
216 
217  Eigen::MatrixXd Q(3, 3);
218  Q.setIdentity();
219  swap_schur_blocks(T, 0, 1, 2, Q);
220  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix3d::Identity(), 1e-5);
221 
222  // check eigenvalues A11
223  Eigen::VectorXcd eig1 = T.block(0, 0, 2, 2).eigenvalues();
224  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 0.75), 1e-3) ||
225  essentially_equal(eig1[0], std::complex<double>(-0.5, -0.75), 1e-3))
226  << "Eigenvalue: " << eig1[0];
227  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 0.75), 1e-3) ||
228  essentially_equal(eig1[1], std::complex<double>(-0.5, -0.75), 1e-3))
229  << "Eigenvalue: " << eig1[1];
230 
231  EXPECT_NEAR(T(2, 2), -1, 1e-10);
232 }
233 
234 TEST_F(TestSchur, swap_blocks_schur_form3_standardize)
235 {
236  // Eigen::MatrixXd A(6, 6);
237  // A << -17, -105.25, -332, -590, -536, -320, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0;
238 
239  // -> Schur-Decomposition
240  //
241  // T =
242  // -8.0000 30.7409 -57.5046 -247.0842 -169.7203 867.0578
243  // 0 -4.0000 7.0149 30.1406 20.7053 -105.7696
244  // 0 0 -2.0000 -7.5820 -5.1700 26.3801
245  // 0 0 0.5276 -2.0000 -1.3243 6.2260
246  // 0 0 0 0 -0.5000 3.4406
247  // 0 0 0 0 -0.2906 -0.5000
248 
249  Eigen::MatrixXd T(6, 6);
250  T << -8.0000, 30.7409, -57.5046, -247.0842, -169.7203, 867.0578, 0, -4.0000, 7.0149, 30.1406, 20.7053, -105.7696, 0, 0, -2.0000, -7.5820, -5.1700,
251  26.3801, 0, 0, 0.5276, -2.0000, -1.3243, 6.2260, 0, 0, 0, 0, -0.5000, 3.4406, 0, 0, 0, 0, -0.2906, -0.5000;
252 
253  Eigen::MatrixXd T_original = T; // backup for final test
254 
255  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(6, 6);
256  swap_schur_blocks(T, 2, 2, 2, Q1, true);
257  EXPECT_EQ_MATRIX((Q1.transpose() * Q1).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
258  // check eigenvalues A11
259  Eigen::VectorXcd eig1 = T.block(2, 2, 2, 2).eigenvalues();
260  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
261  << "Eigenvalue: " << eig1[0];
262  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
263  << "Eigenvalue: " << eig1[1];
264  // check eigenvalues A22
265  Eigen::VectorXcd eig2 = T.block(4, 4, 2, 2).eigenvalues();
266  EXPECT_TRUE(essentially_equal(eig2[0], std::complex<double>(-2, 2), 1e-4) || essentially_equal(eig2[0], std::complex<double>(-2, -2), 1e-4))
267  << "Eigenvalue: " << eig2[0];
268  EXPECT_TRUE(essentially_equal(eig2[1], std::complex<double>(-2, 2), 1e-4) || essentially_equal(eig2[1], std::complex<double>(-2, -2), 1e-4))
269  << "Eigenvalue: " << eig2[1];
270 
271  // check standard form
272  auto is_standard_form = [](const Eigen::Ref<const Eigen::Matrix2d>& block) {
273  if (essentially_equal(block(0, 0), block(1, 1), 1e-15) && block(0, 1) * block(1, 0) < 0) return true;
274  return false;
275  };
276  EXPECT_TRUE(is_standard_form(T.block(2, 2, 2, 2))) << "T:\n" << T.block(2, 2, 2, 2);
277  EXPECT_TRUE(is_standard_form(T.block(4, 4, 2, 2))) << "T:\n" << T.block(4, 4, 2, 2);
278 
279  // now swap first two eigenvalues
280  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(6, 6);
281  swap_schur_blocks(T, 0, 1, 1, Q2, true);
282  EXPECT_EQ_MATRIX((Q2.transpose() * Q2).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
283  EXPECT_NEAR(T(0, 0), -4, 1e-10);
284  EXPECT_NEAR(T(1, 1), -8, 1e-10);
285 
286  // now swap 2nd and 3rd eigenvalues
287  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(6, 6);
288  swap_schur_blocks(T, 1, 1, 2, Q3, true);
289 
290  EXPECT_EQ_MATRIX((Q3.transpose() * Q3).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
291  // check eigenvalues A11
292  eig1 = T.block(1, 1, 2, 2).eigenvalues();
293  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
294  << "Eigenvalue: " << eig1[0];
295  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
296  << "Eigenvalue: " << eig1[1];
297  EXPECT_NEAR(T(3, 3), -8, 1e-10);
298  EXPECT_TRUE(is_standard_form(T.block(1, 1, 2, 2))) << "T:\n" << T.block(1, 1, 2, 2);
299 
300  // now swap again 2nd and 3rd to test p=2, q=1 case
301  Eigen::MatrixXd Q4 = Eigen::MatrixXd::Identity(6, 6);
302  swap_schur_blocks(T, 1, 2, 1, Q4, true);
303 
304  EXPECT_EQ_MATRIX((Q4.transpose() * Q4).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
305  // check eigenvalues A11
306  eig1 = T.block(2, 2, 2, 2).eigenvalues();
307  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
308  << "Eigenvalue: " << eig1[0];
309  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
310  << "Eigenvalue: " << eig1[1];
311  EXPECT_NEAR(T(1, 1), -8, 1e-10);
312  EXPECT_TRUE(is_standard_form(T.block(2, 2, 2, 2))) << "T:\n" << T.block(1, 1, 2, 2);
313 
314  // final test, transform T back to the original matrix
315  Eigen::MatrixXd Tf = Q1 * Q2 * Q3 * Q4 * T * Q4.transpose() * Q3.transpose() * Q2.transpose() * Q1.transpose();
316  EXPECT_EQ_MATRIX(Tf, T_original, 1e-4);
317 }
318 
319 TEST_F(TestSchur, swap_blocks_schur_form3_non_standardize)
320 {
321  Eigen::MatrixXd T(6, 6);
322  T << -8.0000, 30.7409, -57.5046, -247.0842, -169.7203, 867.0578, 0, -4.0000, 7.0149, 30.1406, 20.7053, -105.7696, 0, 0, -2.0000, -7.5820, -5.1700,
323  26.3801, 0, 0, 0.5276, -2.0000, -1.3243, 6.2260, 0, 0, 0, 0, -0.5000, 3.4406, 0, 0, 0, 0, -0.2906, -0.5000;
324 
325  Eigen::MatrixXd T_original = T; // backup for final test
326 
327  Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(6, 6);
328  swap_schur_blocks(T, 2, 2, 2, Q1, false);
329  EXPECT_EQ_MATRIX((Q1.transpose() * Q1).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
330  // check eigenvalues A11
331  Eigen::VectorXcd eig1 = T.block(2, 2, 2, 2).eigenvalues();
332  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
333  << "Eigenvalue: " << eig1[0];
334  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
335  << "Eigenvalue: " << eig1[1];
336  // check eigenvalues A22
337  Eigen::VectorXcd eig2 = T.block(4, 4, 2, 2).eigenvalues();
338  EXPECT_TRUE(essentially_equal(eig2[0], std::complex<double>(-2, 2), 1e-4) || essentially_equal(eig2[0], std::complex<double>(-2, -2), 1e-4))
339  << "Eigenvalue: " << eig2[0];
340  EXPECT_TRUE(essentially_equal(eig2[1], std::complex<double>(-2, 2), 1e-4) || essentially_equal(eig2[1], std::complex<double>(-2, -2), 1e-4))
341  << "Eigenvalue: " << eig2[1];
342 
343  // now swap first two eigenvalues
344  Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(6, 6);
345  swap_schur_blocks(T, 0, 1, 1, Q2, false);
346  EXPECT_EQ_MATRIX((Q2.transpose() * Q2).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
347  EXPECT_NEAR(T(0, 0), -4, 1e-10);
348  EXPECT_NEAR(T(1, 1), -8, 1e-10);
349 
350  // now swap 2nd and 3rd eigenvalues
351  Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(6, 6);
352  swap_schur_blocks(T, 1, 1, 2, Q3, false);
353 
354  EXPECT_EQ_MATRIX((Q3.transpose() * Q3).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
355  // check eigenvalues A11
356  eig1 = T.block(1, 1, 2, 2).eigenvalues();
357  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
358  << "Eigenvalue: " << eig1[0];
359  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
360  << "Eigenvalue: " << eig1[1];
361  EXPECT_NEAR(T(3, 3), -8, 1e-10);
362 
363  // now swap again 2nd and 3rd to test p=2, q=1 case
364  Eigen::MatrixXd Q4 = Eigen::MatrixXd::Identity(6, 6);
365  swap_schur_blocks(T, 1, 2, 1, Q4, false);
366 
367  EXPECT_EQ_MATRIX((Q4.transpose() * Q4).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
368  // check eigenvalues A11
369  eig1 = T.block(2, 2, 2, 2).eigenvalues();
370  EXPECT_TRUE(essentially_equal(eig1[0], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[0], std::complex<double>(-0.5, -1), 1e-4))
371  << "Eigenvalue: " << eig1[0];
372  EXPECT_TRUE(essentially_equal(eig1[1], std::complex<double>(-0.5, 1), 1e-4) || essentially_equal(eig1[1], std::complex<double>(-0.5, -1), 1e-4))
373  << "Eigenvalue: " << eig1[1];
374  EXPECT_NEAR(T(1, 1), -8, 1e-10);
375 
376  // final test, transform T back to the original matrix
377  Eigen::MatrixXd Tf = Q1 * Q2 * Q3 * Q4 * T * Q4.transpose() * Q3.transpose() * Q2.transpose() * Q1.transpose();
378  EXPECT_EQ_MATRIX(Tf, T_original, 1e-4);
379 }
380 
381 TEST_F(TestSchur, reorder_schur_blocks_real_2d)
382 {
383  Eigen::Matrix2d T;
384  T << 8, 31, 0, -4;
385 
386  Eigen::Matrix2d Q = Eigen::Matrix2d::Identity();
387 
388  int subspace_dim = 0;
389 
390  auto is_neg_real = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() < 0; };
391  reorder_schur_blocks(T, Q, is_neg_real, &subspace_dim);
392 
393  EXPECT_NEAR(T(0, 0), -4, 1e-10);
394  EXPECT_NEAR(T(1, 1), 8, 1e-10);
395  EXPECT_NEQ_MATRIX(Q, Eigen::Matrix2d::Identity(), 1e-5);
396  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix2d::Identity(), 1e-5);
397  EXPECT_EQ(subspace_dim, 1);
398 
399  subspace_dim = -10;
400  auto is_pos_real = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() > 0; };
401  reorder_schur_blocks(T, Q, is_pos_real);
402 
403  EXPECT_NEAR(T(0, 0), 8, 1e-10);
404  EXPECT_NEAR(T(1, 1), -4, 1e-10);
405  EXPECT_EQ(subspace_dim, -10);
406 
407  auto always_true = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return true; };
408  reorder_schur_blocks(T, Q, always_true);
409 
410  EXPECT_NEAR(T(0, 0), 8, 1e-10);
411  EXPECT_NEAR(T(1, 1), -4, 1e-10);
412 
413  auto always_false = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return false; };
414  reorder_schur_blocks(T, Q, always_false);
415 
416  EXPECT_NEAR(T(0, 0), 8, 1e-10);
417  EXPECT_NEAR(T(1, 1), -4, 1e-10);
418 }
419 
420 TEST_F(TestSchur, reorder_schur_blocks_real_6d)
421 {
422  Eigen::MatrixXd T(6, 6);
423  T << 6, -28.9828, 114.6002, 334.8702, 642.8458, 858.6334, 0, 5, -18.9797, -55.4600, -106.4651, -142.2044, 0, 0, 4, 10.9599, 21.0520, 28.0873, 0,
424  0, 0, -3, -5.1069, -6.8522, 0, 0, 0, 0, -2, -2.3436, 0, 0, 0, 0, 0, -1;
425 
426  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6);
427 
428  int subspace_dim = 0;
429 
430  auto select = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() < -1; };
431  reorder_schur_blocks(T, Q, select, &subspace_dim);
432 
433  EXPECT_LT(T(0, 0), -1);
434  EXPECT_LT(T(1, 1), -1);
435  EXPECT_GE(T(2, 2), -1);
436  EXPECT_GE(T(3, 3), -1);
437  EXPECT_GE(T(5, 5), -1);
438  EXPECT_GE(T(5, 5), -1);
439  EXPECT_EQ(subspace_dim, 2);
440 
441  EXPECT_NEQ_MATRIX(Q, Eigen::MatrixXd::Identity(6, 6), 1e-5);
442  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::MatrixXd::Identity(6, 6), 1e-5);
443 }
444 
445 TEST_F(TestSchur, reorder_schur_blocks_complex_4d)
446 {
447  /*
448  T =
449  2.0000 -5.6559 4.9142 -29.6122
450  0.3978 2.0000 -1.4659 8.9097
451  0 0 -1.0000 5.0024
452  0 0 -0.7996 -1.0000
453  */
454  Eigen::Matrix4d T;
455  T << 2, -5.6559, 4.9142, -29.6122, 0.3978, 2, -1.4659, 8.9097, 0, 0, -1, 5.0024, 0, 0, -0.7996, -1;
456 
457  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity();
458 
459  int subspace_dim = 0;
460 
461  auto select = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() < 0; };
462  reorder_schur_blocks(T, Q, select, &subspace_dim);
463 
464  double real1 = T.topLeftCorner(2, 2).eigenvalues()[0].real();
465  double real2 = T.bottomRightCorner(2, 2).eigenvalues()[0].real();
466 
467  EXPECT_TRUE(essentially_equal(real1, -1, 1e-6)) << real1;
468  EXPECT_TRUE(essentially_equal(real2, 2, 1e-6)) << real2;
469 
470  EXPECT_EQ(subspace_dim, 2);
471 
472  EXPECT_NEQ_MATRIX(Q, Eigen::Matrix4d::Identity(), 1e-5);
473  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix4d::Identity(), 1e-5);
474 }
475 
476 TEST_F(TestSchur, reorder_schur_blocks_mixed_3d)
477 {
478  /*
479  T =
480  2.0000 -5.7972 3.4035
481  0.3881 2.0000 -0.8847
482  0 0 -1.0000
483  */
484  Eigen::Matrix3d T;
485  T << 2, -5.7972, 3.4035, 0.3881, 2, -0.8847, 0, 0, -1;
486 
487  Eigen::Matrix3d Q = Eigen::Matrix3d::Identity();
488 
489  int subspace_dim = 0;
490 
491  auto select = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() < 0; };
492  reorder_schur_blocks(T, Q, select, &subspace_dim);
493 
494  double real1 = T.bottomRightCorner(2, 2).eigenvalues()[0].real();
495  EXPECT_TRUE(essentially_equal(real1, 2, 1e-6)) << real1;
496  EXPECT_NEAR(T(0, 0), -1, 1e-10);
497  EXPECT_EQ(subspace_dim, 1);
498 
499  EXPECT_NEQ_MATRIX(Q, Eigen::Matrix3d::Identity(), 1e-5);
500  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix3d::Identity(), 1e-5);
501 
502  // swap back
503  auto select2 = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() > 0; };
504  reorder_schur_blocks(T, Q, select2, &subspace_dim);
505 
506  real1 = T.topLeftCorner(2, 2).eigenvalues()[0].real();
507  EXPECT_TRUE(essentially_equal(real1, 2, 1e-6)) << real1;
508 
509  EXPECT_NEAR(T(2, 2), -1, 1e-10);
510  EXPECT_EQ(subspace_dim, 2);
511 
512  EXPECT_NEQ_MATRIX(Q, Eigen::Matrix3d::Identity(), 1e-5);
513  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::Matrix3d::Identity(), 1e-5);
514 }
515 
516 TEST_F(TestSchur, reorder_schur_blocks_mixed_8d)
517 {
518  /*
519  T =
520  4 6.8834 -22.8817 -68.4663 -60.1433 -350.9628 643.1324 989.0186
521  0 2.0000 -5.6484 -16.7104 -14.6790 -85.6590 156.9690 241.3865
522  0 0.3983 2.0000 5.0269 4.4159 25.7680 -47.2178 -72.6187
523  0 0 0 -3.0000 -2.3424 -13.6697 25.0501 38.5195
524  0 0 0 0 -1.0000 -4.8323 8.5495 13.1573
525  0 0 0 0 0.8278 -1.0000 1.4519 2.2920
526  0 0 0 0 0 0 -2.0000 -2.5705
527  0 0 0 0 0 0 0 -1.0000
528  */
529 
530  Eigen::MatrixXd T(8, 8);
531  T << 4, 6.8834, -22.8817, -68.4663, -60.1433, -350.9628, 643.1324, 989.0186, 0, 2.0000, -5.6484, -16.7104, -14.6790, -85.6590, 156.9690, 241.3865,
532  0, 0.3983, 2.0000, 5.0269, 4.4159, 25.7680, -47.2178, -72.6187, 0, 0, 0, -3.0000, -2.3424, -13.6697, 25.0501, 38.5195, 0, 0, 0, 0, -1.0000,
533  -4.8323, 8.5495, 13.1573, 0, 0, 0, 0, 0.8278, -1.0000, 1.4519, 2.2920, 0, 0, 0, 0, 0, 0, -2.0000, -2.5705, 0, 0, 0, 0, 0, 0, 0, -1.0000;
534 
535  Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(8, 8);
536 
537  int subspace_dim = 0;
538 
539  auto select = [](const Eigen::Ref<const Eigen::MatrixXd>& block) { return block.eigenvalues()[0].real() < 0; };
540  reorder_schur_blocks(T, Q, select, &subspace_dim);
541 
542  // search for blocks
543  int row = 0;
544  while (row < T.rows())
545  {
546  // check, if we have a 1 x 1 or 2 x 2 block
547  int p;
548  if (row + 1 >= T.rows())
549  p = 1;
550  else if (approx_zero(T(row + 1, row), 1e-10))
551  p = 1;
552  else
553  p = 2;
554 
555  if (p == 1)
556  {
557  if (row < 5)
558  EXPECT_LT(T(row, row), 0);
559  else
560  EXPECT_GE(T(row, row), 0);
561  }
562  else
563  {
564  double real = T.block(row, row, 2, 2).eigenvalues()[0].real();
565  if (row < 5)
566  EXPECT_LT(real, 0);
567  else
568  EXPECT_GE(real, 0);
569  }
570 
571  row += p;
572  }
573  EXPECT_EQ(subspace_dim, 5);
574 
575  EXPECT_NEQ_MATRIX(Q, Eigen::MatrixXd::Identity(8, 8), 1e-5);
576  EXPECT_EQ_MATRIX((Q.transpose() * Q).eval(), Eigen::MatrixXd::Identity(8, 8), 1e-5);
577 }
TestSchur::~TestSchur
virtual ~TestSchur()
Definition: test_schur.cpp:47
corbo::essentially_equal
bool essentially_equal(double a, double b, double epsilon=1e-6)
Check if two doubles are essentially equal.
Definition: value_comparison.h:102
corbo::approx_zero
bool approx_zero(double val, double epsilon=1e-10)
Check if a double is appoximately zero.
Definition: value_comparison.h:124
real
float real
Definition: datatypes.h:10
macros.h
console.h
TEST_F
TEST_F(TestSchur, schur_decomposition_2d1)
Definition: test_schur.cpp:59
value_comparison.h
corbo::schur_decomposition_2d
void schur_decomposition_2d(Eigen::Ref< Eigen::Matrix2d > T, Eigen::Ref< Eigen::Matrix2d > U)
Perform the 2D Real Schur decomposition.
Definition: schur.cpp:66
TestSchur
Definition: test_schur.cpp:41
EXPECT_NEQ_MATRIX
#define EXPECT_NEQ_MATRIX(A, B, tol)
Definition: macros.h:68
row
EIGEN_DEVICE_FUNC RowXpr row(Index i)
This is the const version of row(). *‍/.
Definition: BlockMethods.h:859
corbo::swap_schur_blocks
bool swap_schur_blocks(Eigen::Ref< Eigen::MatrixXd > T, int ra11, int p, int q, Eigen::Ref< Eigen::MatrixXd > Q, bool standardize)
Definition: schur.cpp:192
EXPECT_EQ_MATRIX
#define EXPECT_EQ_MATRIX(A, B, tol)
Definition: macros.h:61
block
EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL EIGEN_DEVICE_FUNC BlockXpr block(Index startRow, Index startCol, Index blockRows, Index blockCols)
This is the const version of block(Index,Index,Index,Index). *‍/.
Definition: BlockMethods.h:64
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
TestSchur::TestSchur
TestSchur()
Definition: test_schur.cpp:45
schur.h
corbo::reorder_schur_blocks
bool reorder_schur_blocks(Eigen::Ref< Eigen::MatrixXd > T, Eigen::Ref< Eigen::MatrixXd > Q, Predicate predicate, int *subspace_dim, bool standardize)
Definition: schur.hpp:57

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