Feedbk.java

/*
 * $Id: Feedbk.java,v 1.13 2008/03/23 14:29:07 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.matrix.DoubleMatrix;


/**
 * さまざまなネガティブフィードバック結合を実現するクラスです。
 * 
 * <p>Common negative feedback
 * 
 * @author koga
 * @version $Revision: 1.13 $
 * @see org.mklab.tool.control.Feedback
 */
public class Feedbk {

  /**
   * ネガティブフィードバック結合したシステムを生成します。
   * 
   * <p>結合の方法を表す<code>type</code>は、次の値から選ぶ。
   * 
   * <pre><code> 1: 前向き = I, 後ろ向き = (A1,B1,C1,D1) 2: 前向き = (A1,B1,C1,D1), 後ろ向き = I 3: 前向き = (A1,B1,C1,D1), 後ろ向き = (A2,B2,C2,D2) 4: 前向き = (A1,B1,C1,D1), 後ろ向き = 状態フィードバック 5: 前向き = (A1,B1,C1,D1), 後ろ向き =
   * 出力フィードバック </code></pre>
   * 
   * @param A1 A1行列
   * @param B1 B1行列
   * @param C1 C1行列
   * @param D1 D1行列
   * @param type タイプ
   * @return フィードバック結合したシステム (feedback-connected system)
   */
  public static List<DoubleMatrix> feedbk(DoubleMatrix A1, DoubleMatrix B1, DoubleMatrix C1, DoubleMatrix D1, int type) {
    String message;
    if ((message = Abcdchk.abcdchk(A1, B1, C1, D1)).length() > 0) {
      throw new RuntimeException(message);
    }

    DoubleMatrix A = null, B = null, C = null, D = null;

    switch (type) {
      case 1:
        DoubleMatrix D1i = D1.createUnit().add(D1).inverse();
        A = A1.subtract(B1.multiply(D1i).multiply(C1));
        B = B1.multiply(D1i);
        C = D1i.multiply(C1).unaryMinus();
        D = D1i;
        break;
      case 2:
        D1i = D1.createUnit().add(D1).inverse();
        A = A1.subtract(B1.multiply(D1i).multiply(C1));
        B = B1.multiply(D1i);
        C = D1i.multiply(C1);
        D = D1i.multiply(D1);
        break;
      default:
        throw new IllegalArgumentException();
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
  }

  /**
   * @param A1 A1行列
   * @param B1 B1行列
   * @param C1 C1行列
   * @param D1 D1行列
   * @param type タイプ
   * @param FH フィードバック行列
   * @return feedback-connected system
   */
  public static List<DoubleMatrix> feedbk(DoubleMatrix A1, DoubleMatrix B1, DoubleMatrix C1, DoubleMatrix D1, int type, DoubleMatrix FH) {
    String message;
    if ((message = Abcdchk.abcdchk(A1, B1, C1, D1)).length() > 0) {
      throw new RuntimeException(message);
    }

    DoubleMatrix A = null, B = null, C = null, D = null;

    switch (type) {
      case 4:
        DoubleMatrix F = FH;
        A = A1.subtract(B1.multiply(F));
        B = B1;
        C = C1.subtract(D1.multiply(F));
        D = D1;
        break;
      case 5:
        DoubleMatrix H = FH;
        A = A1.subtract(H.multiply(C1));
        B = B1.subtract(H.multiply(D1));
        C = C1;
        D = D1;
        break;
      default:
        throw new IllegalArgumentException();
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
  }

  /**
   * @param A1 A1行列
   * @param B1 B1行列
   * @param C1 C1行列
   * @param D1 D1行列
   * @param type タイプ
   * @param A2 A2行列
   * @param B2 B2行列
   * @param C2 C2行列
   * @param D2 D2行列
   * @return feedback-connected system
   */
  public static List<DoubleMatrix> feedbk(DoubleMatrix A1, DoubleMatrix B1, DoubleMatrix C1, DoubleMatrix D1, int type, DoubleMatrix A2, DoubleMatrix B2, DoubleMatrix C2, DoubleMatrix D2) {
    String message;
    if ((message = Abcdchk.abcdchk(A1, B1, C1, D1)).length() > 0) {
      throw new RuntimeException(message);
    }

    DoubleMatrix A = null, B = null, C = null, D = null;

    switch (type) {
      case 3:
        if ((message = Abcdchk.abcdchk(A2, B2, C2, D2)).length() > 0) {
          throw new RuntimeException(message);
        }

        DoubleMatrix D21 = D2.multiply(D1);
        DoubleMatrix D12 = D1.multiply(D2);
        DoubleMatrix D21i = D21.createUnit().add(D21).inverse();
        DoubleMatrix D12i = D12.createUnit().add(D12).inverse();
        DoubleMatrix A11 = A1.subtract(B1.multiply(D21i).multiply(D2).multiply(C1));
        DoubleMatrix A12 = B1.multiply(D21i).multiply(C2).unaryMinus();
        DoubleMatrix A21 = B2.multiply(D12i).multiply(C1);
        DoubleMatrix A22 = A2.subtract(B2.multiply(D12i).multiply(D1).multiply(C2));
        A = A11.appendRight(A12).appendDown(A21.appendRight(A22));
        B = B1.multiply(D21i).appendDown(B2.multiply(D12i.multiply(D1)));
        C = D12i.multiply(C1).appendRight(D12i.multiply(D1).multiply(C2).unaryMinus());
        D = D12i.multiply(D1);
        /*
         * A = [[A1 - B1*D21i*D2*C1, -B1*D21i*C2 ] [ B2*D12i*C1 A2 -
         * B2*D12i*D1*C2]]; B = [[ B1*D21i ] [B2*D12i*D1]]; C = [D12i*C1,
         * -D12i*D1*C2]; D = D12i*D1;
         */
        break;
      default:
        throw new IllegalArgumentException();
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
  }

}