Pplace.java

/*
 * $Id: Pplace.java,v 1.51 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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.IntMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Makepoly;


/**
 * 極配置実現する状態フィードバックゲインを求めるクラスです。
 * 
 * <p> Pole placement
 * 
 * @author koga
 * @version $Revision: 1.51 $
 */
public class Pplace {

  /**
   * システム行列 <code>A,B</code> で表されるシステムを、状態フィードバックしたとき 固有値をPにするようにフィードバックゲイン <code>F</code> を求めます。
   * 
   * <p>この <code>F</code> を用いて状態フィードバック則
   * 
   * <pre><code> u = -Fx </code></pre>
   * 
   * をシステムに適用したとき,システムは
   * 
   * <pre><code>
   * 
   * x' = (A - BF)x
   * 
   * </code></pre>
   * 
   * となり新システムの固有値は、 <code>P</code> と一致します。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @return 状態フィードバックゲイン
   */
  public static DoubleMatrix pplace(DoubleMatrix A, DoubleMatrix B, DoubleComplexMatrix poles) {
    final boolean batch = true;
    return pplace(A, B, poles, batch);
  }

  /**
   * システム行列 <code>A,B</code> で表されるシステムを、状態フィードバックしたとき 固有値をPにするようにフィードバックゲイン <code>F</code> を求めます。
   * 
   * <p>この <code>F</code> を用いて状態フィードバック則
   * 
   * <pre><code> u = -Fx </code></pre>
   * 
   * をシステムに適用したとき,システムは
   * 
   * <pre><code>
   * 
   * x' = (A - BF)x
   * 
   * </code></pre>
   * 
   * となり新システムの固有値は、 <code>P</code> と一致します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @return 状態フィードバックゲイン
   */
  public static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM pplace(
      RM A, RM B, CM poles) {
    final boolean batch = true;
    return pplace(A, B, poles, batch);
  }

