MotorDiscreteObserver.java

/*
 * $Id$
 *
 * 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.discrete.DoubleZeroOrderHoldSystem;
import org.mklab.tool.control.system.sampled.DoubleBaseSampledDataDynamicSystem;


/**
 * モータと離散時間オブザーバーの結合システムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$, 2004/04/23
 */
public class MotorDiscreteObserver extends DoubleBaseSampledDataDynamicSystem {

  /** モータ */
  MotorWithFullState motor = new MotorWithFullState();
  /** 状態フィードバック制御器 */
  StateFeedback stateFeedback = new StateFeedback();
  /** 離散時間オブザーバー */
  DiscreteObserver discObserver = new DiscreteObserver();
  /** ゼロ次ホールド */
  DoubleZeroOrderHoldSystem hold = new DoubleZeroOrderHoldSystem();

  /**
   * コンストラクター
   */
  public MotorDiscreteObserver() {
    super(1, 1, 2, 1);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix continuousStateEquation(double t, DoubleMatrix xc,  DoubleMatrix xd, DoubleMatrix inputOutput) {
    this.motor.setState(xc);

    DoubleMatrix u = inputOutput.getSubVector(1, 1);
    return this.motor.stateEquation(t, this.motor.getState(), u);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix discreteStateEquation(double t, DoubleMatrix xc, DoubleMatrix xd, DoubleMatrix inputOutput) {
    this.motor.setState(xc);
    this.discObserver.setState(xd);

    DoubleMatrix uy = inputOutput.getSubVector(1, 2);
    int k = (int)(t / this.discObserver.getSamplingInterval());
    return this.discObserver.stateEquation(k, this.discObserver.getState(), uy);
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix inputOutputEquation(double t, DoubleMatrix xc, DoubleMatrix xd) {
    this.motor.setState(xc);
    this.discObserver.setState(xd);

    DoubleMatrix y = this.motor.outputEquation(t, this.motor.getState());

    DoubleMatrix xh;
    if (this.hold.isAtSamplingPoint()) {
      int k = (int)Math.floor(t / this.discObserver.getSamplingInterval());
      xh = this.discObserver.outputEquation(k, this.discObserver.getState(), y);
      xh = this.hold.outputEquation(t, xh);
    } else {
      xh = this.hold.outputEquation(t, null);
    }

    DoubleMatrix u = this.stateFeedback.outputEquation(t, xh).unaryMinus();
    return u.appendDown(y);
  }

  /**
   * {@inheritDoc}
   */
  public double getNextSamplingTime(double t, double tolerance) {
    double samplingInterval = this.discObserver.getSamplingInterval();
    double nextSamplingTime = Math.ceil(t / samplingInterval) * samplingInterval;
    if (Math.abs(t - nextSamplingTime) < tolerance) {
      nextSamplingTime = t + this.discObserver.getSamplingInterval();
    }
    return nextSamplingTime;
  }

  /**
   * @see org.mklab.nfc.ode.DifferentialDifferenceSystem#setAtSamplingPoint(boolean)
   */
  @Override
  public void setAtSamplingPoint(boolean samplingPoint) {
    super.setAtSamplingPoint(samplingPoint);
    this.hold.setAtSamplingPoint(samplingPoint);
    this.discObserver.setAtSamplingPoint(samplingPoint);
  }

  /**
   * @see org.mklab.nfc.ode.DifferentialDifferenceSystem#isAtSamplingPoint()
   */
  @Override
  public boolean isAtSamplingPoint() {
    return this.hold.isAtSamplingPoint() || this.discObserver.isAtSamplingPoint();
  }

  /**
   * サンプリング周期を設定します。
   * 
   * @param interval サンプリング周期
   */
  public void setSamplingInterval(double interval) {
    this.hold.setSamplingInterval(interval);
    this.discObserver.setSamplingInterval(interval);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setContinuousInitialState(DoubleMatrix initialState) {
    this.motor.setInitialState(initialState);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setDiscreteInitialState(DoubleMatrix initialState) {
    this.discObserver.setInitialState(initialState);
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousInitialState()
   */
  @Override
  public DoubleMatrix getContinuousInitialState() {
    return this.motor.getInitialState();
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteInitialState()
   */
  @Override
  public DoubleMatrix getDiscreteInitialState() {
    return this.discObserver.getInitialState();
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setContinuousState(DoubleMatrix state) {
    this.motor.setState(state);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public void setDiscreteState(DoubleMatrix state) {
    this.discObserver.setState(state);
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousState()
   */
  @Override
  public DoubleMatrix getContinuousState() {
    return this.motor.getState();
  }

  /**
   * @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteState()
   */
  @Override
  public DoubleMatrix getDiscreteState() {
    return this.discObserver.getState();
  }

}