Dhinf.java

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

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;

import org.mklab.nfc.eig.DoubleEigenSolution;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.ElementHolder;
import org.mklab.nfc.matrix.IndexedMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoubleNumberUtil;
import org.mklab.tool.matrix.Diag;
import org.mklab.tool.matrix.Matrix4;


/**
 * 離散時間系のH∞問題を解くクラスです。
 * 
 * <p>Solve H_infinity Problem (Discrete-time case)
 * 
 * @author koga
 * @version $Revision: 1.41 $
 * @see org.mklab.tool.control.Hinf
 * @see org.mklab.tool.control.Ric
 */
public class Dhinf {

  /**
   * 一般化プラント
   * 
   * <pre><code> n mw mu [[ x(k+1) ] n [[ A B1 B2 ] [[ x(k) ] [[ x ] [ z(k) ] = pz [ C1 D11 D12 ] [ w(k) ] = G [ w ] [ y(k) ]] py [ C2 D21 D22 ]] [ u(k) ]] [ u ]] </code></pre>
   * 
   * について、対応するリカッティ方程式を解き、
   * 
   * <pre><code> n py [[ q(k+1) ] = n [[ Ac Bc ] [[ q(k) ] = Gc [[ q ] [ u(k) ]] mu [ Cc 0 ]] [ y(k) ]] [ y ]] </code></pre>
   * 
   * で定義される、H∞制御器<code>Gc</code>を返します。
   * 
   * @param G 制御対象の一般化プラント
   * @param n 状態数
   * @param mw wの次数
   * @param pz zの次数
   * @param gamma gamma
   * @return H∞制御器 (H-infinity controller)
   * @throws IOException キーボードから読み込めない場合
   * @throws NumberFormatException 数値以外の値が入力された場合
   */
  public static DoubleMatrix dhinfAug(DoubleMatrix G, int n, int mw, int pz, double gamma) throws NumberFormatException, IOException {
    int RG = G.getRowSize();
    int CG = G.getColumnSize();
    int mu = CG - n - mw;
    int py = RG - n - pz;

    // DoubleMatrix Gc = RealMatrix.Z(n + mu, n + py);

    if (mu <= 0 || py <= 0) {
      throw new IllegalArgumentException(Messages.getString("Dhinf.0")); //$NON-NLS-1$
    }

    DoubleMatrix A = G.getSubMatrix(1, n, 1, n);
    DoubleMatrix B1 = G.getSubMatrix(1, n, n + 1, n + mw);
    DoubleMatrix B2 = G.getSubMatrix(1, n, n + mw + 1, CG);

    DoubleMatrix C1 = G.getSubMatrix(n + 1, n + pz, 1, n);
    DoubleMatrix D11 = G.getSubMatrix(n + 1, n + pz, n + 1, n + mw);
    DoubleMatrix D12 = G.getSubMatrix(n + 1, n + pz, n + mw + 1, CG);

    DoubleMatrix C2 = G.getSubMatrix(n + pz + 1, RG, 1, n);
    DoubleMatrix D21 = G.getSubMatrix(n + pz + 1, RG, n + 1, n + mw);
    DoubleMatrix D22 = G.getSubMatrix(n + pz + 1, RG, n + mw + 1, CG);

    double eps1 = DoubleNumberUtil.EPS;
    double eps2 = DoubleNumberUtil.EPS;

    BufferedReader br = new BufferedReader(new InputStreamReader(System.in));
    System.out.print("esp1 = " + eps1); //$NON-NLS-1$
    eps1 = Double.parseDouble(br.readLine());
    System.out.print("esp2 = " + eps2); //$NON-NLS-1$
    eps2 = Double.parseDouble(br.readLine());

    B1 = B1.appendRight(A.createUnit(n).multiply(eps2)).appendRight(A.createZero(n, py));
    C1 = C1.appendDown(A.createUnit(n).multiply(eps1)).appendDown(A.createZero(mu, n));
    D11 = Diag.diag(D11, A.createZero(n + mu, n + py));
    D12 = D12.appendDown(A.createZero(n, mu)).appendDown(A.createUnit(mu).multiply(eps1));
    D21 = D21.appendRight(A.createZero(py, n)).appendRight(A.createUnit(py).multiply(eps2));

    DoubleMatrix D = Matrix4.matrix4(D11, D12, D21, D22);
    DoubleMatrix C = C1.appendDown(C2);
    DoubleMatrix B = B1.appendRight(B2);
    DoubleMatrix newG = Matrix4.matrix4(A, B, C, D);
    int newNW = n + mw + py;
    int newPZ = n + pz + mu;

    return dhinf(newG, n, newNW, newPZ, gamma);
  }

