DoubleRateLimiter.java

/*
 * $Id: RateLimiter.java,v 1.17 2008/07/15 15:27:15 koga Exp $
 *
 * Copyright (C) 2004 Masanobu Koga. All rights reserved.
 */
package org.mklab.tool.control.system.discontinuous;

import java.util.ArrayList;
import java.util.List;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.ode.DoublePiecewiseContinuousAlgebraicSystem;
import org.mklab.nfc.ode.EquationSolver;
import org.mklab.nfc.ode.PiecewiseUtil;
import org.mklab.tool.control.system.continuous.DoubleBaseContinuousStaticSystem;
import org.mklab.tool.control.system.parameter.Parameter;
import org.mklab.tool.control.system.parameter.StringExternalizable;


/**
 * 信号の変化率制限システムを表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.17 $
 */
public class DoubleRateLimiter extends DoubleBaseContinuousStaticSystem implements DoublePiecewiseContinuousAlgebraicSystem, StringExternalizable {

  /** 変化率の下限 */
  @Parameter(name = "rateLowerBound", description = "RateLimiter.1", internationalization = true)
  private double rateLowerBound;

  /** 変化率の上限 */
  @Parameter(name = "rateUpperBound", description = "RateLimiter.3", internationalization = true)
  private double rateUpperBound;

  /** 初期出力 */
  @Parameter(name = "initialOutput", description = "RateLimiter.5", internationalization = true)
  private DoubleMatrix initialOutput = new DoubleMatrix(1, 1);

  /** 1刻み前の時刻 */
  private double previousTime;

  /** 1呼び出し前の出力 */
  private DoubleMatrix previousOutput;

  /** 入力の変化率 */
  private DoubleMatrix inputRate;

  /**
   * コンストラクター
   */
  public DoubleRateLimiter() {
    this(-1, 1);
  }

  /**
   * 新しく生成された<code>Saturation</code>オブジェクトを初期化します。
   * 
   * @param rateLowerBound 出力の最小値
   * @param rateUpperBound 出力の最大値
   */
  public DoubleRateLimiter(final double rateLowerBound, final double rateUpperBound) {
    super(-1, -1);
    setAutoSize(true);
    setHasDirectFeedthrough(true);
    setRateLowerBound(rateLowerBound);
    setRateUpperBound(rateUpperBound);

    initialize();
  }

  /**
   * 初期出力を設定します。
   * 
   * @param initialOutput 初期出力
   */
  public void setInitialOutput(final DoubleMatrix initialOutput) {
    this.initialOutput = initialOutput.createClone();
  }

