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<>(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;
    }

}