DiscretePIDSystem.java

/*
 * Created on 2012/07/24
 * Copyright (C) 2012 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.AnyRealPolynomial;
import org.mklab.nfc.scalar.AnyRealRationalPolynomial;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.LinearSystemFactory;


/**
 * 離散時間PIDシステムを表すクラスです。
 * 
 * @author esumi
 * @version $Revision$, 2012/07/24
 * @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 DiscretePIDSystem<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 DiscreteLinearDynamicSystem<RS, RM, CS, CM> {

  /** PIDコントローラのタイプ */
  private DiscretePIDType pidType = DiscretePIDType.PARALELL;
  /** 積分手法 */
  private DiscretePIDIntegralType integralPattern = DiscretePIDIntegralType.FORWARD_EULER;
  /** フィルター手法 */
  private DiscretePIDIntegralType filterPattern = DiscretePIDIntegralType.FORWARD_EULER;

  /**
   * Creates {@link DiscretePIDSystem}.
   * 
   * @param sunit unit of scalar
   */
  public DiscretePIDSystem(RS sunit) {
    super(sunit);
  }

  /**
   * PIDシステムを設定します。
   * 
   * @param p 比例項P
   * @param i 積分項I
   * @param d 微分項D
   * @param n フィルターN
   */
  public void setLinearSystem(RS p, RS i, RS d, RS n) {
    AnyRealPolynomial<RS, RM, CS, CM> aNum = null;
    AnyRealPolynomial<RS, RM, CS, CM> aDen = null;
    AnyRealPolynomial<RS, RM, CS, CM> bNum = null;
    AnyRealPolynomial<RS, RM, CS, CM> bDen = null;

    RS smTime = getSamplingInterval();
    RS[] an;
    RS[] ad;

    switch (this.integralPattern) {
      case FORWARD_EULER:
        an = this.sunit.createArray(1);
        an[0] = smTime;
        aNum = new AnyRealPolynomial<>(an);

        ad = this.sunit.createArray(2);
        ad[0] = this.sunit.createUnit().unaryMinus();
        ad[1] = this.sunit.createUnit();
        aDen = new AnyRealPolynomial<>(ad);
        break;
      case BACKWARD_EULER:
        an = this.sunit.createArray(2);
        an[0] = this.sunit.createZero();
        an[1] = smTime;
        aNum = new AnyRealPolynomial<>(an);

        ad = this.sunit.createArray(2);
        ad[0] = this.sunit.createUnit().unaryMinus();
        ad[1] = this.sunit.createUnit();
        aDen = new AnyRealPolynomial<>(ad);
        break;
      case TRAPEZOIDAL:
        an = this.sunit.createArray(2);
        an[0] = smTime;
        an[1] = smTime.clone();
        aNum = new AnyRealPolynomial<>(an);

        ad = this.sunit.createArray(2);
        ad[0] = this.sunit.create(2).unaryMinus();
        ad[1] = this.sunit.create(2);
        aDen = new AnyRealPolynomial<>(ad);
        break;
      default:
        throw new IllegalArgumentException();
    }

    RS[] bn;
    RS[] bd;

    switch (this.filterPattern) {
      case FORWARD_EULER:
        bn = this.sunit.createArray(1);
        bn[0] = smTime;
        bNum = new AnyRealPolynomial<>(bn);

        bd = this.sunit.createArray(2);
        bd[0] = this.sunit.create(1).unaryMinus();
        bd[1] = this.sunit.create(1);
        bDen = new AnyRealPolynomial<>(bd);
        break;
      case BACKWARD_EULER:
        bn = this.sunit.createArray(2);
        bn[0] = this.sunit.create(0);
        bn[1] = smTime;
        bNum = new AnyRealPolynomial<>(bn);

        bd = this.sunit.createArray(2);
        bd[0] = this.sunit.create(1).unaryMinus();
        bd[1] = this.sunit.create(1);
        bDen = new AnyRealPolynomial<>(bd);
        break;
      case TRAPEZOIDAL:
        bn = this.sunit.createArray(2);
        bn[0] = smTime;
        bn[1] = smTime.clone();
        bNum = new AnyRealPolynomial<>(bn);

        bd = this.sunit.createArray(2);
        bd[0] = this.sunit.create(2).unaryMinus();
        bd[1] = this.sunit.create(2);
        bDen = new AnyRealPolynomial<>(bd);
        break;
      default:
        throw new IllegalArgumentException();
    }

    AnyRealRationalPolynomial<RS, RM, CS, CM> a = new AnyRealRationalPolynomial<>(aNum, aDen); 
    AnyRealRationalPolynomial<RS, RM, CS, CM> b = new AnyRealRationalPolynomial<>(bNum, bDen); 

    AnyRealRationalPolynomial<RS, RM, CS, CM> Gs;

    switch (this.pidType) {
      case PARALELL:
        Gs = a.multiply(i).add(p).add(b.multiply(n).add(1).inverse().multiply(n).multiply(d));
        break;
      case IDEAL:
        Gs = a.multiply(i).add(1).add(b.multiply(n).add(1).inverse().multiply(n).multiply(d)).multiply(p);
        break;
      default:
        throw new IllegalArgumentException();
    }
    setLinearSystem(LinearSystemFactory.createLinearSystem(Gs));
  }

  /**
   * 積分手法を設定します。
   * 
   * @param integralPattern 積分手法
   */
  public void setIntegralPattern(DiscretePIDIntegralType integralPattern) {
    this.integralPattern = integralPattern;
  }

  /**
   * フィルター手法を設定します。
   * 
   * @param filterPattern フィルター手法
   */
  public void setFilterPattern(DiscretePIDIntegralType filterPattern) {
    this.filterPattern = filterPattern;
  }

  /**
   * PIDコントローラのタイプを設定します。
   * 
   * @param pidType PIDコントローラのタイプ
   */
  public void setPidType(DiscretePIDType pidType) {
    this.pidType = pidType;
  }
}