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.hipparchus.complex.Complex;
20  import org.hipparchus.exception.LocalizedCoreFormats;
21  import org.hipparchus.exception.MathIllegalArgumentException;
22  import org.hipparchus.exception.MathRuntimeException;
23  import org.hipparchus.util.FastMath;
24  
25  /**
26   *
27   * This solver computes the solution using the following approach:
28   *
29   * 1. Compute the Hamiltonian matrix 2. Extract its complex eigen vectors (not
30   * the best solution, a better solution would be ordered Schur transformation)
31   * 3. Approximate the initial solution given by 2 using the Kleinman algorithm
32   * (an iterative method)
33   */
34  public class RiccatiEquationSolverImpl implements RiccatiEquationSolver {
35  
36      /** Internally used maximum iterations. */
37      private static final int MAX_ITERATIONS = 100;
38  
39      /** Internally used epsilon criteria. */
40      private static final double EPSILON = 1e-8;
41  
42      /** The solution of the algebraic Riccati equation. */
43      private final RealMatrix P;
44  
45      /** The computed K. */
46      private final RealMatrix K;
47  
48      /**
49       * Constructor of the solver. A and B should be compatible. B and R must be
50       * multiplicative compatible. A and Q must be multiplicative compatible. R
51       * must be invertible.
52       *
53       * @param A state transition matrix
54       * @param B control multipliers matrix
55       * @param Q state cost matrix
56       * @param R control cost matrix
57       */
58      public RiccatiEquationSolverImpl(final RealMatrix A, final RealMatrix B,
59                                       final RealMatrix Q, final RealMatrix R) {
60  
61          // checking A
62          if (!A.isSquare()) {
63              throw new MathIllegalArgumentException(LocalizedCoreFormats.NON_SQUARE_MATRIX,
64                                                     A.getRowDimension(), A.getColumnDimension());
65          }
66          if (A.getColumnDimension() != B.getRowDimension()) {
67              throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
68                                                     A.getRowDimension(), B.getRowDimension());
69          }
70          MatrixUtils.checkMultiplicationCompatible(B, R);
71          MatrixUtils.checkMultiplicationCompatible(A, Q);
72  
73          // checking R
74          final SingularValueDecomposition svd = new SingularValueDecomposition(R);
75          if (!svd.getSolver().isNonSingular()) {
76              throw new MathIllegalArgumentException(LocalizedCoreFormats.SINGULAR_MATRIX);
77          }
78  
79          final RealMatrix R_inv = svd.getSolver().getInverse();
80  
81          P = computeP(A, B, Q, R, R_inv, MAX_ITERATIONS, EPSILON);
82  
83          K = R_inv.multiplyTransposed(B).multiply(P);
84      }
85  
86      /**
87       * Compute an initial stable solution and then applies the Kleinman
88       * algorithm to approximate it using an EPSILON.
89       *
90       * @param A state transition matrix
91       * @param B control multipliers matrix
92       * @param Q state cost matrix
93       * @param R control cost matrix
94       * @param R_inv inverse of matrix R
95       * @param maxIterations maximum number of iterations
96       * @param epsilon epsilon to be used
97       * @return matrix P, solution of the algebraic Riccati equation
98       */
99      private RealMatrix computeP(final RealMatrix A, final RealMatrix B,
100                                 final RealMatrix Q, final RealMatrix R, final RealMatrix R_inv,
101                                 final int maxIterations, final double epsilon) {
102         final RealMatrix P_ = computeInitialP(A, B, Q, R_inv);
103         return approximateP(A, B, Q, R, R_inv, P_, maxIterations, epsilon);
104     }
105 
106     /**
107      * Compute initial P using the Hamiltonian and the ordered eigen values
108      * decomposition.
109      *
110      * @param A state transition matrix
111      * @param B control multipliers matrix
112      * @param Q state cost matrix
113      * @param R_inv inverse of matrix R
114      * @return initial solution
115      */
116     private RealMatrix computeInitialP(final RealMatrix A, final RealMatrix B,
117                                        final RealMatrix Q, final RealMatrix R_inv) {
118         final RealMatrix B_tran = B.transpose();
119 
120         // computing the Hamiltonian Matrix
121         final RealMatrix m11 = A;
122         final RealMatrix m12 = B.multiply(R_inv).multiply(B_tran).scalarMultiply(-1).scalarAdd(0);
123         final RealMatrix m21 = Q.scalarMultiply(-1).scalarAdd(0);
124         final RealMatrix m22 = A.transpose().scalarMultiply(-1).scalarAdd(0);
125         if (m11.getRowDimension() != m12.getRowDimension()) {
126             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
127                                                    m11.getRowDimension(), m12.getRowDimension());
128         }
129         if (m21.getRowDimension() != m22.getRowDimension()) {
130             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
131                                                    m21.getRowDimension(), m22.getRowDimension());
132         }
133         if (m11.getColumnDimension() != m21.getColumnDimension()) {
134             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
135                                                    m11.getColumnDimension(), m21.getColumnDimension());
136         }
137         if (m21.getColumnDimension() != m22.getColumnDimension()) {
138             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
139                                                    m21.getColumnDimension(), m22.getColumnDimension());
140         }
141 
142         // defining M
143         final RealMatrix m = MatrixUtils.createRealMatrix(m11.getRowDimension() + m21.getRowDimension(),
144                                                           m11.getColumnDimension() + m12.getColumnDimension());
145         // defining submatrixes
146         m.setSubMatrix(m11.getData(), 0, 0);
147         m.setSubMatrix(m12.getData(), 0, m11.getColumnDimension());
148         m.setSubMatrix(m21.getData(), m11.getRowDimension(), 0);
149         m.setSubMatrix(m22.getData(), m11.getRowDimension(), m11.getColumnDimension());
150 
151         // eigen decomposition
152         // numerically bad, but it is used for the initial stable solution for
153         // the
154         // Kleinman Algorithm
155         // it must be ordered in order to work with submatrices
156         final OrderedComplexEigenDecomposition eigenDecomposition =
157                         new OrderedComplexEigenDecomposition(m,
158                                                              EPSILON,
159                                                              ComplexEigenDecomposition.DEFAULT_EPSILON,
160                                                              ComplexEigenDecomposition.DEFAULT_EPSILON_AV_VD_CHECK);
161         final FieldMatrix<Complex> u = eigenDecomposition.getV();
162 
163         // solving linear system
164         // P = U_21*U_11^-1
165         final FieldMatrix<Complex> u11 = u.getSubMatrix(0,
166                                                         m11.getRowDimension() - 1, 0, m11.getColumnDimension() - 1);
167         final FieldMatrix<Complex> u21 = u.getSubMatrix(m11.getRowDimension(),
168                                                         2 * m11.getRowDimension() - 1, 0, m11.getColumnDimension() - 1);
169 
170         final FieldDecompositionSolver<Complex> solver = new FieldLUDecomposition<Complex>(u11).getSolver();
171 
172         if (!solver.isNonSingular()) {
173             throw new MathRuntimeException(LocalizedCoreFormats.SINGULAR_MATRIX);
174         }
175 
176         // solving U_11^{-1}
177         FieldMatrix<Complex> u11_inv = solver.getInverse();
178 
179         // P = U_21*U_11^-1
180         FieldMatrix<Complex> p = u21.multiply(u11_inv);
181 
182         // converting to realmatrix - ignoring precision errors in imaginary
183         // components
184         return convertToRealMatrix(p, Double.MAX_VALUE);
185 
186     }
187 
188     /**
189      * Applies the Kleinman's algorithm.
190      *
191      * @param A state transition matrix
192      * @param B control multipliers matrix
193      * @param Q state cost matrix
194      * @param R control cost matrix
195      * @param R_inv inverse of matrix R
196      * @param initialP initial solution
197      * @param maxIterations maximum number of iterations allowed
198      * @param epsilon convergence threshold
199      * @return improved solution
200      */
201     private RealMatrix approximateP(final RealMatrix A, final RealMatrix B,
202                                     final RealMatrix Q, final RealMatrix R, final RealMatrix R_inv,
203                                     final RealMatrix initialP, final int maxIterations, final double epsilon) {
204         RealMatrix P_ = initialP;
205 
206         double error = 1;
207         int i = 1;
208         while (error > epsilon) {
209             final RealMatrix K_ = P_.multiply(B).multiply(R_inv).scalarMultiply(-1);
210 
211             // X = AA+BB*K1';
212             final RealMatrix X = A.add(B.multiplyTransposed(K_));
213             // Y = -K1*RR*K1' - QQ;
214             final RealMatrix Y = K_.multiply(R).multiplyTransposed(K_).scalarMultiply(-1).subtract(Q);
215 
216             final Array2DRowRealMatrix X_ = (Array2DRowRealMatrix) X.transpose();
217             final Array2DRowRealMatrix Y_ = (Array2DRowRealMatrix) Y;
218             final Array2DRowRealMatrix eyeX =
219                             (Array2DRowRealMatrix) MatrixUtils.createRealIdentityMatrix(X_.getRowDimension());
220 
221             // X1=kron(X',eye(size(X))) + kron(eye(size(X)),X');
222             final RealMatrix X__ = X_.kroneckerProduct(eyeX).add(eyeX.kroneckerProduct(X_));
223             // Y1=reshape(Y,prod(size(Y)),1); %%stack
224             final RealMatrix Y__ = Y_.stack();
225 
226             // PX = inv(X1)*Y1;
227             // sensitive to numerical erros
228             // final RealMatrix PX = MatrixUtils.inverse(X__).multiply(Y__);
229             DecompositionSolver solver = new LUDecomposition(X__).getSolver();
230             if (!solver.isNonSingular()) {
231                 throw new MathRuntimeException(LocalizedCoreFormats.SINGULAR_MATRIX);
232             }
233             final RealMatrix PX = solver.solve(Y__);
234 
235             // P = reshape(PX,sqrt(length(PX)),sqrt(length(PX))); %%unstack
236             final RealMatrix P__ = ((Array2DRowRealMatrix) PX).unstackSquare();
237 
238             // aerror = norm(P - P1);
239             final RealMatrix diff = P__.subtract(P_);
240             // calculationg l2 norm
241             final SingularValueDecomposition svd = new SingularValueDecomposition(diff);
242             error = svd.getNorm();
243 
244             P_ = P__;
245             i++;
246             if (i > maxIterations) {
247                 throw new MathRuntimeException(LocalizedCoreFormats.CONVERGENCE_FAILED);
248             }
249         }
250 
251         return P_;
252     }
253 
254     /** {inheritDoc} */
255     @Override
256     public RealMatrix getP() {
257         return P;
258     }
259 
260     /** {inheritDoc} */
261     @Override
262     public RealMatrix getK() {
263         return K;
264     }
265 
266     /**
267      * Converts a given complex matrix into a real matrix taking into account a
268      * precision for the imaginary components.
269      *
270      * @param matrix complex field matrix
271      * @param tolerance tolerance on the imaginary part
272      * @return real matrix.
273      */
274     private RealMatrix convertToRealMatrix(FieldMatrix<Complex> matrix, double tolerance) {
275         final RealMatrix toRet = MatrixUtils.createRealMatrix(matrix.getRowDimension(), matrix.getRowDimension());
276         for (int i = 0; i < toRet.getRowDimension(); i++) {
277             for (int j = 0; j < toRet.getColumnDimension(); j++) {
278                 Complex c = matrix.getEntry(i, j);
279                 if (c.getImaginary() != 0 && FastMath.abs(c.getImaginary()) > tolerance) {
280                     throw new MathRuntimeException(LocalizedCoreFormats.COMPLEX_CANNOT_BE_CONSIDERED_A_REAL_NUMBER,
281                                                    c.getReal(), c.getImaginary());
282                 }
283                 toRet.setEntry(i, j, c.getReal());
284             }
285         }
286         return toRet;
287     }
288 
289 }