  /**
   * @see org.mklab.tool.control.system.continuous.BaseContinuousStaticSystem#initialize()
   */
  @Override
  public void initialize() {
    this.previousOutput = this.initialOutput.createClone();
    this.previousTime = 0;
    this.inputRate = new DoubleMatrix(this.initialOutput.getRowSize(), 1);
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public DoubleMatrix outputEquation(final double t, final DoubleMatrix u) {
    if (t == this.previousTime) {
      return this.previousOutput.createClone();
    }

    final int size = u.getRowSize();
    final DoubleMatrix output = new DoubleMatrix(size, 1);

    this.inputRate = u.subtract(this.previousOutput).divide(t - this.previousTime);

    for (int i = 1; i <= size; i++) {
      final double uc = u.getDoubleElement(i, 1);
      final double up = this.previousOutput.getDoubleElement(i, 1);
      final double rate = this.inputRate.getDoubleElement(i, 1);

      if (this.rateUpperBound < rate) {
        output.setElement(i, 1, up + this.rateUpperBound * (t - this.previousTime));
      } else if (rate < this.rateLowerBound) {
        output.setElement(i, 1, up + this.rateLowerBound * (t - this.previousTime));
      } else {
        output.setElement(i, 1, uc);
      }
    }

    if (EquationSolver.isTrial() == false) {
      this.previousOutput = output.createClone();
      this.previousTime = t;
    }

    return output;
  }

  /**
   * 変化率の下限を設定します。
   * 
   * @param rateLowerBound 変化率の下限
   */
  public void setRateLowerBound(final double rateLowerBound) {
    this.rateLowerBound = rateLowerBound;
  }

  /**
   * 変化率の上限を設定します。
   * 
   * @param rateUpperBound 変化率の上限
   */
  public void setRateUpperBound(final double rateUpperBound) {
    this.rateUpperBound = rateUpperBound;
  }

  /**
   * 変化率の下限を返します。
   * 
   * @return 変化率の下限
   */
  public double getRateLowerBound() {
    return this.rateLowerBound;
  }

  /**
   * 変化率の上限を返します。
   * 
   * @return 変化率の上限
   */
  public double getRateUpperBound() {
    return this.rateUpperBound;
  }

  /**
   * {@inheritDoc}
   */
  @SuppressWarnings("boxing")
  public List<Integer> getPiece( final double t, final DoubleMatrix u) {
    final int size = u.getRowSize();
    final List<Integer> piece = new ArrayList<>(size);

    for (int i = 1; i <= size; i++) {
      final double rate = this.inputRate.getDoubleElement(i, 1);

      if (rate < this.rateLowerBound) {
        piece.add(0);
      } else if (rate < this.rateUpperBound) {
        piece.add(1);
      } else {
        piece.add(2);
      }
    }

    return piece;
  }

  /**
   * {@inheritDoc}
   */
  @SuppressWarnings("boxing")
  public double getDiscontinuousPoint(final double t1, final DoubleMatrix u1, final double t2, final DoubleMatrix u2) {
    final List<Integer> pieces1 = getPiece(t1, u1);
    final List<Integer> pieces2 = getPiece(t2, u2);
    if (pieces1.equals(pieces2)) {
      return Double.NaN;
    }

    final int number = PiecewiseUtil.getDistinctPiece(pieces1, pieces2);
    final int piece1 = pieces1.get(number - 1);
    final int piece2 = pieces2.get(number - 1);

    final double uu1 = u1.getDoubleElement(number, 1);
    final double uu2 = u2.getDoubleElement(number, 1);
    final double up = this.previousOutput.getDoubleElement(number, 1);

    final double rate1 = (uu1 - up) / (t1 - this.previousTime);
    final double rate2 = (uu2 - up) / (t2 - this.previousTime);

    final boolean fromOneToTwo = piece1 == 1 && piece2 == 2;
    final boolean fromTwoToOne = piece1 == 2 && piece2 == 1;

    if (fromOneToTwo || fromTwoToOne) {
      return t1 + (this.rateUpperBound - rate1) / (rate2 - rate1) * (t2 - t1);
    }

    final boolean fromOneToZero = piece1 == 1 && piece2 == 0;
    final boolean fromZeroToOne = piece1 == 0 && piece2 == 1;

    if (fromOneToZero || fromZeroToOne) {
      return t1 + (rate1 - this.rateLowerBound) / (rate1 - rate2) * (t2 - t1);
    }

    assert false : "never reached"; //$NON-NLS-1$

    return (t1 + t2) / 2;
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setInputSize(int)
   */
  @Override
  public void setInputSize(final int size) {
    super.setInputSize(size);
    super.setOutputSize(size);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#setOutputSize(int)
   */
  @Override
  public void setOutputSize(final int size) {
    super.setInputSize(size);
    super.setOutputSize(size);
  }

  /**
   * @see org.mklab.tool.control.system.parameter.StringExternalizable#getString(java.lang.String)
   */
  public String getString(String key) {
    return Messages.getString(key);
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#equals(java.lang.Object)
   */
  @Override
  public boolean equals(Object o) {
    if (this == o) {
      return true;
    }
    if (!super.equals(o)) {
      return false;
    }
    if (o == null) {
      return false;
    }
    if (o.getClass() != getClass()) {
      return false;
    }
    DoubleRateLimiter castedObj = (DoubleRateLimiter)o;
    return ((this.rateLowerBound == castedObj.rateLowerBound) && (this.rateUpperBound == castedObj.rateUpperBound)
        && (this.initialOutput == null ? castedObj.initialOutput == null : this.initialOutput.equals(castedObj.initialOutput)) && (this.previousTime == castedObj.previousTime)
        && (this.previousOutput == null ? castedObj.previousOutput == null : this.previousOutput.equals(castedObj.previousOutput)) && (this.inputRate == null ? castedObj.inputRate == null
          : this.inputRate.equals(castedObj.inputRate)));
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#hashCode()
   */
  @Override
  public int hashCode() {
    int hashCode = super.hashCode();
    hashCode = 31 * hashCode + (int)(Double.doubleToLongBits(this.rateLowerBound) ^ (Double.doubleToLongBits(this.rateLowerBound) >>> 32));
    hashCode = 31 * hashCode + (int)(Double.doubleToLongBits(this.rateUpperBound) ^ (Double.doubleToLongBits(this.rateUpperBound) >>> 32));
    hashCode = 31 * hashCode + (this.initialOutput == null ? 0 : this.initialOutput.hashCode());
    hashCode = 31 * hashCode + (int)(Double.doubleToLongBits(this.previousTime) ^ (Double.doubleToLongBits(this.previousTime) >>> 32));
    hashCode = 31 * hashCode + (this.previousOutput == null ? 0 : this.previousOutput.hashCode());
    hashCode = 31 * hashCode + (this.inputRate == null ? 0 : this.inputRate.hashCode());
    return hashCode;
  }
}