RiccatiEquationSolverImpl.java
/*
* Licensed to the Hipparchus project under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* The Hipparchus project licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.hipparchus.linear;
import org.hipparchus.complex.Complex;
import org.hipparchus.exception.LocalizedCoreFormats;
import org.hipparchus.exception.MathIllegalArgumentException;
import org.hipparchus.exception.MathRuntimeException;
import org.hipparchus.util.FastMath;
/**
*
* This solver computes the solution using the following approach:
*
* 1. Compute the Hamiltonian matrix 2. Extract its complex eigen vectors (not
* the best solution, a better solution would be ordered Schur transformation)
* 3. Approximate the initial solution given by 2 using the Kleinman algorithm
* (an iterative method)
*/
public class RiccatiEquationSolverImpl implements RiccatiEquationSolver {
/** Internally used maximum iterations. */
private static final int MAX_ITERATIONS = 100;
/** Internally used epsilon criteria. */
private static final double EPSILON = 1e-8;
/** The solution of the algebraic Riccati equation. */
private final RealMatrix P;
/** The computed K. */
private final RealMatrix K;
/**
* Constructor of the solver. A and B should be compatible. B and R must be
* multiplicative compatible. A and Q must be multiplicative compatible. R
* must be invertible.
*
* @param A state transition matrix
* @param B control multipliers matrix
* @param Q state cost matrix
* @param R control cost matrix
*/
public RiccatiEquationSolverImpl(final RealMatrix A, final RealMatrix B,
final RealMatrix Q, final RealMatrix R) {
// checking A
if (!A.isSquare()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.NON_SQUARE_MATRIX,
A.getRowDimension(), A.getColumnDimension());
}
if (A.getColumnDimension() != B.getRowDimension()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
A.getRowDimension(), B.getRowDimension());
}
MatrixUtils.checkMultiplicationCompatible(B, R);
MatrixUtils.checkMultiplicationCompatible(A, Q);
// checking R
final SingularValueDecomposition svd = new SingularValueDecomposition(R);
if (!svd.getSolver().isNonSingular()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.SINGULAR_MATRIX);
}
final RealMatrix R_inv = svd.getSolver().getInverse();
P = computeP(A, B, Q, R, R_inv, MAX_ITERATIONS, EPSILON);
K = R_inv.multiplyTransposed(B).multiply(P);
}
/**
* Compute an initial stable solution and then applies the Kleinman
* algorithm to approximate it using an EPSILON.
*
* @param A state transition matrix
* @param B control multipliers matrix
* @param Q state cost matrix
* @param R control cost matrix
* @param R_inv inverse of matrix R
* @param maxIterations maximum number of iterations
* @param epsilon epsilon to be used
* @return matrix P, solution of the algebraic Riccati equation
*/
private RealMatrix computeP(final RealMatrix A, final RealMatrix B,
final RealMatrix Q, final RealMatrix R, final RealMatrix R_inv,
final int maxIterations, final double epsilon) {
final RealMatrix P_ = computeInitialP(A, B, Q, R_inv);
return approximateP(A, B, Q, R, R_inv, P_, maxIterations, epsilon);
}
/**
* Compute initial P using the Hamiltonian and the ordered eigen values
* decomposition.
*
* @param A state transition matrix
* @param B control multipliers matrix
* @param Q state cost matrix
* @param R_inv inverse of matrix R
* @return initial solution
*/
private RealMatrix computeInitialP(final RealMatrix A, final RealMatrix B,
final RealMatrix Q, final RealMatrix R_inv) {
final RealMatrix B_tran = B.transpose();
// computing the Hamiltonian Matrix
final RealMatrix m11 = A;
final RealMatrix m12 = B.multiply(R_inv).multiply(B_tran).scalarMultiply(-1).scalarAdd(0);
final RealMatrix m21 = Q.scalarMultiply(-1).scalarAdd(0);
final RealMatrix m22 = A.transpose().scalarMultiply(-1).scalarAdd(0);
if (m11.getRowDimension() != m12.getRowDimension()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
m11.getRowDimension(), m12.getRowDimension());
}
if (m21.getRowDimension() != m22.getRowDimension()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
m21.getRowDimension(), m22.getRowDimension());
}
if (m11.getColumnDimension() != m21.getColumnDimension()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
m11.getColumnDimension(), m21.getColumnDimension());
}
if (m21.getColumnDimension() != m22.getColumnDimension()) {
throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
m21.getColumnDimension(), m22.getColumnDimension());
}
// defining M
final RealMatrix m = MatrixUtils.createRealMatrix(m11.getRowDimension() + m21.getRowDimension(),
m11.getColumnDimension() + m12.getColumnDimension());
// defining submatrixes
m.setSubMatrix(m11.getData(), 0, 0);
m.setSubMatrix(m12.getData(), 0, m11.getColumnDimension());
m.setSubMatrix(m21.getData(), m11.getRowDimension(), 0);
m.setSubMatrix(m22.getData(), m11.getRowDimension(), m11.getColumnDimension());
// eigen decomposition
// numerically bad, but it is used for the initial stable solution for
// the
// Kleinman Algorithm
// it must be ordered in order to work with submatrices
final OrderedComplexEigenDecomposition eigenDecomposition =
new OrderedComplexEigenDecomposition(m,
EPSILON,
ComplexEigenDecomposition.DEFAULT_EPSILON,
ComplexEigenDecomposition.DEFAULT_EPSILON_AV_VD_CHECK);
final FieldMatrix<Complex> u = eigenDecomposition.getV();
// solving linear system
// P = U_21*U_11^-1
final FieldMatrix<Complex> u11 = u.getSubMatrix(0,
m11.getRowDimension() - 1, 0, m11.getColumnDimension() - 1);
final FieldMatrix<Complex> u21 = u.getSubMatrix(m11.getRowDimension(),
2 * m11.getRowDimension() - 1, 0, m11.getColumnDimension() - 1);
final FieldDecompositionSolver<Complex> solver = new FieldLUDecomposition<Complex>(u11).getSolver();
if (!solver.isNonSingular()) {
throw new MathRuntimeException(LocalizedCoreFormats.SINGULAR_MATRIX);
}
// solving U_11^{-1}
FieldMatrix<Complex> u11_inv = solver.getInverse();
// P = U_21*U_11^-1
FieldMatrix<Complex> p = u21.multiply(u11_inv);
// converting to realmatrix - ignoring precision errors in imaginary
// components
return convertToRealMatrix(p, Double.MAX_VALUE);
}
/**
* Applies the Kleinman's algorithm.
*
* @param A state transition matrix
* @param B control multipliers matrix
* @param Q state cost matrix
* @param R control cost matrix
* @param R_inv inverse of matrix R
* @param initialP initial solution
* @param maxIterations maximum number of iterations allowed
* @param epsilon convergence threshold
* @return improved solution
*/
private RealMatrix approximateP(final RealMatrix A, final RealMatrix B,
final RealMatrix Q, final RealMatrix R, final RealMatrix R_inv,
final RealMatrix initialP, final int maxIterations, final double epsilon) {
RealMatrix P_ = initialP;
double error = 1;
int i = 1;
while (error > epsilon) {
final RealMatrix K_ = P_.multiply(B).multiply(R_inv).scalarMultiply(-1);
// X = AA+BB*K1';
final RealMatrix X = A.add(B.multiplyTransposed(K_));
// Y = -K1*RR*K1' - QQ;
final RealMatrix Y = K_.multiply(R).multiplyTransposed(K_).scalarMultiply(-1).subtract(Q);
final Array2DRowRealMatrix X_ = (Array2DRowRealMatrix) X.transpose();
final Array2DRowRealMatrix Y_ = (Array2DRowRealMatrix) Y;
final Array2DRowRealMatrix eyeX =
(Array2DRowRealMatrix) MatrixUtils.createRealIdentityMatrix(X_.getRowDimension());
// X1=kron(X',eye(size(X))) + kron(eye(size(X)),X');
final RealMatrix X__ = X_.kroneckerProduct(eyeX).add(eyeX.kroneckerProduct(X_));
// Y1=reshape(Y,prod(size(Y)),1); %%stack
final RealMatrix Y__ = Y_.stack();
// PX = inv(X1)*Y1;
// sensitive to numerical erros
// final RealMatrix PX = MatrixUtils.inverse(X__).multiply(Y__);
DecompositionSolver solver = new LUDecomposition(X__).getSolver();
if (!solver.isNonSingular()) {
throw new MathRuntimeException(LocalizedCoreFormats.SINGULAR_MATRIX);
}
final RealMatrix PX = solver.solve(Y__);
// P = reshape(PX,sqrt(length(PX)),sqrt(length(PX))); %%unstack
final RealMatrix P__ = ((Array2DRowRealMatrix) PX).unstackSquare();
// aerror = norm(P - P1);
final RealMatrix diff = P__.subtract(P_);
// calculationg l2 norm
final SingularValueDecomposition svd = new SingularValueDecomposition(diff);
error = svd.getNorm();
P_ = P__;
i++;
if (i > maxIterations) {
throw new MathRuntimeException(LocalizedCoreFormats.CONVERGENCE_FAILED);
}
}
return P_;
}
/** {inheritDoc} */
@Override
public RealMatrix getP() {
return P;
}
/** {inheritDoc} */
@Override
public RealMatrix getK() {
return K;
}
/**
* Converts a given complex matrix into a real matrix taking into account a
* precision for the imaginary components.
*
* @param matrix complex field matrix
* @param tolerance tolerance on the imaginary part
* @return real matrix.
*/
private RealMatrix convertToRealMatrix(FieldMatrix<Complex> matrix, double tolerance) {
final RealMatrix toRet = MatrixUtils.createRealMatrix(matrix.getRowDimension(), matrix.getRowDimension());
for (int i = 0; i < toRet.getRowDimension(); i++) {
for (int j = 0; j < toRet.getColumnDimension(); j++) {
Complex c = matrix.getEntry(i, j);
if (c.getImaginary() != 0 && FastMath.abs(c.getImaginary()) > tolerance) {
throw new MathRuntimeException(LocalizedCoreFormats.COMPLEX_CANNOT_BE_CONSIDERED_A_REAL_NUMBER,
c.getReal(), c.getImaginary());
}
toRet.setEntry(i, j, c.getReal());
}
}
return toRet;
}
}