Lyap.java

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

import java.util.List;

import org.mklab.nfc.eig.SchurDecomposition;
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.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Rsf2csf;


/**
 * 連続時間系のリヤプノフ方程式の解を求めるクラスです。
 * 
 * <p>Solution of continuous-time Lyapunov equation
 * 
 * @author koga
 * @version $Revision: 1.35 $
 * @see org.mklab.tool.control.Dlyap
 */
public class Lyap {

  /**
   * 連続時間系のリヤプノフ方程式
   * 
   * <pre><code> A*P + P*A# = -Q </code></pre>
   * 
   * の解<code>P</code>を返します。
   * 
   * @param A 連続時間系のシステム行列
   * @param Q 正定対称行列
   * @return 解 (solution)
   */
  public static DoubleMatrix lyap(final DoubleMatrix A, final DoubleMatrix Q) {
    String message;
    if ((message = Abcdchk.abcdchk(A)).length() > 0) {
      throw new RuntimeException(message);
    }

    final DoubleMatrix B = A.conjugateTranspose();

    return lyap(A, B, Q);
  }

  /**
   * 連続時間系のリヤプノフ方程式
   * 
   * <pre><code> A*P + P*A# = -Q </code></pre>
   * 
   * の解<code>P</code>を返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A 連続時間系のシステム行列
   * @param Q 正定対称行列
   * @return 解 (solution)
   */
  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 lyap(final RM A, final RM Q) {
    String message;
    if ((message = Abcdchk.abcdchk(A)).length() > 0) {
      throw new RuntimeException(message);
    }

    final RM B = A.conjugateTranspose();

    return lyap(A, B, Q);
  }

