Hinf.java

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

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.ElementHolder;
import org.mklab.nfc.scalar.DoubleNumber;


/**
 * 連続系のH∞問題を解くクラスです。
 * 
 * <p>Solve H-infinity problem
 * 
 * @author koga
 * @version $Revision: 1.34 $
 * @see org.mklab.tool.control.Dhinf
 */
public class Hinf {

  /**
   * 一般化プラント
   * 
   * <pre><code> . n mw mu [[ x ] n [[ A B1 B2 ] [[ x ] [[ x ] [ z ] = pz [ C1 D11 D12 ] [ w ] = G [ w ] [ y ]] py [ C2 D21 D22 ]] [ u ]] [ u ]] </code></pre>
   * 
   * について、適当なリカッティ方程式を解き、H∞制御器
   * 
   * <pre><code> . n py [[ q ] = n [[ Ac Bc ] [[ q ] = Gc [[ q ] [ u ]] mu [ Cc 0 ]] [ y ]] [ y ]] </code></pre>
   * 
   * を返します。
   * 
   * @param G 制御対象の一般化プラント
   * @param n 状態数
   * @param mw wの時数
   * @param pz zの時数
   * @param gamma gamma
   * @return H∞制御器 (H-infinity controller)
   */
  public static DoubleMatrix hinf(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;

    if (mu <= 0 || py <= 0) {
      throw new RuntimeException(Messages.getString("Hinf.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);

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

    DoubleNumber tmp = D11.maxSingularValue();
    if (tmp.isGreaterThan(1)) {
      System.err.println(Messages.getString("Hinf.1") + tmp.multiply(gamma)); //$NON-NLS-1$
    }

    if (D12.minSingularValue().isLessThanOrEquals(0)) {
      throw new RuntimeException(Messages.getString("Hinf.2")); //$NON-NLS-1$
    }

    if (D21.transpose().minSingularValue().isLessThanOrEquals(0)) {
      throw new RuntimeException(Messages.getString("Hinf.3")); //$NON-NLS-1$
    }

    DoubleMatrix IDTD = D11.createUnit(D11.getColumnSize()).subtract(D11.transpose().multiply(D11)).inverse();
    DoubleMatrix IDDT = D11.createUnit(D11.getRowSize()).subtract(D11.multiply(D11.transpose())).inverse();
    DoubleMatrix DTD = D12.transpose().multiply(IDDT).multiply(D12).inverse();

    DoubleMatrix TMP1 = B2.add(B1.multiply(IDTD).multiply(D11.transpose()).multiply(D12));
    DoubleMatrix ARx = A.add(B1.multiply(IDTD).multiply(D11.transpose()).multiply(C1)).subtract(TMP1.multiply(DTD).multiply(D12.transpose()).multiply(IDDT).multiply(C1));
    DoubleMatrix QRx = C1.transpose().multiply(IDDT).multiply(C1).subtract(C1.transpose().multiply(IDDT).multiply(D12).multiply(DTD).multiply(D12.transpose()).multiply(IDDT).multiply(C1));
    DoubleMatrix RRx = TMP1.multiply(DTD).multiply(TMP1.transpose()).subtract(B1.multiply(IDTD).multiply(B1.transpose()));

    DoubleMatrix X = Ric.ric(ARx, QRx, RRx);

    DoubleComplexMatrix pc1 = X.eigenValue();
    ElementHolder<DoubleNumber> tmp1 = pc1.getRealPart().minimum();
    DoubleNumber tmp11 = tmp1.getElement();
    int row1 = tmp1.getRow();
    int column1 = tmp1.getColumn();
    if (tmp11.isLessThanOrEquals(0)) {
      System.err.println(Messages.getString("Hinf.4")); //$NON-NLS-1$
      System.err.println(Messages.getString("Hinf.5") + pc1.getElement(row1, column1)); //$NON-NLS-1$
    }

    DoubleMatrix TMP2 = IDTD.multiply(D11.transpose().multiply(C1).add(B1.transpose().multiply(X)));
    DoubleMatrix Ab = A.add(B1.multiply(TMP2));
    DoubleMatrix B2b = B2.add(B1.multiply(IDTD).multiply(D11.transpose()).multiply(D12));
    DoubleMatrix C2b = C2.add(D21.multiply(TMP2));
    DoubleMatrix D22b = D22.add(D21.multiply(IDTD).multiply(D11.transpose()).multiply(D12));
    DoubleMatrix DDT = D21.multiply(IDTD).multiply(D21.transpose()).inverse();

    DoubleMatrix TMP3 = D12.transpose().multiply(D11).multiply(IDTD).multiply(D11.transpose().multiply(C1).add(B1.transpose().multiply(X))).add(D12.transpose().multiply(C1)).add(B2.transpose().multiply(X));
    DoubleMatrix F = DTD.multiply(TMP3).unaryMinus();
    DoubleMatrix RRy = C2b.transpose().multiply(DDT).multiply(C2b).subtract(TMP3.transpose().multiply(DTD).multiply(TMP3));

    DoubleMatrix TMP4 = B1.multiply(IDTD).multiply(D21.transpose());
    DoubleMatrix ARy = Ab.subtract(TMP4.multiply(DDT).multiply(C2b)).transpose();
    DoubleMatrix QRy = B1.multiply(IDTD).multiply(B1.transpose()).subtract(TMP4.multiply(DDT).multiply(TMP4.transpose()));

    DoubleMatrix Y = Ric.ric(ARy, QRy, RRy);

    DoubleComplexMatrix pc2 = Y.eigenValue();
    ElementHolder<DoubleNumber> tmp2 = pc2.getRealPart().minimum();
    DoubleNumber tmp22 = tmp2.getElement();
    if (tmp22.isLessThanOrEquals(0)) {
      System.err.println(Messages.getString("Hinf.6")); //$NON-NLS-1$
      System.err.println(Messages.getString("Hinf.7")); //$NON-NLS-1$
      pc1.print("pc"); //$NON-NLS-1$
    }

    DoubleComplexMatrix pc3 = X.multiply(Y).eigenValue();
    ElementHolder<DoubleNumber> tmp3 = pc3.getRealPart().maximum();
    DoubleNumber tmp33 = tmp3.getElement();
    int row3 = tmp3.getRow();
    int column3 = tmp3.getColumn();
    if (tmp33.isGreaterThanOrEquals(gamma * gamma)) {
      System.err.println(Messages.getString("Hinf.8")); //$NON-NLS-1$
      System.err.print(Messages.getString("Hinf.9") + pc3.getElement(row3, column3) + Messages.getString("Hinf.10")); //$NON-NLS-1$ //$NON-NLS-2$
      System.err.println("gamma^2 = " + gamma * gamma); //$NON-NLS-1$
    }

    DoubleMatrix L = Y.multiply(C2b.transpose()).add(TMP1).multiply(DDT).unaryMinus();
    DoubleMatrix Ac = Ab.add(B2b.multiply(F)).add(L.multiply(C2b)).add(L.multiply(D22b).multiply(F));
    DoubleMatrix Bc = L.unaryMinus();
    DoubleMatrix Cc = F.createClone();

    DoubleMatrix Gc = Ac.appendRight(Bc).appendDown(Cc.appendRight(Cc.createZero(Cc.getRowSize(), Bc.getColumnSize())));

    return Gc;
  }

}