  /**
   * システム
   * 
   * <pre><code>
   * 
   * . x = Ax + bu
   * 
   * </code></pre>
   * 
   * に状態フィードバック
   * 
   * <pre><code> u = -Fx </code></pre>
   * 
   * を施して、閉ループ系の極が <code>P</code> となる (すなわち <code>p = eigval(A-b*f)</code> )ような フィードバックゲイン行列 <code>F</code> を返します。
   * 
   * <p>閉ループ系の極が設計仕様として与えられた <code>P</code> から10%以上 離れている場合、警告メッセージが表示されます。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @param batch バッチ処理をするならばtrue、そうでなければfalse
   * @return 状態フィードバックゲイン
   */
  private static DoubleMatrix pplace(DoubleMatrix A, DoubleMatrix B, DoubleComplexMatrix poles, boolean batch) {
    final DoubleMatrix F;
    if (B.getColumnSize() == 1) {
      F = pplaceSISO(A, B, poles);
    } else {
      F = pplaceMIMO(A, B, poles, batch);
    }

    // Check results
    DoubleComplexMatrix Pc = A.subtract(B.multiply(F)).eigenValue().sort().getMatrix();
    DoubleComplexMatrix Po = poles.sort().getMatrix();
    final IntMatrix index = Po.compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    Po = Po.getSubVector(index);
    Pc = Pc.getSubVector(index);

    final DoubleComplexMatrix error = Po.subtract(Pc).absElementWise();
    final DoubleComplexMatrix relativeError = error.divideElementWise(Po.absElementWise());

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

    if (relativeError.max().isGreaterThan(unit.divide(10))) {
      System.err.println("Pplace: " + Messages.getString("Pplace.0")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    return F;
  }

  /**
   * システム
   * 
   * <pre><code>
   * 
   * . x = Ax + bu
   * 
   * </code></pre>
   * 
   * に状態フィードバック
   * 
   * <pre><code> u = -Fx </code></pre>
   * 
   * を施して、閉ループ系の極が <code>P</code> となる (すなわち <code>p = eigval(A-b*f)</code> )ような フィードバックゲイン行列 <code>F</code> を返します。
   * 
   * <p>閉ループ系の極が設計仕様として与えられた <code>P</code> から10%以上 離れている場合、警告メッセージが表示されます。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @param batch バッチ処理をするならばtrue、そうでなければfalse
   * @return 状態フィードバックゲイン
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM pplace(
      RM A, RM B, CM poles, boolean batch) {
    final RM F;
    if (B.getColumnSize() == 1) {
      F = pplaceSISO(A, B, poles);
    } else {
      F = pplaceMIMO(A, B, poles, batch);
    }

    // Check results
    CM Pc = A.subtract(B.multiply(F)).eigenValue().sort().getMatrix();
    CM Po = poles.sort().getMatrix();
    final IntMatrix index = Po.compareElementWise(".!=", 0).find(); //$NON-NLS-1$
    Po = Po.getSubVector(index);
    Pc = Pc.getSubVector(index);

    final RM error = Po.subtract(Pc).absElementWise().getRealPart();
    final RM relativeError = error.divideElementWise(Po.absElementWise().getRealPart());

    final RS unit = A.getElement(1, 1).createUnit();

    if (relativeError.max().isGreaterThan(unit.divide(10))) {
      System.err.println("Pplace: " + Messages.getString("Pplace.0")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    return F;
  }

  /**
   * SISOシステムの極配置を実現する状態フィードバックゲインを返します。
   * 
   * @param A システム行列
   * @param b 入力行列
   * @param poles 指定極
   * @return 状態フィードバックゲイン
   */
  private static DoubleMatrix pplaceSISO(DoubleMatrix A, DoubleMatrix b, DoubleComplexMatrix poles) {
    final String message = Abcdchk.abcdchk(A, b);
    if (message.length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = A.getRowSize();

    if (stateSize != poles.length()) {
      throw new IllegalArgumentException("Pplace: " + Messages.getString("Pplace.1")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    final DoubleMatrix p1 = Makepoly.makepoly(A).getCoefficients();
    final DoubleMatrix p2 = Makepoly.makepoly(poles).getCoefficients();

    final DoubleMatrix Vi = Ctrm.ctrm(A, b).inverse();
    final DoubleMatrix Ti = Obsm.obsm(A, Vi.getRowVector(stateSize));

    final DoubleMatrix F = p2.subtract(p1).getSubVector(1, stateSize).multiply(Ti);

    return F;
  }

  /**
   * SISOシステムの極配置を実現する状態フィードバックゲインを返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列
   * @param b 入力行列
   * @param poles 指定極
   * @return 状態フィードバックゲイン
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM pplaceSISO(
      RM A, RM b, CM poles) {
    final String message = Abcdchk.abcdchk(A, b);
    if (message.length() > 0) {
      throw new IllegalArgumentException(message);
    }

    final int stateSize = A.getRowSize();

    if (stateSize != poles.length()) {
      throw new IllegalArgumentException("Pplace: " + Messages.getString("Pplace.1")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    final RM p1 = Makepoly.makepoly(A).getCoefficients();
    final RM p2 = Makepoly.makepoly(poles).getCoefficients();

    final RM Vi = Ctrm.ctrm(A, b).inverse();
    final RM Ti = Obsm.obsm(A, Vi.getRowVector(stateSize));

    final RM F = p2.subtract(p1).getSubVector(1, stateSize).multiply(Ti);

    return F;
  }

  /**
   * MIMOシステムの極配置を実現する状態フィードバックゲインを返します。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @param batch バッチ処理をするならばtrue、そうでなければfalse
   * @return 状態フィードバックゲイン
   */
  private static DoubleMatrix pplaceMIMO(DoubleMatrix A, DoubleMatrix B, DoubleComplexMatrix poles, boolean batch) {
    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    final DoubleMatrix gzi;
    if (batch) {
      //gzi = A.createUnit(inputSize, stateSize);
      final long seed = 0x12345678L;
      gzi = A.createUniformRandom(inputSize, stateSize, seed);
    } else {
      gzi = A.createZero(inputSize, stateSize);
    }

    final DoubleComplexMatrix P;
    P = poles;

    final DoubleMatrix V = A.createZero();

    final DoubleNumber epsilon = A.getElement(1, 1).getMachineEpsilon();

    while (true) {
      for (int i = 1; i <= stateSize; i++) {
        final DoubleComplexNumber p = P.getElement(i);
        final DoubleNumber a = p.getRealPart();
        final DoubleNumber b = p.getImaginaryPart();

        if (b.abs().isLessThan(epsilon)) { // P(i) is a real pole
          V.setColumnVector(i, A.subtract(A.createUnit().multiply(a)).inverse().multiply(B).multiply(gzi.getColumnVector(i)));
        } else { // P(i) and P(i+1) are complex conjugate poles
          final DoubleMatrix tmp = A.subtract(A.createUnit().multiply(a)).power(2).add(A.createUnit().multiply(b.multiply(b))).inverse();
          final DoubleMatrix V1 = tmp.multiply(A.subtract(A.createUnit().multiply(a))).multiply(B);
          final DoubleMatrix V2 = tmp.multiply(B.multiply(b));
          V.setColumnVector(i, V1.multiply(gzi.getColumnVector(i)).subtract(V2.multiply(gzi.getColumnVector(i + 1))));
          V.setColumnVector(i + 1, V1.multiply(gzi.getColumnVector(i + 1)).add(V2.multiply(gzi.getColumnVector(i))));
          i++;
        }
      }

      // V is a singular matrix
      if (V.minSingularValue().isLessThan(epsilon)) {
        throw new RuntimeException("Pplace: " + Messages.getString("Pplace.2")); //$NON-NLS-1$ //$NON-NLS-2$
      }
      break;

      // read gzi;
    }

    final DoubleMatrix F = gzi.multiply(V.inverse());
    return F;
  }

  /**
   * MIMOシステムの極配置を実現する状態フィードバックゲインを返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列
   * @param B 入力行列
   * @param poles 指定極
   * @param batch バッチ処理をするならばtrue、そうでなければfalse
   * @return 状態フィードバックゲイン
   */
  private static <RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> RM pplaceMIMO(
      RM A, RM B, CM poles, boolean batch) {
    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();

    final RM gzi;
    if (batch) {
      //gzi = A.createUnit(inputSize, stateSize);
      final long seed = 0x12345678L;
      gzi = A.createUniformRandom(inputSize, stateSize, seed);
    } else {
      gzi = A.createZero(inputSize, stateSize);
    }

    final CM P;
    P = poles;

    final RM V = A.createZero();

    final RS epsilon = A.getElement(1, 1).getMachineEpsilon();

    while (true) {
      for (int i = 1; i <= stateSize; i++) {
        final CS p = P.getElement(i);
        final RS a = p.getRealPart();
        final RS b = p.getImaginaryPart();

        if (b.abs().isLessThan(epsilon)) { // P(i) is a real pole
          V.setColumnVector(i, A.subtract(A.createUnit().multiply(a)).inverse().multiply(B).multiply(gzi.getColumnVector(i)));
        } else { // P(i) and P(i+1) are complex conjugate poles
          final RM tmp = A.subtract(A.createUnit().multiply(a)).power(2).add(A.createUnit().multiply(b.multiply(b))).inverse();
          final RM V1 = tmp.multiply(A.subtract(A.createUnit().multiply(a))).multiply(B);
          final RM V2 = tmp.multiply(B.multiply(b));
          V.setColumnVector(i, V1.multiply(gzi.getColumnVector(i)).subtract(V2.multiply(gzi.getColumnVector(i + 1))));
          V.setColumnVector(i + 1, V1.multiply(gzi.getColumnVector(i + 1)).add(V2.multiply(gzi.getColumnVector(i))));
          i++;
        }
      }

      // V is a singular matrix
      if (V.minSingularValue().isLessThan(epsilon)) {
        throw new RuntimeException("Pplace: " + Messages.getString("Pplace.2")); //$NON-NLS-1$ //$NON-NLS-2$
      }
      break;

      // read gzi;
    }

    final RM F = gzi.multiply(V.inverse());
    return F;
  }

}