  /**
   * 行列方程式
   * 
   * <pre><code> A*P + P*B = -Q </code></pre>
   * 
   * の解<code>P</code>を返します。
   * 
   * @param A 方程式の係数行列
   * @param B 方程式の係数行列
   * @param Q 方程式の係数行列
   * @return 解 (solution)
   */
  public static DoubleMatrix lyap(final DoubleMatrix A, final DoubleMatrix B, final DoubleMatrix Q) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new RuntimeException(message);
    }

    int n = A.getRowSize();
    int nb = B.getRowSize();

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

    SchurDecomposition<DoubleNumber, DoubleMatrix> UaTa = A.schurDecompose();
    DoubleMatrix UaRe = UaTa.getU();
    DoubleMatrix TaRe = UaTa.getT();

    List<DoubleComplexMatrix> tmp1 = Rsf2csf.rsf2csf(UaRe, TaRe);
    DoubleComplexMatrix Ua = tmp1.get(0);
    DoubleComplexMatrix Ta = tmp1.get(1);

    SchurDecomposition<DoubleNumber, DoubleMatrix> UbTb = B.schurDecompose();
    DoubleMatrix UbRe = UbTb.getU();
    DoubleMatrix TbRe = UbTb.getT();

    List<DoubleComplexMatrix> tmp2 = Rsf2csf.rsf2csf(UbRe, TbRe);
    DoubleComplexMatrix Ub = tmp2.get(0);
    DoubleComplexMatrix Tb = tmp2.get(1);

    // Check all combinations of diagonal elements of Ta and Tb
    DoubleComplexMatrix Pa = Ta.createOnes(nb, 1).multiply(Ta.diagonalToVector().conjugateTranspose());
    DoubleComplexMatrix Pb = Tb.diagonalToVector().multiply(Tb.createOnes(1, n));
    DoubleComplexMatrix PP = Pa.absElementWise().add(Pb.absElementWise());

    if (PP.compareElementWise(".==", 0).anyTrue() //$NON-NLS-1$
        || Pa.add(Pb).absElementWise().subtract(PP.multiply(PP.getElement(1, 1).getMachineEpsilon().multiply(1000))).compareElementWise(".<", 0).anyTrue()) { //$NON-NLS-1$
      System.err.println(Messages.getString("Lyap.3")); //$NON-NLS-1$
    }

    DoubleComplexMatrix Q2 = Ua.conjugateTranspose().unaryMinus().multiply(new DoubleComplexMatrix(Q)).multiply(Ub);

    // 1st column
    DoubleComplexMatrix firstY = Ta.add(new DoubleComplexMatrix(A.createUnit(n)).multiply(Tb.getElement(1, 1))).leftDivide(Q2.getColumnVector(1));
    DoubleComplexMatrix Y = firstY.createZero(n, nb);

    Y.setColumnVector(1, firstY);

    // 2nd column -- Rows(B)'th column
    for (int i = 2; i <= nb; i++) {
      IntMatrix index = IntMatrix.series(1, i - 1);

      DoubleComplexMatrix tmp3 = Ta.add(new DoubleComplexMatrix(A.createUnit(n)).multiply(Tb.getElement(i, i)));
      DoubleComplexMatrix tmp4 = Q2.getColumnVector(i).subtract(Y.getColumnVectors(index).multiply(Tb.getSubMatrix(index, i)));
      Y.setColumnVector(i, tmp3.leftDivide(tmp4));
    }

    DoubleComplexMatrix P = Ua.multiply(Y).multiply(Ub.conjugateTranspose());

    if (A.isReal() && B.isReal() && Q.isReal()) {
      return P.getRealPart();
    }

    return P.getRealPart();
  }

  /**
   * 行列方程式
   * 
   * <pre><code> A*P + P*B = -Q </code></pre>
   * 
   * の解<code>P</code>を返します。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A 方程式の係数行列
   * @param B 方程式の係数行列
   * @param Q 方程式の係数行列
   * @return 解 (solution)
   */
  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 lyap(final RM A, final RM B, final RM Q) {
    String message;
    if ((message = Abcdchk.abcdchk(A, B)).length() > 0) {
      throw new RuntimeException(message);
    }

    int n = A.getRowSize();
    int nb = B.getRowSize();

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

    SchurDecomposition<RS, RM> UaTa = A.schurDecompose();
    RM UaRe = UaTa.getU();
    RM TaRe = UaTa.getT();

    List<CM> tmp1 = Rsf2csf.rsf2csf(UaRe, TaRe);
    CM Ua = tmp1.get(0);
    CM Ta = tmp1.get(1);

    SchurDecomposition<RS, RM> UbTb = B.schurDecompose();
    RM UbRe = UbTb.getU();
    RM TbRe = UbTb.getT();

    List<CM> tmp2 = Rsf2csf.rsf2csf(UbRe, TbRe);
    CM Ub = tmp2.get(0);
    CM Tb = tmp2.get(1);

    // Check all combinations of diagonal elements of Ta and Tb
    CM Pa = Ta.createOnes(nb, 1).multiply(Ta.diagonalToVector().conjugateTranspose());
    CM Pb = Tb.diagonalToVector().multiply(Tb.createOnes(1, n));
    CM PP = Pa.absElementWise().add(Pb.absElementWise());

    if (PP.compareElementWise(".==", 0).anyTrue() //$NON-NLS-1$
        || Pa.add(Pb).absElementWise().subtract(PP.multiply(PP.getElement(1, 1).getMachineEpsilon().multiply(1000))).compareElementWise(".<", 0).anyTrue()) { //$NON-NLS-1$
      System.err.println(Messages.getString("Lyap.3")); //$NON-NLS-1$
    }

    CM Q2 = Ua.conjugateTranspose().unaryMinus().multiply(Q.toComplex()).multiply(Ub);

    // 1st column
    CM firstY = Ta.add(A.createUnit(n).toComplex().multiply(Tb.getElement(1, 1))).leftDivide(Q2.getColumnVector(1));
    CM Y = firstY.createZero(n, nb);

    Y.setColumnVector(1, firstY);

    // 2nd column -- Rows(B)'th column
    for (int i = 2; i <= nb; i++) {
      IntMatrix index = IntMatrix.series(1, i - 1);

      CM tmp3 = Ta.add(A.createUnit(n).toComplex().multiply(Tb.getElement(i, i)));
      CM tmp4 = Q2.getColumnVector(i).subtract(Y.getColumnVectors(index).multiply(Tb.getSubMatrix(index, i)));
      Y.setColumnVector(i, tmp3.leftDivide(tmp4));
    }

    CM P = Ua.multiply(Y).multiply(Ub.conjugateTranspose());

    if (A.isReal() && B.isReal() && Q.isReal()) {
      return P.getRealPart();
    }

    return P.getRealPart();
  }

}