DoubleBode.java
/*
* $Id: Bode.java,v 1.85 2008/04/10 03:34:27 koga Exp $
*
* Copyright (C) 2004 Koga Laboratory. All rights reserved.
*/
package org.mklab.tool.control;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.misc.LogarithmicallySpacedVector;
import org.mklab.nfc.scalar.DoubleComplexNumber;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.nfc.util.Pause;
import org.mklab.tool.graph.gnuplot.Gnuplot;
import org.mklab.tool.matrix.Unwrap;
/**
* ボード線図を描画するためのデータ(ゲイン、位相)を求めるクラスです。
*
* <p>Bode response
*
* @author koga
* @version $Revision: 1.85 $
*/
public class DoubleBode {
/** システム */
private DoubleLinearSystem system;
/** サンプリング周期 */
private double samplingInterval;
/**
* 新しく生成された<code>Bode</code>オブジェクトを初期化します。
*
* @param system 線形システム
*/
public DoubleBode(final DoubleLinearSystem system) {
this.system = (DoubleLinearSystem)system.clone();
this.system.setTimeDomainType(TimeDomainType.CONTINUOUS);
}
/**
* 新しく生成された<code>Bode</code>オブジェクトを初期化します。
*
* @param system 線形システム
* @param samplingInterval サンプリング周期
*/
public DoubleBode(final DoubleLinearSystem system, final double samplingInterval) {
this.system = (DoubleLinearSystem)system.clone();
this.samplingInterval = samplingInterval;
this.system.setTimeDomainType(TimeDomainType.DISCRETE);
}
/**
* システムの周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)を返します。
*
* @param angularFrequencies 評価する角周波数(radian/s)
* @return 周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)
*/
public List<List<DoubleMatrix>> getGainAndPhase(final DoubleMatrix angularFrequencies) {
final List<List<DoubleMatrix>> magnitudesPhases = new ArrayList<>();
if (this.system.isContinuous()) {
for (int inputNumber = 1; inputNumber <= this.system.getInputSize(); inputNumber++) {
final List<DoubleMatrix> magnitudePhase = getGainAndPhaseForContinuous(angularFrequencies, inputNumber);
magnitudesPhases.add(magnitudePhase);
}
} else {
for (int inputNumber = 1; inputNumber <= this.system.getInputSize(); inputNumber++) {
final List<DoubleMatrix> magnitudePhase = getGainAndPhaseForDiscrete(angularFrequencies, inputNumber);
magnitudesPhases.add(magnitudePhase);
}
}
return magnitudesPhases;
}
/**
* システムの周波数応答(大きさ(絶対値)、偏角(度)、評価周波数)を返します。
*
* @param angularFrequencies 評価する角周波数(radian/s)
* @param inputNumber 入力番号
* @return 周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)
*/
private List<DoubleMatrix> getGainAndPhaseForContinuous(final DoubleMatrix angularFrequencies, final int inputNumber) {
final DoubleNumber unit = angularFrequencies.getElement(1, 1);
final DoubleComplexNumber j = new DoubleComplexNumber(0,1);
final DoubleComplexMatrix jw = new DoubleComplexMatrix(angularFrequencies).multiply(j);
final DoubleComplexMatrix gjw = new DoubleFrequencyResponse(this.system).getResponse(jw, inputNumber);
final DoubleMatrix magnitude = gjw.absElementWise().getRealPart();
final DoubleMatrix phase = gjw.logElementWise().getImaginaryPart();
final DoubleNumber unwarpTolerance = unit.createPI().multiply(170).divide(180);
final DoubleMatrix unwrapedPhase = Unwrap.unwrapRowWise(phase, unwarpTolerance);
final DoubleNumber phaseDeviationTolerance = unit.createUnit().divide(10000);
final DoubleMatrix compensatedPhase = compensateForIntegratorAndDerivative(magnitude, unwrapedPhase, angularFrequencies, phaseDeviationTolerance);
final DoubleNumber pi = unit.createPI();
final DoubleMatrix phaseOfDegree = compensatedPhase.multiply(pi.inverse().multiply(180));
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {magnitude, phaseOfDegree, angularFrequencies}));
}
/**
* 積分器と微分器のための補正をした位相を返します。
*
* @param magnitudes 大きさ
* @param phases 位相
* @param angularFrequencies 評価する角周波数
* @param phaseDeviationTolerance 許容誤差
* @return 補正後の位相
*/
private DoubleMatrix compensateForIntegratorAndDerivative(final DoubleMatrix magnitudes, final DoubleMatrix phases, final DoubleMatrix angularFrequencies,
final DoubleNumber phaseDeviationTolerance) {
final DoubleMatrix modifiedPhases = phases.createClone();
final int size = angularFrequencies.length();
final DoubleNumber pi = magnitudes.getElement(1, 1).createPI();
for (int outputNumber = 1; outputNumber <= magnitudes.getRowSize(); outputNumber++) {
final DoubleMatrix phase = phases.getRowVector(outputNumber);
if (phase.max().subtract(phase.min()).isGreaterThan(phaseDeviationTolerance)) {
continue;
}
final DoubleMatrix gain = magnitudes.getRowVector(outputNumber).log10ElementWise().multiply(20);
final DoubleMatrix gainDeviation = gain.getSubVector(2, size).subtract(gain.getSubVector(1, size - 1));
final DoubleMatrix wLog10 = angularFrequencies.log10ElementWise();
final DoubleMatrix wLog10Deviation = wLog10.getSubVector(2, size).subtract(wLog10.getSubVector(1, size - 1));
final DoubleMatrix rates = gainDeviation.divideElementWise(wLog10Deviation);
final DoubleNumber degree = rates.mean().divide(20).round();
modifiedPhases.setRowVector(outputNumber, magnitudes.createOnes(1, size).multiply(degree.multiply(pi.divide(2))));
}
return modifiedPhases;
}
/**
* ゲイン線図と位相線図を描画します。
*
* @param gnuplot Gnuplot
* @param angularFrequencies 評価する角周波数(radian/s)
*/
public void plot(final Gnuplot gnuplot, final DoubleMatrix angularFrequencies) {
new BodePlotter(gnuplot).plot(getGainAndPhase(angularFrequencies), angularFrequencies);
}
/**
* ゲイン線図と位相線図を描画します。
*
* @param gnuplot Gnuplot
*/
public void plot(final Gnuplot gnuplot) {
final DoubleMatrix angularFrequencies;
if (this.system.isContinuous()) {
angularFrequencies = LogarithmicallySpacedVector.create(-2, 2);
} else {
final double period = 2 * Math.PI / this.samplingInterval;
angularFrequencies = LogarithmicallySpacedVector.create(-1, Math.log10(period));
}
plot(gnuplot, angularFrequencies);
}
/**
* システムの<code>inputNumber</code>番目の入力からの周波数応答(絶対値，位相[度]，評価周波数)を返します。
*
* @param angularFrequencies 伝達関数を評価する角周波数 [rad/sec]
* @param inputNumber 入力番号
* @return {magnitude, phase}
*
* <p>周波数応答(絶対値，位相[度]，評価周波数)
*
* <p>(magnitude(absolute value), phase(in degrees))
*/
private List<DoubleMatrix> getGainAndPhaseForDiscrete(DoubleMatrix angularFrequencies, int inputNumber) {
final DoubleNumber unit = angularFrequencies.getElement(1, 1);
final DoubleComplexNumber j = new DoubleComplexNumber(0,1);
final DoubleComplexMatrix exp_jwT = new DoubleComplexMatrix(angularFrequencies).multiply(j.multiply(this.samplingInterval)).expElementWise();
final DoubleComplexMatrix gjw = new DoubleFrequencyResponse(this.system).getResponse(exp_jwT, inputNumber);
final DoubleMatrix magnitude = gjw.absElementWise().getRealPart();
final DoubleMatrix phase = gjw.logElementWise().getImaginaryPart();
final DoubleMatrix unwrapedPhase = Unwrap.unwrapRowWise(phase);
final DoubleNumber pi = unit.createPI();
final DoubleMatrix phaseOfDegree = unwrapedPhase.multiply(pi.inverse().multiply(180));
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {magnitude, phaseOfDegree, angularFrequencies}));
}
/**
* メインメソッド
*
* @param args コマンドライン引数
* @throws IOException キーボードから入力できない場合
*/
public static void main(String[] args) throws IOException {
final DoublePolynomial s = new DoublePolynomial("s"); //$NON-NLS-1$
final DoubleRationalPolynomial gc = new DoubleRationalPolynomial(new DoublePolynomial(1), s.add(1));
final Gnuplot gnuplot1 = new Gnuplot();
final DoubleMatrix w = LogarithmicallySpacedVector.create(-2, 2);
new DoubleBode(DoubleLinearSystemFactory.createLinearSystem(gc)).plot(gnuplot1, w);
final DoublePolynomial z = new DoublePolynomial("z"); //$NON-NLS-1$
final DoubleRationalPolynomial gz = new DoubleRationalPolynomial(new DoublePolynomial(0.5), z.subtract(0.5));
final Gnuplot gnuplot2 = new Gnuplot();
new DoubleBode(DoubleLinearSystemFactory.createLinearSystem(gz), 0.1).plot(gnuplot2);
try {
Pause.pause();
} finally {
gnuplot1.close();
gnuplot2.close();
}
}
}