FirstOrderHoldSystem.java

/*
 * $Id$
 *
 * Copyright (C) 2004-2005 Koga Laboratory. All rights reserved.
 */

package org.mklab.tool.control.system.discrete;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * １次ホールドシステムを表すクラスです。
 * 
 * @author koga
 * @version $Revision$
 * @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 FirstOrderHoldSystem<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>> extends HoldSystem<RS,RM,CS,CM> {

  /**
   * Creates {@link FirstOrderHoldSystem}.
   * @param sunit unit of scalar
   */
  public FirstOrderHoldSystem(RS sunit) {
    super(sunit);
  }
  
  /**
   * {@inheritDoc}
   */
  @Override
  public RM outputEquation(final RS t, final RM u) {
    final int size = getInputSize();

    if (isAtSamplingPoint()) {
      RM state = getState();
      setState(u.appendDown(state.getSubVector(1, size)));
    }

    final RS interval = getSamplingInterval();
    final RM state = getState();
    final RM u0 = state.getSubVector(1, size);
    final RM u1 = state.getSubVector(size + 1, 2 * size);
    final RM ratio = u0.subtract(u1).divide(interval);
    int k = (int)t.divide(interval).floor().toDouble();

    if (!isAtSamplingPoint() && t.subtract(interval.multiply(k)).isLessThanOrEquals(0)) {
      k = k - 1;
    }

    return u0.add(ratio.multiply(t.subtract(interval.multiply(k))));
  }

  /**
   * @see org.mklab.tool.control.system.SystemOperator#initialize()
   */
  @Override
  public void initialize() {
    final int size = getInputSize();

    if (size < 0) {
      return;
    }

    setState(this.sunit.createZeroGrid(2 * size, 1));
  }
}