ContinuousObserver.java

/*
 * $Id: ContinuousObserver.java,v 1.5 2008/06/26 10:10:34 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.model.matxbook;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.system.continuous.DoubleBaseContinuousExplicitDynamicSystem;


/**
 * 連続時間オブザーバーを表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.5 $, 2004/04/23
 */
public class ContinuousObserver extends DoubleBaseContinuousExplicitDynamicSystem {

  /** 係数行列 */
  DoubleMatrix Ah = new DoubleMatrix(new double[] {-3});
  /** 係数行列 */
  DoubleMatrix Bh = new DoubleMatrix(new double[] {-6});
  /** 係数行列 */
  DoubleMatrix Jh = new DoubleMatrix(new double[] {1});
  /** 係数行列 */
  DoubleMatrix Ch = new DoubleMatrix(new double[][] { {0}, {1}});
  /** 係数行列 */
  DoubleMatrix Dh = new DoubleMatrix(new double[][] { {1}, {2}});

  /**
   * コンストラクター
   */
  public ContinuousObserver() {
    super(2, 2, 1);
  }
  
  /**
   * {@inheritDoc}
   */
  public DoubleMatrix stateEquation(double t, DoubleMatrix z, DoubleMatrix uy) {
    DoubleMatrix u = uy.getSubVector(1, 1);
    DoubleMatrix y = uy.getSubVector(2, 2);
    DoubleMatrix dz = this.Ah.multiply(z).add(this.Bh.multiply(y)).add(this.Jh.multiply(u));
    return dz;
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public DoubleMatrix outputEquation(double t, DoubleMatrix z, DoubleMatrix uy) {
    DoubleMatrix y = uy.getSubVector(2, 2);
    DoubleMatrix xh = this.Ch.multiply(z).add(this.Dh.multiply(y));
    return xh;
  }
}