  /**
   * @param G 制御対象の一般化プラント
   * @param n 状態数
   * @param mw wの次数
   * @param pz zの次数
   * @param gamma gamma
   * @return H∞制御器 (H-infinity controller)
   */
  public static DoubleMatrix dhinf(DoubleMatrix G, int n, int mw, int pz, double gamma) {
    int RG = G.getRowSize();
    int CG = G.getColumnSize();
    int mu = CG - n - mw;
    int py = RG - n - pz;

    // DoubleMatrix Gc = new RealMatrix(n + mu, n + py);

    if (mu <= 0 || py <= 0) {
      throw new IllegalArgumentException(Messages.getString("Dhinf.1")); //$NON-NLS-1$
    }

    DoubleMatrix A = G.getSubMatrix(1, n, 1, n);
    DoubleMatrix B1 = G.getSubMatrix(1, n, n + 1, n + mw);
    DoubleMatrix B2 = G.getSubMatrix(1, n, n + mw + 1, CG);

    DoubleMatrix C1 = G.getSubMatrix(n + 1, n + pz, 1, n);
    DoubleMatrix D11 = G.getSubMatrix(n + 1, n + pz, n + 1, n + mw);
    DoubleMatrix D12 = G.getSubMatrix(n + 1, n + pz, n + mw + 1, CG);

    DoubleMatrix C2 = G.getSubMatrix(n + pz + 1, RG, 1, n);
    DoubleMatrix D21 = G.getSubMatrix(n + pz + 1, RG, n + 1, n + mw);
    DoubleMatrix D22 = G.getSubMatrix(n + pz + 1, RG, n + mw + 1, CG);

    B1 = B1.divide(gamma);
    D11 = D11.divide(gamma);
    D21 = D21.divide(gamma);

    DoubleNumber tmp = D11.maxSingularValue();
    if (tmp.isGreaterThanOrEquals(1)) {
      throw new IllegalArgumentException(Messages.getString("Dhinf.2") + tmp.multiply(gamma)); //$NON-NLS-1$
    }

    if (D12.minSingularValue().isLessThanOrEquals(0)) {
      throw new IllegalArgumentException(Messages.getString("Dhinf.3")); //$NON-NLS-1$
    }

    if (D21.transpose().minSingularValue().isLessThanOrEquals(0)) {
      throw new IllegalArgumentException(Messages.getString("Dhinf.4")); //$NON-NLS-1$
    }

    DoubleMatrix IDTD = A.createUnit(mw).subtract(D11.transpose().multiply(D11)).inverse();
    DoubleMatrix IDDT = A.createUnit(pz).subtract(D11.multiply(D11.transpose())).inverse();
    DoubleMatrix DTD = D12.transpose().multiply(IDDT).multiply(D12).inverse();

    DoubleMatrix TMP = B2.add(B1.multiply(IDTD).multiply(D11.transpose()).multiply(D12));
    DoubleMatrix AR = A.add(B1.multiply(IDTD).multiply(D11.transpose()).multiply(C1)).subtract(TMP.multiply(DTD).multiply(D12.transpose()).multiply(IDDT).multiply(C1));
    DoubleMatrix QR = C1.transpose().multiply(IDDT).multiply(C1).subtract(C1.transpose().multiply(IDDT).multiply(D12).multiply(DTD).multiply(D12.transpose()).multiply(IDDT).multiply(C1));
    DoubleMatrix RR = TMP.multiply(DTD).multiply(TMP.transpose()).subtract(B1.multiply(IDTD).multiply(B1.transpose()));

    DoubleMatrix HR = Matrix4.matrix4(A.createUnit(n), RR.unaryMinus(), A.createZero(n, n), AR.transpose());
    DoubleMatrix HL = Matrix4.matrix4(AR, A.createZero(n, n), QR.unaryMinus(), A.createUnit(n));

    DoubleEigenSolution tp = HL.eigenDecompose(HR);
    DoubleComplexMatrix S =  tp.getValue();
    DoubleComplexMatrix T = tp.getVector();

    IndexedMatrix<DoubleComplexNumber,DoubleComplexMatrix> tp2 = S.diagonalToVector().sort();
    S = tp2.getMatrix();
    IntMatrix idx = tp2.getIndices();

    idx = idx.flipLeftRight();
    S = S.flipUpDown();
    T = T.getSubMatrix(1, T.getRowSize(), idx);

    DoubleComplexMatrix U = T.getSubMatrix(1, 2, A);
    DoubleComplexMatrix V = T.getSubMatrix(2, 2, A);

    DoubleMatrix X = V.multiply(U.inverse()).getRealPart();

    DoubleComplexMatrix pc1 = X.eigenValue();
    ElementHolder<DoubleNumber> temp1 = pc1.getRealPart().minimum();
    DoubleNumber tmp1 = temp1.getElement();
    // int i = temp.getInteger(1);
    if (tmp1.isLessThan(0)) {
      System.err.println(Messages.getString("Dhinf.5")); //$NON-NLS-1$
      System.err.println(Messages.getString("Dhinf.6") + pc1); //$NON-NLS-1$
    }

    DoubleMatrix B = B1.appendRight(B2);
    DoubleMatrix D = D11.appendRight(D12);
    TMP = Diag.diag(A.createUnit(mw), A.createZero(mu, mu));

    DoubleMatrix F = A.createZero(mu, mw).appendRight(A.createUnit(mu)).unaryMinus().multiply(D.transpose().multiply(D.subtract(TMP)).inverse())
        .multiply(B.transpose().multiply(X).multiply(A).add(D.transpose().multiply(C1)));

    DoubleComplexMatrix pc2 = A.add(B2.multiply(F)).eigenValue();
    ElementHolder<DoubleNumber> temp2 = pc2.absElementWise().getRealPart().maximum();
    DoubleNumber tmp2 = temp2.getElement();
    // i = temp.getInteger(1);
    if (tmp2.isGreaterThanOrEquals(1)) {
      System.err.println(Messages.getString("Dhinf.7")); //$NON-NLS-1$
      System.err.println(Messages.getString("Dhinf.8") + pc2); //$NON-NLS-1$
    }

    TMP = A.createUnit(mw).subtract(D11.transpose().multiply(D11)).subtract(B1.transpose().multiply(X).multiply(B1)).inverse();
    DoubleMatrix B3 = B1.transpose().multiply(X).multiply(B2).add(D11.transpose().multiply(D12));
    DoubleMatrix Ab = A.add(B1.multiply(TMP).multiply((D11.transpose()).multiply(C1)).add(B1.transpose().multiply(X).multiply(A)));
    DoubleMatrix B2b = B2.add(B1.multiply(TMP).multiply(B3));
    DoubleMatrix C2b = C2.add(D21.multiply(TMP).multiply((D11.transpose()).multiply(C1)).add(B1.transpose().multiply(X).multiply(A)));
    DoubleMatrix D22b = D22.add(D21.multiply(TMP).multiply(B3));
    DoubleMatrix TMP2 = D12.transpose().multiply(D12).add(B2.transpose().multiply(X).multiply(B2)).add(B3.transpose().multiply(TMP).multiply(B3));

    DTD = D21.multiply(TMP).multiply(D21.transpose()).inverse();
    AR = Ab.subtract(B1.multiply(TMP).multiply(D21.transpose()).multiply(DTD).multiply(C2b)).transpose();
    QR = B1.multiply(TMP).multiply(B1.transpose()).subtract(B1.multiply(TMP).multiply(D21.transpose()).multiply(DTD).multiply(D21).multiply(TMP).multiply(B1.transpose()));
    RR = C2b.transpose().multiply(DTD).multiply(C2b).subtract(F.transpose().multiply(TMP2).multiply(F));

    HR = Matrix4.matrix4(A.createUnit(n), RR.unaryMinus(), A.createZero(n, n), AR.transpose());
    HL = Matrix4.matrix4(AR, A.createZero(n, n), QR.unaryMinus(), A.createUnit(n));

    tp = HL.eigenDecompose(HR);
    S =  tp.getValue();
    T = tp.getVector();

    tp2 = S.diagonalToVector().sort();
    S = tp2.getMatrix();
    idx = tp2.getIndices();
    idx = idx.flipLeftRight();
    S = S.flipUpDown();
    T = T.getSubMatrix(1, T.getRowSize(), idx);

    U = T.getSubMatrix(1, 2, A);
    V = T.getSubMatrix(2, 2, A);

    DoubleMatrix Y = V.multiply(U.inverse()).getRealPart();

    DoubleComplexMatrix pc3 = Y.eigenValue();
    ElementHolder<DoubleNumber> temp3 = pc3.getRealPart().minimum();
    DoubleNumber tmp3 = temp3.getElement();
    // i = temp.getInteger(1);
    if (tmp3.isLessThan(0)) {
      System.err.println(Messages.getString("Dhinf.9")); //$NON-NLS-1$
      System.err.println(Messages.getString("Dhinf.10") + pc3); //$NON-NLS-1$
    }

    TMP = A.createUnit(n).subtract(Y.multiply(F.transpose()).multiply(TMP2).multiply(F)).inverse().multiply(Y).multiply(C2b.transpose());
    TMP2 = (A.createUnit(mw).subtract(D11.transpose().multiply(D11)).subtract(B1.transpose().multiply(X).multiply(B1))).inverse();
    DoubleMatrix L = (B1.multiply(TMP2).multiply(D21.transpose()).add(A.multiply(TMP))).multiply(D21.multiply(TMP2).multiply(D21.transpose()).add(C2b.multiply(TMP))).inverse().unaryMinus();

    DoubleComplexMatrix pc4 = A.add(L.multiply(C2b)).eigenValue();
    ElementHolder<DoubleNumber> temp4 = pc4.absElementWise().getRealPart().maximum();
    DoubleNumber tmp4 = temp4.getElement();
    // i = temp.getInteger(1);
    if (tmp4.isGreaterThanOrEquals(1)) {
      System.err.println(Messages.getString("Dhinf.11")); //$NON-NLS-1$
      System.err.println(Messages.getString("Dhinf.12") + pc4); //$NON-NLS-1$
    }

    DoubleMatrix Ac = Ab.add(B2b.multiply(F)).add(L.multiply(C2b)).add(L.multiply(D22b).multiply(F));
    DoubleMatrix Bc = L.unaryMinus();
    DoubleMatrix Cc = F;

    DoubleMatrix Gc = Matrix4.matrix4(Ac, Bc, Cc, A.createZero(Cc.getRowSize(), Bc.getColumnSize()));

    return Gc;
  }

}