View Javadoc
1   /*
2    * Licensed to the Hipparchus project under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      https://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.hipparchus.linear;
18  
19  import org.junit.jupiter.api.Test;
20  
21  import static org.junit.jupiter.api.Assertions.assertEquals;
22  
23  class RiccatiEquationSolverTest {
24  
25      @Test
26      void test_real_2_2() {
27          // AA = [-3 2;1 1];
28          // BB = [0;1];
29  
30          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
31              { -3, 2 }, { 1, 1 }
32          });
33  
34          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
35              { 0 }, { 1 }
36          });
37  
38          RealMatrix R = MatrixUtils.createRealIdentityMatrix(1);
39          RealMatrix Q = MatrixUtils.createRealIdentityMatrix(2);
40  
41          RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
42  
43          RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
44              { 0.3221, 0.7407 }, { 0.7407, 3.2277 }
45          });
46  
47          checkEquals(P_expected, a.getP(), 1.0e-4);
48  
49          // P
50          // 0.3221 0.7407
51          // 0.7407 3.2277
52          // K
53          // 0.7407 3.2277
54      }
55  
56      @Test
57      void test_imaginary_2_2() {
58          // AA = [3 -2;4 -1];
59          // BB = [0;1];
60  
61          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
62              { 3, -2 }, { 4, -1 }
63          });
64  
65          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
66              { 0 }, { 1 }
67          });
68  
69          RealMatrix R = MatrixUtils.createRealIdentityMatrix(1);
70          RealMatrix Q = MatrixUtils.createRealIdentityMatrix(2);
71  
72          RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
73  
74          RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
75              { 19.7598, -7.6430 }, { -7.6430, 4.7072 }
76          });
77  
78          checkEquals(P_expected, a.getP(), 1.0e-4);
79  
80          // P
81          // 19.7598 -7.6430
82          // -7.6430 4.7072
83          //
84          // K
85          // -7.6430 4.7072
86  
87      }
88  
89      @Test
90      void test_imaginary_6_6() {
91  
92          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
93              { 1, 0, 0, 1, 0, 0 }, { 1, 0, 0, 0, 1, 0 },
94              { 1, 0, 0, 0, 0, 1 }, { 0, 0, 0, 0, 0, 0 },
95              { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
96          });
97  
98          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
99              { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { -0.0032, 0, 0 },
100             { 0, -0.0028, 0 }, { 0, 0, -0.0019 }
101         });
102 
103         RealMatrix R = MatrixUtils.createRealIdentityMatrix(3);
104         RealMatrix Q = MatrixUtils.createRealIdentityMatrix(6);
105 
106         RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
107 
108         RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
109             { 2.2791, 0.0036, 0.0045, 2.1121, 0.0628, 0.0982 },
110             { 0.0036, 0.0002, -0.0000, 0.0017, 0.0029, -0.0011 },
111             { 0.0045, -0.0000, 0.0003, 0.0022, -0.0011, 0.0034 },
112             { 2.1121, 0.0017, 0.0022, 2.0307, 0.0305, 0.0479 },
113             { 0.0628, 0.0029, -0.0011, 0.0305, 0.0746, -0.0387 },
114             { 0.0982, -0.0011, 0.0034, 0.0479, -0.0387, 0.0967 } });
115 
116         checkEquals(P_expected, a.getP().scalarMultiply(1e-05), 1.0e-4);
117 
118         // 1.0e+05 *
119         // 2.2791 0.0036 0.0045 2.1121 0.0628 0.0982
120         // 0.0036 0.0002 -0.0000 0.0017 0.0029 -0.0011
121         // 0.0045 -0.0000 0.0003 0.0022 -0.0011 0.0034
122         // 2.1121 0.0017 0.0022 2.0307 0.0305 0.0479
123         // 0.0628 0.0029 -0.0011 0.0305 0.0746 -0.0387
124         // 0.0982 -0.0011 0.0034 0.0479 -0.0387 0.0967
125     }
126 
127     @Test
128     void test_imaginary_6_6_ill_conditioned() {
129         // A = [0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 0 0 0 0 0;
130         // 0 0 0 0 0 0];
131         // B = [ 0 0 0; 0 0 0; 0 0 0; -0.0032 0 0; 0 -0.0028 0; 0 0 -0.0019];
132         // R= [1 0 0; 0 1 0; 0 0 1];
133         // Q = [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;
134         // 0 0 0 0 0 1];
135 
136         RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
137             { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, 0 },
138             { 0, 0, 0, 0, 0, 1 }, { 0, 0, 0, 0, 0, 0 },
139             { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
140         });
141 
142         RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
143             { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { -0.0032, 0, 0 },
144             { 0, -0.0028, 0 }, { 0, 0, -0.0019 }
145         });
146 
147         RealMatrix R = MatrixUtils.createRealIdentityMatrix(3);
148         RealMatrix Q = MatrixUtils.createRealIdentityMatrix(6);
149         RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
150 
151         RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
152             { 25.02, 0.0, -0.0, 312.5, -0.0, -0.0 },
153             { -0.0, 26.7448, -0.0, -0.0, 357.1429, -0.0 },
154             { -0.0, -0.0, 32.4597, 0.0, -0.0, 526.3158 },
155             { 312.5, 0.0, 0.0, 7818.7475, 0.0, 0.0 },
156             { -0.0, 357.1429, -0.0, 0.0, 9551.7235, -0.0 },
157             { -0.0, -0.0, 526.3158, 0.0, -0.0, 17084.0482 }
158         });
159 
160         checkEquals(P_expected, a.getP(), 1.0e-4);
161 
162     }
163 
164     private void checkEquals(final RealMatrix reference, final RealMatrix m, final double tol) {
165         assertEquals(reference.getRowDimension(), m.getRowDimension());
166         assertEquals(reference.getColumnDimension(), m.getColumnDimension());
167         for (int i = 0; i < reference.getRowDimension(); ++i) {
168             for (int j = 0; j < reference.getColumnDimension(); ++j) {
169                 assertEquals(reference.getEntry(i, j), m.getEntry(i, j), tol);
170             }
171         }
172     }
173 
174 }