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.Assert;
20  import org.junit.Test;
21  
22  public class RiccatiEquationSolverTest {
23  
24      @Test
25      public void test_real_2_2() {
26          // AA = [-3 2;1 1];
27          // BB = [0;1];
28  
29          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
30              { -3, 2 }, { 1, 1 }
31          });
32  
33          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
34              { 0 }, { 1 }
35          });
36  
37          RealMatrix R = MatrixUtils.createRealIdentityMatrix(1);
38          RealMatrix Q = MatrixUtils.createRealIdentityMatrix(2);
39  
40          RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
41  
42          RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
43              { 0.3221, 0.7407 }, { 0.7407, 3.2277 }
44          });
45  
46          checkEquals(P_expected, a.getP(), 1.0e-4);
47  
48          // P
49          // 0.3221 0.7407
50          // 0.7407 3.2277
51          // K
52          // 0.7407 3.2277
53      }
54  
55      @Test
56      public void test_imaginary_2_2() {
57          // AA = [3 -2;4 -1];
58          // BB = [0;1];
59  
60          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
61              { 3, -2 }, { 4, -1 }
62          });
63  
64          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
65              { 0 }, { 1 }
66          });
67  
68          RealMatrix R = MatrixUtils.createRealIdentityMatrix(1);
69          RealMatrix Q = MatrixUtils.createRealIdentityMatrix(2);
70  
71          RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
72  
73          RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
74              { 19.7598, -7.6430 }, { -7.6430, 4.7072 }
75          });
76  
77          checkEquals(P_expected, a.getP(), 1.0e-4);
78  
79          // P
80          // 19.7598 -7.6430
81          // -7.6430 4.7072
82          //
83          // K
84          // -7.6430 4.7072
85  
86      }
87  
88      @Test
89      public void test_imaginary_6_6() {
90  
91          RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
92              { 1, 0, 0, 1, 0, 0 }, { 1, 0, 0, 0, 1, 0 },
93              { 1, 0, 0, 0, 0, 1 }, { 0, 0, 0, 0, 0, 0 },
94              { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
95          });
96  
97          RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
98              { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { -0.0032, 0, 0 },
99              { 0, -0.0028, 0 }, { 0, 0, -0.0019 }
100         });
101 
102         RealMatrix R = MatrixUtils.createRealIdentityMatrix(3);
103         RealMatrix Q = MatrixUtils.createRealIdentityMatrix(6);
104 
105         RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
106 
107         RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
108             { 2.2791, 0.0036, 0.0045, 2.1121, 0.0628, 0.0982 },
109             { 0.0036, 0.0002, -0.0000, 0.0017, 0.0029, -0.0011 },
110             { 0.0045, -0.0000, 0.0003, 0.0022, -0.0011, 0.0034 },
111             { 2.1121, 0.0017, 0.0022, 2.0307, 0.0305, 0.0479 },
112             { 0.0628, 0.0029, -0.0011, 0.0305, 0.0746, -0.0387 },
113             { 0.0982, -0.0011, 0.0034, 0.0479, -0.0387, 0.0967 } });
114 
115         checkEquals(P_expected, a.getP().scalarMultiply(1e-05), 1.0e-4);
116 
117         // 1.0e+05 *
118         // 2.2791 0.0036 0.0045 2.1121 0.0628 0.0982
119         // 0.0036 0.0002 -0.0000 0.0017 0.0029 -0.0011
120         // 0.0045 -0.0000 0.0003 0.0022 -0.0011 0.0034
121         // 2.1121 0.0017 0.0022 2.0307 0.0305 0.0479
122         // 0.0628 0.0029 -0.0011 0.0305 0.0746 -0.0387
123         // 0.0982 -0.0011 0.0034 0.0479 -0.0387 0.0967
124     }
125 
126     @Test
127     public void test_imaginary_6_6_ill_conditioned() {
128         // 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;
129         // 0 0 0 0 0 0];
130         // B = [ 0 0 0; 0 0 0; 0 0 0; -0.0032 0 0; 0 -0.0028 0; 0 0 -0.0019];
131         // R= [1 0 0; 0 1 0; 0 0 1];
132         // 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;
133         // 0 0 0 0 0 1];
134 
135         RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
136             { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, 0 },
137             { 0, 0, 0, 0, 0, 1 }, { 0, 0, 0, 0, 0, 0 },
138             { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
139         });
140 
141         RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
142             { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { -0.0032, 0, 0 },
143             { 0, -0.0028, 0 }, { 0, 0, -0.0019 }
144         });
145 
146         RealMatrix R = MatrixUtils.createRealIdentityMatrix(3);
147         RealMatrix Q = MatrixUtils.createRealIdentityMatrix(6);
148         RiccatiEquationSolver a = new RiccatiEquationSolverImpl(A, B, Q, R);
149 
150         RealMatrix P_expected = MatrixUtils.createRealMatrix(new double[][] {
151             { 25.02, 0.0, -0.0, 312.5, -0.0, -0.0 },
152             { -0.0, 26.7448, -0.0, -0.0, 357.1429, -0.0 },
153             { -0.0, -0.0, 32.4597, 0.0, -0.0, 526.3158 },
154             { 312.5, 0.0, 0.0, 7818.7475, 0.0, 0.0 },
155             { -0.0, 357.1429, -0.0, 0.0, 9551.7235, -0.0 },
156             { -0.0, -0.0, 526.3158, 0.0, -0.0, 17084.0482 }
157         });
158 
159         checkEquals(P_expected, a.getP(), 1.0e-4);
160 
161     }
162 
163     private void checkEquals(final RealMatrix reference, final RealMatrix m, final double tol) {
164         Assert.assertEquals(reference.getRowDimension(), m.getRowDimension());
165         Assert.assertEquals(reference.getColumnDimension(), m.getColumnDimension());
166         for (int i = 0; i < reference.getRowDimension(); ++i) {
167             for (int j = 0; j < reference.getColumnDimension(); ++j) {
168                 Assert.assertEquals(reference.getEntry(i, j), m.getEntry(i, j), tol);
169             }
170         }
171     }
172 
173 }