Zp2ss.java
/*
* $Id: Zp2ss.java,v 1.37 2008/07/17 07:30:03 koga Exp $
*
* Copyright (C) 2004 Koga Laboratory. All rights reserved.
*/
package org.mklab.tool.control;
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.NormType;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.tool.matrix.Ccpair;
/**
* 極とゼロ点の集合から状態空間表現に変換するクラスです。
*
* <p>Zero-pole to state-space conversion
*
* @author koga
* @version $Revision: 1.37 $
* @see org.mklab.tool.control.Zp2tf
* @see org.mklab.tool.control.Zp2tfm
* @see org.mklab.tool.control.Zp2tfn
* @see org.mklab.tool.control.Ss2zp
*/
public class Zp2ss {
/**
* 極<code>p</code>、ゼロ点<code>z</code>,ゲイン<code>k</code>であるシステム
*
* <pre><code> (s-z1)(s-z2)...(s-zn) G(s) = k --------------------- (s-p1)(s-p2)...(s-pn) </code></pre>
*
* の状態空間表現
*
* <pre><code>
*
* . x = Ax + Bu y = Cx + Du </code></pre>
*
* の係数行列<code>A,B,C,D</code>を返します。
*
* <p>係数行列<code>A,B,C,D</code>は、ブロック対角形式で返されます。
*
* @param z_ ゼロ点の集合
* @param p_ 極の集合
* @param k ゲイン
* @return {A,B,C,D} (状態空間表現) (state space representation)
*/
public static List<DoubleMatrix> zp2ss(DoubleComplexMatrix z_, DoubleComplexMatrix p_, DoubleMatrix k) {
DoubleMatrix mat = z_.getRealPart();
DoubleNumber unit = mat.getElement(1, 1);
DoubleComplexMatrix z = z_;
DoubleComplexMatrix p = p_;
if (k.length() != z.getColumnSize() && z.length() != 0) {
throw new RuntimeException(Messages.getString("Zp2ss.0")); //$NON-NLS-1$
}
DoubleMatrix A, B, C, D;
DoubleMatrix nu, de;
// multiple output case
if (z.getColumnSize() > 1) {
List<DoubleMatrix> tmp = Zp2tf.zp2tf(z, p, k);
nu = tmp.get(0);
de = tmp.get(1);
List<DoubleMatrix> tmp2 = Tf2ss.tf2ss(nu, de);
A = tmp2.get(0);
B = tmp2.get(1);
C = tmp2.get(2);
D = tmp2.get(3);
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
}
// single output case
if (p.length() != 0) {
p = p.getSubVector(p.absElementWise().isInfiniteElementWise().notElementWise().find());
}
if (z.length() != 0) {
z = z.getSubVector(z.absElementWise().isInfiniteElementWise().notElementWise().find());
}
// Make groups for complex pairs
int np = p.length();
int nz = z.length();
if (p.length() != 0) {
p = Ccpair.ccpair(p, p.norm(NormType.TWO).getRealPart().multiply(unit.getMachineEpsilon()).multiply(10 * np));
}
if (z.length() != 0) {
z = Ccpair.ccpair(z, z.norm(NormType.TWO).getRealPart().multiply(unit.getMachineEpsilon()).multiply(10 * nz));
}
// The last element of p and z may be real number
if (np % 2 != 0 && nz % 2 != 0) {
// s - z(nz) p(np) - z(nz)
// G(s) = ---------- = ------------- + 1
// s - p(np) s - p(np)
A = p.getSubVector(np, np).getRealPart();
B = mat.createUnit(1);
C = unit.createGrid(new DoubleNumber[] {p.getElement(np).getRealPart().subtract(z.getElement(nz).getRealPart())});
D = mat.createUnit(1);
np--;
nz--;
} else if (np % 2 != 0) {
// 1
// G(s) = ---------
// s - p(np)
A = p.getSubVector(np, np).getRealPart();
B = mat.createUnit(1);
C = mat.createUnit(1);
D = mat.createZero(1, 1);
np--;
} else if (nz % 2 != 0) {
// (s - z(nz))
// G(s) = ------------------------
// (s - p(np-1))*(s - p(np))
nu = unit.createGrid(new DoubleNumber[] {unit.createUnit(), (z.getElement(nz).unaryMinus()).getRealPart()});
de = unit.createGrid(new DoubleNumber[] {unit.createUnit(), (p.getElement(np - 1).add(p.getElement(np)).unaryMinus()).getRealPart(),
(p.getElement(np - 1).multiply(p.getElement(np))).getRealPart()});
A = unit.createGrid(new DoubleNumber[] {de.getElement(2).unaryMinus(), de.getElement(3).unaryMinus()}).appendDown(
unit.createGrid(new DoubleNumber[] {unit.createUnit(), unit.createZero()}));
B = unit.createGrid(2, 1, new DoubleNumber[][] { {unit.createUnit()}, {unit.createZero()}});
C = unit.createGrid(new DoubleNumber[] {unit.createUnit(), nu.getElement(2)});
D = mat.createZero(1, 1);
nz--;
np = np - 2;
} else {
A = mat.createZero(0, 0);
B = mat.createZero(0, 0);
C = mat.createZero(0, 0);
D = mat.createZero(1, 1); // revised 2001.08.02
}
int i;
for (i = 1; i < nz; i = i + 2) {
// (s - z(i))*(s - z(i+1))
// G(s) = -----------------------
// (s - p(i))*(s - p(i+1))
nu = unit.createGrid(new DoubleNumber[] {unit.createUnit(), (z.getElement(i).add(z.getElement(i + 1)).unaryMinus()).getRealPart(),
(z.getElement(i).multiply(z.getElement(i + 1))).getRealPart()});
de = unit.createGrid(new DoubleNumber[] {unit.createUnit(), (p.getElement(i).add(p.getElement(i + 1)).unaryMinus()).getRealPart(),
(p.getElement(i).multiply(p.getElement(i + 1))).getRealPart()});
DoubleMatrix a = unit.createGrid(new DoubleNumber[] {de.getElement(2).unaryMinus(), de.getElement(3).unaryMinus()}).appendDown(
unit.createGrid(new DoubleNumber[] {unit.createUnit(), unit.createZero()}));
DoubleMatrix b = unit.createGrid(2, 1, new DoubleNumber[][] { {unit.createUnit()}, {unit.createZero()}});
DoubleMatrix c = unit.createGrid(new DoubleNumber[] {nu.getElement(2).subtract(de.getElement(2)), nu.getElement(3).subtract(de.getElement(3))});
DoubleMatrix d = mat.createUnit(1);
List<DoubleMatrix> tmp = Series.series(A, B, C, D, a, b, c, d);
A = tmp.get(0);
B = tmp.get(1);
C = tmp.get(2);
D = tmp.get(3);
}
for (; i < np; i = i + 2) {
// 1
// G(s) = -----------------------
// (s - p(i))*(s - p(i+1))
de = unit.createGrid(new DoubleNumber[] {unit.createUnit(), (p.getElement(i).add(p.getElement(i + 1)).unaryMinus()).getRealPart(),
(p.getElement(i).multiply(p.getElement(i + 1))).getRealPart()});
final DoubleMatrix a1 = unit.createGrid(new DoubleNumber[] {de.getElement(2).unaryMinus(), de.getElement(3).unaryMinus()});
final DoubleMatrix a2 = unit.createGrid(new DoubleNumber[] {unit.createUnit(), unit.createZero()});
DoubleMatrix a = a1.appendDown(a2);
DoubleMatrix b = unit.createGrid(2, 1, new DoubleNumber[][] { {unit.createUnit()}, {unit.createZero()}});
DoubleMatrix c = unit.createGrid(new DoubleNumber[] {unit.createZero(), unit.createUnit()});
DoubleMatrix d = mat.createZero(1, 1);
List<DoubleMatrix> tmp = Series.series(A, B, C, D, a, b, c, d);
A = tmp.get(0);
B = tmp.get(1);
C = tmp.get(2);
D = tmp.get(3);
}
C = k.vectorToDiagonal().multiply(C);
D = k.vectorToDiagonal().multiply(D);
return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {A, B, C, D}));
}
}