Feedback.java

/*
 * $Id: Feedback.java,v 1.15 2008/03/23 14:29:08 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;


/**
 * 2つの線形システムをネガティブフィードバック結合したシステムを求めるクラスです。
 * 
 * <p>Connect two systems using negative feedback
 * 
 * @author koga
 * @version $Revision: 1.15 $
 * @see org.mklab.tool.control.Series
 * @see org.mklab.tool.control.Parallel
 */
public class Feedback {

  /**
   * 2個のシステム<code>s1</code>と<code>s2</code>をネガティブフィードバック結合したシステムを生成します。
   * 
   * <p>2個のシステム
   * 
   * <pre><code> . x1 = A1*x1 + B1*u1 u + y y1 = C1*x1 + D1*u1 ----&gt;o--&gt;s1---+---&gt; . - &circ; | x2 = A2*x2 + B2*u2 | | y2 = C2*x2 + D2*u2 +---s2&lt;--+ </code></pre>
   * 
   * を考える。これらのシステムを<code>u1 = u - y2</code>の関係で結合すると、新しい システムは
   * 
   * <pre><code> x = A*x + B*u y = C*x + B*u </code></pre>
   * 
   * となります。ただし、出力<code>y</code>はシステム1の出力です。
   * 
   * @param A1 A1行列
   * @param B1 B1行列
   * @param C1 C1行列
   * @param D1 D1行列
   * @param A2 A2行列
   * @param B2 B2行列
   * @param C2 C2行列
   * @param D2 D2行列
   * @return フィードバック結合されたシステム (feedback-connected system)
   */
  public static List<DoubleMatrix> feedback(DoubleMatrix A1, DoubleMatrix B1, DoubleMatrix C1, DoubleMatrix D1, DoubleMatrix A2, DoubleMatrix B2, DoubleMatrix C2, DoubleMatrix D2) {
    String message1;
    if ((message1 = Abcdchk.abcdchk(A1, B1, C1, D1)).length() > 0) {
      throw new RuntimeException(message1);
    }
    String message2;
    if ((message2 = Abcdchk.abcdchk(A2, B2, C2, D2)).length() > 0) {
      throw new RuntimeException(message2);
    }

    int p1 = D1.getRowSize();
    int m1 = D1.getColumnSize();
    int p2 = D2.getRowSize();
    // int m2 = D2.getColSize();

    DoubleMatrix T = A1.createUnit(p1).add(D1.multiply(D2));
    // A = [[A1-B1*D2/T*C1 B1*(D2/T*D1-I(m1))*C2]
    // [ B2/T*C1 A2-B2/T*D1*C2 ]];
    DoubleMatrix tmp11 = A1.subtract(B1.multiply(D2.divide(T).multiply(C1)));
    DoubleMatrix tmp12 = B1.multiply(D2.divide(T).subtract(A1.createUnit(m1))).multiply(C2);
    DoubleMatrix tmp21 = B2.divide(T).multiply(C1);
    DoubleMatrix tmp22 = A2.subtract(B2.divide(T).multiply(D1).multiply(C2));
    DoubleMatrix A = tmp11.appendRight(tmp12).appendDown(tmp21.appendRight(tmp22));

    // B = [[B1*(I(p2)-D2/T*D1)]
    // [ B2/T*D1 ]];
    DoubleMatrix tmp1 = B1.multiply(A1.createUnit(p2).subtract(D2.divide(T).multiply(D1)));
    DoubleMatrix tmp2 = B2.divide(T).multiply(D1);
    DoubleMatrix B = tmp1.appendDown(tmp2);
    DoubleMatrix C = T.leftDivide(C1).appendRight(T.leftDivide(D1).multiply(C2).unaryMinus());
    // [T\C1, -T\D1*C2];
    DoubleMatrix D = T.leftDivide(D1);
    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
  }

}