LqServoDesigner.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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.Lqr;
import org.mklab.tool.control.system.LinearSystemOperator;
import org.mklab.tool.control.system.SystemOperator;


/**
 * 入力外乱に対するLQ最適サーボのための状態フィードバックと積分ゲインを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.7 $, 2004/05/31
 * @param <RS> type of real scalar
 * @param <RM> type of real matrix
 * @param <CS> type of complex scalar
 * @param <CM> type of complex matrix
 */
public class LqServoDesigner<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>> {

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

  /**
   * コンストラクター
   * 
   * @param plant 制御対象(線形システム)
   */
  public LqServoDesigner(final SystemOperator<RS,RM,CS,CM> plant) {
    this.plant = (LinearSystemOperator<RS,RM,CS,CM>)plant;
  }

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

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

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

    final int stateSize = A.getRowSize();
    final int inputSize = B.getColumnSize();
    final int outputSize = C.getRowSize();
    
    final RS sunit = A.getElement(1,1).createUnit();

    final RM H = C.getRowVector(inputSize);
    final RM S = A.appendRight(B).appendDown(H.appendRight(sunit.createZeroGrid(inputSize, inputSize)));

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

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

    final RM 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(RM Qe, RM Re) {
    this.Qe = Qe;
    this.Re = Re;
  }

  /**
   * 閉ループ系の極を表示します。
   */
  public void showClosedLoopPoles() {
    design();
    RM A = this.plant.getA();
    RM B = this.plant.getB();
    CM 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 RM Qe = new DoubleMatrix(new double[] {100*100, 1}).vectorToDiagonal();
  //    final RM 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 RM Qe = new DoubleMatrix(new double[] {1, 10*10, 140*140}).vectorToDiagonal();
  //    final RM 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();
  //  }
}