DoubleObserverDesigner.java

/*
 * $Id: ObserverDesigner.java,v 1.5 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.util.List;

import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.Obsg;
import org.mklab.tool.control.system.DoubleLinearSystemOperator;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.continuous.DoubleContinuousLinearDynamicSystem;


/**
 * ゴピナスの方法で連続時間最小次元オブザーバを設計するクラスです。
 * 
 * @author koga
 * @version $Revision: 1.5 $, 2004/05/31
 */
public class DoubleObserverDesigner {

  /** 状態推定の対象(線形システム) */
  private DoubleLinearSystemOperator plant;
  /** オブザーバーの極 */
  private DoubleComplexMatrix observerPoles;
  /** オブザーバー */
  private DoubleContinuousLinearDynamicSystem observer;

  /**
   * コンストラクター
   * 
   * @param plant 状態推定の対象(線形システム)
   */
  public DoubleObserverDesigner(final DoubleSystemOperator plant) {
    this.plant = (DoubleLinearSystemOperator)plant;
  }

  /**
   * オブザーバーの極を設定します。
   * 
   * @param observerPoles オブザーバーの極
   */
  public void setObserverPoles(final DoubleComplexMatrix observerPoles) {
    this.observerPoles = observerPoles;
  }

  /**
   * 最小次元連続時間オブザーバを返します。
   * 
   * @return 連続時間オブザーバ
   */
  public DoubleContinuousLinearDynamicSystem getObserver() {
    design();
    return this.observer;
  }

  /**
   * ゴピナスの方法で最小次元連続時間オブザーバを設計します。
   */
  private void design() {
    final List<DoubleMatrix> coefficientMatrices = Obsg.obsg(this.plant.getA(), this.plant.getB(), this.plant.getC(), this.observerPoles);

    final DoubleMatrix Ah = coefficientMatrices.get(0);
    final DoubleMatrix Bh = coefficientMatrices.get(1);
    final DoubleMatrix Ch = coefficientMatrices.get(2);
    final DoubleMatrix Dh = coefficientMatrices.get(3);
    final DoubleMatrix Jh = coefficientMatrices.get(4);

    final DoubleMatrix A = Ah;
    final DoubleMatrix B = Jh.appendRight(Bh);
    final DoubleMatrix C = Ch;
    final DoubleMatrix D = new DoubleMatrix(Dh.getRowSize(), Jh.getColumnSize()).appendRight(Dh);
    this.observer = new DoubleContinuousLinearDynamicSystem(A, B, C, D);
  }

  /**
   * 状態推定の対象(システム)の入力数を返します。
   * 
   * @return 状態推定の対象(システム)の入力数
   */
  public int getPlantInputSize() {
    return this.plant.getB().getColumnSize();
  }

  /**
   * 状態推定の対象(システム)の出力数を返します。
   * 
   * @return 状態推定の対象(システム)の出力数
   */
  public int getPlantOutputSize() {
    return this.plant.getC().getRowSize();
  }

  /**
   * オブザーバの係数行列を表示します。
   */
  @SuppressWarnings("nls")
  public void showObserver() {
    design();

    final int inputSize = getPlantInputSize();
    final int outputSize = getPlantOutputSize();

    final DoubleMatrix Ah = this.observer.getA();
    final DoubleMatrix Jh = this.observer.getB().getColumnVectors(1, inputSize);
    final DoubleMatrix Bh = this.observer.getB().getColumnVectors(inputSize + 1, inputSize + outputSize);
    final DoubleMatrix Ch = this.observer.getC();
    final DoubleMatrix Dh = this.observer.getD().getColumnVectors(inputSize + 1, inputSize + outputSize);

    Ah.print("Ah");
    System.out.println("");
    Jh.print("Jh");
    System.out.println("");
    Bh.print("Bh");
    System.out.println("");
    Ch.print("Ch");
    System.out.println("");
    Dh.print("Dh");
    System.out.println("");
  }

  /**
   * オブザーバーの極が指定済みで、オブザーバーが設計可能であるか判定します。
   * 
   * @return オブザーバーが設計可能ならばtrue、そうでなければfalse
   */
  public boolean isAvailable() {
    if (this.observerPoles == null) {
      return false;
    }
    return true;
  }

  //
  //  /**
  //   * メインメソッド
  //   * 
  //   * @param args
  //   *        コマンドライン引数
  //   */
  //  public static void main(String[] args) {
  //    final ObserverDesigner designer = new ObserverDesigner(new LinearSinglePendulum());
  //    final DoubleMatrix observerPoles = new DoubleComplexMatrix(new double[] {-20, -20}, new double[] {0, 0}).transpose();
  //    designer.setObserverPoles(observerPoles);
  //    designer.showObserver();
  //  }

  /**
   * 状態推定の対象(線形システム)を設定します。
   * 
   * @param plant 状態推定の対象(線形システム)
   */
  public void setPlant(final DoubleLinearSystemOperator plant) {
    this.plant = plant;
  }
}