PoleViewer.java

/*
 * $Id: PoleViewer.java,v 1.14 2008/03/30 00:30:47 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system.controller;

import java.io.IOException;

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.nfc.util.Pause;
import org.mklab.tool.control.system.LinearSystemOperator;
import org.mklab.tool.graph.gnuplot.Canvas;
import org.mklab.tool.graph.gnuplot.Gnuplot;


/**
 * 閉ループ系の極とオブザーバの極を複素平面にプロットするクラスです。
 * 
 * @author koga
 * @version $Revision: 1.14 $, 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 PoleViewer<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 RS RATIO_OF_VIEWREGION_MAXVALUE;

  /** 実部が負の領域と正の領域の比率 */
  private RS RATIO_OF_NEGATIVE_POSITIVE_REGION;

  /** 倒立振子の線形モデル */
  private LinearSystemOperator<RS,RM,CS,CM> linearModel;

  /** 描画範囲のxの最大値 */
  private RS xMax;

  /** 描画範囲のxの最小値 */
  private RS xMin;

  /** 描画範囲のyの最大値 */
  private RS yMax;

  /** 描画範囲のyの最小値 */
  private RS yMin;

  /**
   * コンストラクタ
   * 
   * @param linearModel 倒立振子の線形モデル
   * @param sunit unit of scalar
   */
  public PoleViewer(LinearSystemOperator<RS,RM,CS,CM> linearModel, RS sunit) {
    this.linearModel = linearModel;
    
    this.RATIO_OF_VIEWREGION_MAXVALUE = sunit.create(15).divide(10);
    this.RATIO_OF_NEGATIVE_POSITIVE_REGION = sunit.create(1).divide(4);
  }

  /**
   * 閉ループ系の極とオブザーバの極を複素平面にプロットします。
   * 
   * @param F 状態フィードバック行列
   * @param observer オブザーバ
   * @throws IOException キーボードから入力できない場合
   */
  public void plot(RM F, LinearSystemOperator<RS,RM,CS,CM> observer) throws IOException {
    RM A = this.linearModel.getA();
    RM B = this.linearModel.getB();
    RM Ah = observer.getA();
    CM closedLoopPoles = A.subtract(B.multiply(F)).eigenValue();
    CM observerPoles = Ah.eigenValue();

    setupRange(closedLoopPoles, observerPoles);
    plotPoles(closedLoopPoles, observerPoles);
  }

  /**
   * 閉ループ系の極とオブザーバの極をプロットします。
   * 
   * @param closedLoopPoles 閉ループ系の極
   * @param observerPoles オブザーバの極
   * @throws IOException キーボードから入力できない場合
   */
  @SuppressWarnings("nls")
  private void plotPoles(CM closedLoopPoles, CM observerPoles) throws IOException {
    Gnuplot gp = new Gnuplot();
    Canvas canvas = gp.createCanvas();
    canvas.doCommand("set data style points");
    canvas.setXLabel("Re");
    canvas.setYLabel("Im");
    canvas.setGridVisible(true);
    canvas.setTitle("Closed-loop poles and observer poles");
    canvas.doCommand("set xrange [" + this.RATIO_OF_VIEWREGION_MAXVALUE.multiply(this.xMin) + ":" + this.RATIO_OF_VIEWREGION_MAXVALUE .multiply(this.xMax) + "]");
    canvas.doCommand("set yrange [" + this.RATIO_OF_VIEWREGION_MAXVALUE.multiply(this.yMin) + ":" + this.RATIO_OF_VIEWREGION_MAXVALUE.multiply(this.yMax) + "]");

    //canvas.plot(closedLoopPoles.getRealPart().transpose(), closedLoopPoles.getImaginaryPart().transpose(), new String[] {"closed-loop poles"});

    canvas.setHolding(true);
    //canvas.plot(observerPoles.getRealPart().transpose(), observerPoles.getImaginaryPart().transpose(), new String[] {"observer poles"});
    canvas.setHolding(false);

    Pause.pause();
    gp.close();
  }

  /**
   * 閉ループ系の極とオブザーバーの極の複素平面上での位置を考慮して描画範囲を設定します。
   * 
   * @param closedLoopPoles 閉ループ系の極
   * @param observerPoles オブザーバの極
   */
  private void setupRange(CM closedLoopPoles, CM observerPoles) {
    RM realPart = closedLoopPoles.getRealPart().appendDown(observerPoles.getRealPart());
    RM imagPart = closedLoopPoles.getImaginaryPart().appendDown(observerPoles.getImaginaryPart());
    this.xMax = realPart.max();
    this.xMin = realPart.min();
    this.yMax = imagPart.max();
    this.yMin = imagPart.min();
    if (this.xMax.isLessThan(0)) {
      this.xMax = this.xMin.multiply(this.RATIO_OF_NEGATIVE_POSITIVE_REGION).unaryMinus();
    }
    if (this.xMin.isGreaterThan(0)) {
      this.xMin = this.xMax.multiply(this.RATIO_OF_NEGATIVE_POSITIVE_REGION).unaryMinus();
    }
  }
  //
  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   * @throws InterruptedException
  //   *         キャンセルボタンが押された場合
  //   * @throws IOException キーボードから入力できない場合 
  //   */
  //  public static void main(String[] args) throws InterruptedException, IOException {
  //    final LinearSinglePendulum linearPendulum = new LinearSinglePendulum();
  //    final RM Q = new DoubleMatrix(new double[] {1.0E5, 1.0E5, 1, 1}).vectorToDiagonal();
  //    final RM R = new DoubleMatrix(new double[] {1});
  //    final LqrStateFeedback stateFeedback = new LqrStateFeedback();
  //    stateFeedback.setWeightingMatrix(Q, R);
  //    final RM F = stateFeedback.getGain();
  //    
  //    final RM observerPoles = new DoubleComplexMatrix(new double[] {-20, -20}, new double[] {0, 0}).transpose();
  //    final ContinuousObserver observer = new ContinuousObserver();
  //    observer.setObserverPoles(observerPoles);
  //    
  //    final PoleViewer viewer = new PoleViewer(linearPendulum);
  //    viewer.plot(F, observer);
  //  }

}