Bode.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.util.ArrayList;
import java.util.List;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.matrix.Unwrap;
/**
* ボード線図を描画するためのデータ(ゲイン、位相)を求めるクラスです。
*
* <p>Bode response
*
* @author koga
* @version $Revision: 1.85 $
* @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 Bode<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>> {
/** システム */
private LinearSystem<RS,RM,CS,CM> system;
/** サンプリング周期 */
private RS samplingInterval;
/**
* 新しく生成された<code>Bode</code>オブジェクトを初期化します。
*
* @param system 線形システム
*/
public Bode(final LinearSystem<RS,RM,CS,CM> system) {
this.system = (LinearSystem<RS,RM,CS,CM>)system.clone();
this.system.setTimeDomainType(TimeDomainType.CONTINUOUS);
}
/**
* 新しく生成された<code>Bode</code>オブジェクトを初期化します。
*
* @param system 線形システム
* @param samplingInterval サンプリング周期
*/
public Bode(final LinearSystem<RS,RM,CS,CM> system, final RS samplingInterval) {
this.system = (LinearSystem<RS,RM,CS,CM>)system.clone();
this.samplingInterval = samplingInterval;
this.system.setTimeDomainType(TimeDomainType.DISCRETE);
}
/**
* システムの周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)を返します。
*
* @param angularFrequencies 評価する角周波数(radian/s)
* @return 周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)
*/
public List<List<RM>> getGainAndPhase(final RM angularFrequencies) {
final List<List<RM>> magnitudesPhases = new ArrayList<>();
if (this.system.isContinuous()) {
for (int inputNumber = 1; inputNumber <= this.system.getInputSize(); inputNumber++) {
final List<RM> magnitudePhase = getGainAndPhaseForContinuous(angularFrequencies, inputNumber);
magnitudesPhases.add(magnitudePhase);
}
} else {
for (int inputNumber = 1; inputNumber <= this.system.getInputSize(); inputNumber++) {
final List<RM> magnitudePhase = getGainAndPhaseForDiscrete(angularFrequencies, inputNumber);
magnitudesPhases.add(magnitudePhase);
}
}
return magnitudesPhases;
}
/**
* システムの周波数応答(大きさ(絶対値)、偏角(度)、評価周波数)を返します。
*
* @param angularFrequencies 評価する角周波数(radian/s)
* @param inputNumber 入力番号
* @return 周波数応答(大きさ(絶対値)，偏角(度)，評価周波数)
*/
private List<RM> getGainAndPhaseForContinuous(final RM angularFrequencies, final int inputNumber) {
final RS unit = angularFrequencies.getElement(1, 1);
final CS j = unit.toComplex().create(unit.createZero(), unit.createUnit());
final CM jw = angularFrequencies.toComplex().multiply(j);
final CM gjw = new FrequencyResponse<>(this.system).getResponse(jw, inputNumber);
final RM magnitude = gjw.absElementWise().getRealPart();
final RM phase = gjw.logElementWise().getImaginaryPart();
final RS unwarpTolerance = unit.createPI().multiply(170).divide(180);
final RM unwrapedPhase = Unwrap.unwrapRowWise(phase, unwarpTolerance);
final RS phaseDeviationTolerance = unit.createUnit().divide(10000);
final RM compensatedPhase = compensateForIntegratorAndDerivative(magnitude, unwrapedPhase, angularFrequencies, phaseDeviationTolerance);
final RS pi = unit.createPI();
final RM phaseOfDegree = compensatedPhase.multiply(pi.inverse().multiply(180));
final List<RM> gp = new ArrayList<>();
gp.add(magnitude);
gp.add(phaseOfDegree);
gp.add(angularFrequencies);
return gp;
}
/**
* 積分器と微分器のための補正をした位相を返します。
*
* @param magnitudes 大きさ
* @param phases 位相
* @param angularFrequencies 評価する角周波数
* @param phaseDeviationTolerance 許容誤差
* @return 補正後の位相
*/
private RM compensateForIntegratorAndDerivative(final RM magnitudes, final RM phases, final RM angularFrequencies,
final RS phaseDeviationTolerance) {
final RM modifiedPhases = phases.createClone();
final int size = angularFrequencies.length();
final RS pi = magnitudes.getElement(1, 1).createPI();
for (int outputNumber = 1; outputNumber <= magnitudes.getRowSize(); outputNumber++) {
final RM phase = phases.getRowVector(outputNumber);
if (phase.max().subtract(phase.min()).isGreaterThan(phaseDeviationTolerance)) {
continue;
}
final RM gain = magnitudes.getRowVector(outputNumber).log10ElementWise().multiply(20);
final RM gainDeviation = gain.getSubVector(2, size).subtract(gain.getSubVector(1, size - 1));
final RM wLog10 = angularFrequencies.log10ElementWise();
final RM wLog10Deviation = wLog10.getSubVector(2, size).subtract(wLog10.getSubVector(1, size - 1));
final RM rates = gainDeviation.divideElementWise(wLog10Deviation);
final RS 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 RM angularFrequencies) {
// new BodePlotter(gnuplot).plot(getGainAndPhase(angularFrequencies), angularFrequencies);
// }
// /**
// * ゲイン線図と位相線図を描画します。
// *
// * @param gnuplot Gnuplot
// */
// public void plot(final Gnuplot gnuplot) {
// final RM angularFrequencies;
// final RS sunit = this.system.getA().getElement(1,1).createUnit();
// if (this.system.isContinuous()) {
// angularFrequencies = LogarithmicallySpacedVector.create(sunit.create(-2), sunit.create(2));
// } else {
// final RS period = this.samplingInterval.inverse().multiply(2).multiply(this.samplingInterval.createPI());
// angularFrequencies = LogarithmicallySpacedVector.create(sunit.create(-1), period.log10());
// }
// 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<RM> getGainAndPhaseForDiscrete(RM angularFrequencies, int inputNumber) {
final RS unit = angularFrequencies.getElement(1, 1);
final CS j = unit.toComplex().create(unit.createZero(), unit.createUnit());
final CM exp_jwT = angularFrequencies.toComplex().multiply(j.multiply(this.samplingInterval)).expElementWise();
final CM gjw = new FrequencyResponse<>(this.system).getResponse(exp_jwT, inputNumber);
final RM magnitude = gjw.absElementWise().getRealPart();
final RM phase = gjw.logElementWise().getImaginaryPart();
final RM unwrapedPhase = Unwrap.unwrapRowWise(phase);
final RS pi = unit.createPI();
final RM phaseOfDegree = unwrapedPhase.multiply(pi.inverse().multiply(180));
final List<RM>gp = new ArrayList<>();
gp.add(magnitude);
gp.add(phaseOfDegree);
gp.add(angularFrequencies);
return gp;
}
}