Dlqr.java

/*
 * $Id: Dlqr.java,v 1.39 2008/07/17 07:30:03 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 */
package org.mklab.tool.control;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import org.mklab.nfc.eig.DoubleEigenSolution;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IndexedMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.NormType;
import org.mklab.nfc.scalar.DoubleNumber;


/**
 * 離散時間系のLQRを求めるクラスです。
 * 
 * <p>Discrete-time linear quadratic regulator
 * 
 * @author koga
 * @version $Revision: 1.39 $
 * @see org.mklab.tool.control.Lqr
 * @see org.mklab.tool.control.Dlqe
 */
public class Dlqr {

  /**
   * 離散時間線形システム
   * 
   * <pre><code> x[n + 1] = Ax[n] + Bu[n] </code></pre>
   * 
   * について、二次形式評価関数
   * 
   * <pre><code> J = Sum (x#Qx + u#Ru) </code> </pre>を最小にする、状態フィードバック則 <code> u = -Fx </code> のフィードバックゲイン行列 <code> F</code> を返します。
   * 
   * <p>また、リカッティ方程式 <pre><code> P - A#PA + A#PB(R + B#PB)&tilde;BP#A - Q = 0 </code></pre>の解 <code> P</code> を返します。
   * 
   * @param A A行列
   * @param B B行列
   * @param Q Q行列
   * @param R R行列
   * @return {F, P} linear quadratic regulator
   * 
   */
  public static List<DoubleMatrix> dlqr(DoubleMatrix A, DoubleMatrix B, DoubleMatrix Q, DoubleMatrix R) {
    DoubleMatrix S = B.createZero();
    return dlqr(A, B, Q, R, S);
  }

  /**
   * 評価関数が
   * 
   * <pre><code> J = Sum (x#Qx + u#Ru + 2 x#Su) dt </code></pre>
   * 
   * となるよう、入力<code>u</code>と状態<code>x</code>の積の重み行列を <code>S</code>とします。
   * 
   * @param A A行列
   * @param B B行列
   * @param Q Q行列
   * @param R R行列
   * @param S S行列
   * @return {F, P} linear quadratic regulator
   */
  public static List<DoubleMatrix> dlqr(DoubleMatrix A, DoubleMatrix B, DoubleMatrix Q, DoubleMatrix R, DoubleMatrix S) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new IllegalArgumentException(message);
    }

    int n = A.getRowSize();

    if (n != Q.getRowSize() || n != Q.getColumnSize()) {
      throw new IllegalArgumentException(Messages.getString("Dlqr.0")); //$NON-NLS-1$
    }
    if (R.getRowSize() != R.getColumnSize() || B.getColumnSize() != R.getRowSize()) {
      throw new IllegalArgumentException(Messages.getString("Dlqr.1")); //$NON-NLS-1$
    }

    DoubleNumber unit = A.getElement(1, 1);

    DoubleNumber tolerance1 = Q.frobNorm().multiply(unit.getMachineEpsilon());
    boolean con11 = (Q.eigenValue()).getRealPart().compareElementWise(".<", tolerance1.unaryMinus()).anyTrue(); //$NON-NLS-1$

    DoubleNumber qDifference = (Q.conjugateTranspose().subtract(Q).norm(NormType.ONE));
    boolean con12 = qDifference.divide(Q.norm(NormType.ONE)).isGreaterThan(tolerance1);

    if (con11 || con12) {
      throw new RuntimeException(Messages.getString("Dlqr.3")); //$NON-NLS-1$
    }

    DoubleNumber tolerance2 = (R.frobNorm()).multiply(unit.getMachineEpsilon());
    boolean con21 = (R.eigenValue()).getRealPart().compareElementWise(".<=", tolerance2.unaryMinus()).anyTrue(); //$NON-NLS-1$

    DoubleNumber rDifference = (R.conjugateTranspose().subtract(R).norm(NormType.ONE));
    boolean con22 = rDifference.divide(R.norm(NormType.ONE)).isGreaterThan(tolerance2);

    if (con21 || con22) {
      throw new RuntimeException(Messages.getString("Dlqr.4")); //$NON-NLS-1$
    }

    DoubleMatrix G11 = A.add(B.divide(R).multiply(B.conjugateTranspose()).divide(A.conjugateTranspose()).multiply(Q));
    DoubleMatrix G12 = B.divide(R).multiply(B.conjugateTranspose()).divide(A.conjugateTranspose()).unaryMinus();
    DoubleMatrix G21 = A.conjugateTranspose().leftDivide(Q).unaryMinus();
    DoubleMatrix G22 = A.inverse().conjugateTranspose();
    DoubleMatrix G = G11.appendRight(G12).appendDown(G21.appendRight(G22));
    DoubleEigenSolution tmp1 = G.eigenDecompose();
    DoubleComplexMatrix D = tmp1.getValue();
    DoubleComplexMatrix V = tmp1.getVector();

    D = D.diagonalToVector();

    // Sort on magnitude of eigenvalues
    IndexedMatrix<?,?> tmp2 = (D.absElementWise()).sort();
    DoubleMatrix Dr = (DoubleMatrix)tmp2.getMatrix();
    IntMatrix idx = tmp2.getIndices().transpose();

    if (!(Dr.getElement(n).isLessThan(1) && Dr.getElement(n + 1).isGreaterThan(1))) {
      throw new RuntimeException(Messages.getString("Dlqr.5")); //$NON-NLS-1$
    }

    // Select eigenvectors corresponding to the eigenvalues inside unit circle
    DoubleMatrix P = (V.getSubMatrix(n + 1, 2 * n, idx.getSubVector(1, n)).divide(V.getSubMatrix(1, n, idx.getSubVector(1, n)))).getRealPart();
    DoubleMatrix F = R.add(B.conjugateTranspose().multiply(P).multiply(B)).leftDivide(B.conjugateTranspose().multiply(P).multiply(A).add(S.conjugateTranspose()));
    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {F, P}));
    //return new MatxList(new Object[] {F, P});
  }

}