1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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
28
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
50
51
52
53
54 }
55
56 @Test
57 void test_imaginary_2_2() {
58
59
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
81
82
83
84
85
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
119
120
121
122
123
124
125 }
126
127 @Test
128 void test_imaginary_6_6_ill_conditioned() {
129
130
131
132
133
134
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 }