DoubleLqServoDesigner.java

/*
 * $Id: LqServoDesigner.java,v 1.7 2008/07/16 04:58:04 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system.controller;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.Lqr;
import org.mklab.tool.control.system.DoubleLinearSystemOperator;
import org.mklab.tool.control.system.DoubleSystemOperator;


/**
 * 入力外乱に対するLQ最適サーボのための状態フィードバックと積分ゲインを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.7 $, 2004/05/31
 */
public class DoubleLqServoDesigner {

  /** 制御対象(線形システム) */
  private DoubleLinearSystemOperator plant;
  /** 出力に関する重み行列 */
  private DoubleMatrix Qe;
  /** 入力に関する重み行列 */
  private DoubleMatrix Re;
  /** 入力外乱に対するLQ最適サーボのための状態フィードバックゲイン行列 */
  private DoubleMatrix F1;
  /** 入力外乱に対するLQ最適サーボのための積分ゲイン行列 */
  private DoubleMatrix F2;

  /**
   * コンストラクター
   * 
   * @param plant 制御対象(線形システム)
   */
  public DoubleLqServoDesigner(final DoubleSystemOperator plant) {
    this.plant = (DoubleLinearSystemOperator)plant;
  }

  /**
   * 入力外乱に対するLQ最適サーボのための状態フィードバック行列を返します。
   * 
   * @return 入力外乱に対するLQ最適サーボのための状態フィードバック行列
   */
  public DoubleMatrix getStateFeedback() {
    design();
    return this.F1;
  }

  /**
   * 入力外乱に対するLQ最適サーボのための積分ゲイン行列を返します。
   * 
   * @return 入力外乱に対するLQ最適サーボのための積分ゲイン行列
   */
  public DoubleMatrix getIntegratorGain() {
    design();
    return this.F2;
  }

  /**
   * 入力外乱に対するLQ最適サーボのための状態フィードバック行列を設計します。
   */
  private void design() {
    final DoubleMatrix A = this.plant.getA();
    final DoubleMatrix B = this.plant.getB();
    final DoubleMatrix C = this.plant.getC();

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();
    final int outputSize = C.getRowSize();

    final DoubleMatrix H = C.getRowVector(inputSize);
    final DoubleMatrix S = A.appendRight(B).appendDown(H.appendRight(new DoubleMatrix(inputSize, inputSize)));

    final DoubleMatrix Ae = A.appendDown(H).appendRight(new DoubleMatrix(stateSize + inputSize, inputSize));
    final DoubleMatrix Be = B.appendDown(new DoubleMatrix(inputSize, 1));
    final DoubleMatrix Ce = C.appendRight(new DoubleMatrix(outputSize, inputSize)).multiply(S.inverse());

    final DoubleMatrix Q = Ce.transpose().multiply(this.Qe).multiply(Ce);

    final DoubleMatrix Fe = Lqr.lqr(Ae, Be, Q, this.Re).get(0);
    this.F1 = Fe.getColumnVectors(1, stateSize);
    this.F2 = Fe.getColumnVectors(stateSize + 1, stateSize + inputSize);
  }

  /**
   * 重み行列QeとReを設定します。
   * 
   * @param Qe 出力に関する重み行列
   * @param Re 入力に関する重み行列
   */
  public void setWeightingMatrices(DoubleMatrix Qe, DoubleMatrix Re) {
    this.Qe = Qe;
    this.Re = Re;
  }

  /**
   * 閉ループ系の極を表示します。
   */
  public void showClosedLoopPoles() {
    design();
    DoubleMatrix A = this.plant.getA();
    DoubleMatrix B = this.plant.getB();
    DoubleComplexMatrix closedLoopPoles = A.subtract(B.multiply(this.F1)).eigenValue();
    closedLoopPoles.print(Messages.getString("LqServoDesigner.0")); //$NON-NLS-1$
  }

  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   */
  //  public static void main(String[] args) {
  //    //designPendulum();
  //    designDoublePendulum();
  //  }
  //
  //  /**
  //   * (1重)倒立振子の設計を行います。
  //   */
  //  static void designPendulum() {
  //    final LqServoDesigner designer = new LqServoDesigner(new LinearSinglePendulum());
  //    
  //    final DoubleMatrix Qe = new DoubleMatrix(new double[] {100*100, 1}).vectorToDiagonal();
  //    final DoubleMatrix Re = new DoubleMatrix(new double[] {1});
  //    designer.setWeightingMatrix(Qe, Re);
  //    
  //    System.out.println("入力外乱に対するLQ最適サーボのための状態フィードバック行列");
  //    designer.getStateFeedback().print("F1");
  //    System.out.println("");
  //    System.out.println("入力外乱に対するLQ最適サーボのための積分ゲイン行列");
  //    designer.getIntegratorGain().print("F2");
  //    System.out.println("");
  //    designer.showClosedLoopPoles();
  //  }
  //  
  //  /**
  //   * 2重倒立振子の設計を行います。
  //   */
  //  static void designDoublePendulum() {
  //    final LinearDoublePendulum system = new LinearDoublePendulum();
  //    final LqServoDesigner designer = new LqServoDesigner(system);
  //
  //    final DoubleMatrix Qe = new DoubleMatrix(new double[] {1, 10*10, 140*140}).vectorToDiagonal();
  //    final DoubleMatrix Re = new DoubleMatrix(new double[] {1});
  //    designer.setWeightingMatrix(Qe, Re);
  //
  //    System.out.println("入力外乱に対するLQ最適サーボのための状態フィードバック行列");
  //    designer.getStateFeedback().print("F1");
  //    System.out.println("");
  //    System.out.println("入力外乱に対するLQ最適サーボのための積分ゲイン行列");
  //    designer.getIntegratorGain().print("F2");
  //    System.out.println("");
  //    designer.showClosedLoopPoles();
  //  }
}