Obsg.java

/*
 * $Id: Obsg.java,v 1.31 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.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.DoubleComplexMatrix;
import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.DoubleNumber;
import org.mklab.nfc.scalar.RealNumericalScalar;


/**
 * ゴピナスの方法を用いて最小次元オブザーバを求めるクラスです。
 * 
 * <p>Minimal order observer by Gopinath's method
 * 
 * @author koga
 * @version $Revision: 1.31 $
 * @see org.mklab.tool.control.Pplace
 */
public class Obsg {
  /**
   * システム
   * 
   * <pre><code>
   *  dx = Ax + Bu
   *  y = Cx
   *  </code></pre>
   * 
   * について、ゴピナスの方法で設計した最少次オブザーバ
   * 
   * <pre><code>
   * dz = Ah*z + Bh*y + Jh*u
   * xh = Ch*z + Dh*y
   *  </code></pre>
   * 
   * の係数行列を返します。ただし、オブザーバの極は <code>poles</code>です。
   * 
   * @param A システム行列A
   * @param B システム行列B
   * @param C システム行列C
   * @param poles オブザーバの極
   * @return {Ah, Bh, Ch, Dh, Jh } オブザーバのシステム行列
   */
  public static List<DoubleMatrix> obsg(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleComplexMatrix poles) {
    final int n = A.getRowSize();
    final int m = B.getColumnSize();
    final int p = C.getRowSize();
    final int r = n - p;
    
    if ( C.rank() == n) {
      throw new  IllegalArgumentException("Obsg: " + Messages.getString("Obsg.2")); //$NON-NLS-1$ //$NON-NLS-2$
    }
    
    if (r != poles.length()) {
      throw new  IllegalArgumentException("Obsg: " +  Messages.getString("Obsg.3")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    final DoubleMatrix Tinv = C.kernel().transpose().appendDown(C);
    final DoubleMatrix T = Tinv.inverse();
    final DoubleMatrix Ab = Tinv.multiply(A).multiply(T);
    final DoubleMatrix Bb = Tinv.multiply(B);
    // DoubleMatrix Cb = C.multiply(T);
    final DoubleMatrix Ab11 = Ab.getSubMatrix(1, r, 1, r);
    final DoubleMatrix Ab12 = Ab.getSubMatrix(1, r, r + 1, n);
    final DoubleMatrix Ab21 = Ab.getSubMatrix(r + 1, n, 1, r);
    final DoubleMatrix Ab22 = Ab.getSubMatrix(r + 1, n, r + 1, n);
    final DoubleMatrix Bb1 = Bb.getSubMatrix(1, r, 1, m);
    final DoubleMatrix Bb2 = Bb.getSubMatrix(r + 1, n, 1, m);

    final DoubleMatrix L = Pplace.pplace(Ab11.transpose(), Ab21.transpose().unaryMinus(), poles).transpose().unaryMinus();

    final DoubleMatrix U = L.createUnit(r).appendRight(L.unaryMinus()).multiply(Tinv);
    final DoubleMatrix Ah = Ab11.subtract(L.multiply(Ab21));
    final DoubleMatrix Bh = Ah.multiply(L).add(Ab12).subtract(L.multiply(Ab22));
    final DoubleMatrix Ch = T.multiply(T.createUnit(r).appendDown(T.createZero(p, r)));
    final DoubleMatrix Dh = T.multiply(L.appendDown(A.createUnit(p)));
    final DoubleMatrix Jh = Bb1.subtract(L.multiply(Bb2));

    final DoubleNumber unit = A.getElement(1, 1);
    final DoubleNumber eps = unit.getMachineEpsilon().multiply(100);

    if ((((Ch.multiply(U).add(Dh.multiply(C).subtract(Ch.createUnit(Ch.getRowSize(), Ch.getRowSize()))))).absElementWise()).max().isGreaterThan(eps)) {
      throw new RuntimeException("Obsg: " + Messages.getString("Obsg.0")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Ah, Bh, Ch, Dh, Jh}));
    //return new MatxList(new Object[] {Ah, Bh, Ch, Dh, Jh});
  }

  /**
   * システム行列<code>A,B,C</code>から最小次元オブザーバを設計します。
   * 
   * <p>この時重み行列<code>Q,R</code>の値を入力しなければならない。(未完成) 最小次元オブザーバとは
   * 
   * <pre><code>
   * dz = Ah*z + Bh*y + Jh*u
   * xh = Ch*z + Dh*y
   * </code></pre>
   * 
   * のように表されます。
   * 
   * @param A システム行列A
   * @param B システム行列B
   * @param C システム行列C
   * @return {Ah, Bh, Ch, Dh, Jh } オブザーバのシステム行列
   */
  public static List<DoubleMatrix> obsg(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C) {
    final int n = A.getRowSize();
    final int p = C.getRowSize();
    final int r = n - p;
    
    if ( C.rank() == n) {
      throw new  IllegalArgumentException("Obsg: " + Messages.getString("Obsg.2")); //$NON-NLS-1$ //$NON-NLS-2$
    }
    
    final DoubleMatrix Tinv = C.kernel().transpose().appendDown(C);
    final DoubleMatrix T = Tinv.inverse();
    final DoubleMatrix Ab = Tinv.multiply(A).multiply(T);
    final DoubleMatrix Bb = Tinv.multiply(B);
    // DoubleMatrix Cb = C.multiply(T);
    final DoubleMatrix Ab11 = Ab.getSubMatrix(1, r, 1, r);
    final DoubleMatrix Ab12 = Ab.getSubMatrix(1, r, r + 1, n);
    final DoubleMatrix Ab21 = Ab.getSubMatrix(r + 1, n, 1, r);
    final DoubleMatrix Ab22 = Ab.getSubMatrix(r + 1, n, r + 1, n);
    final DoubleMatrix Bb1 = Bb.getSubMatrix(1, r, 1, 0);
    final DoubleMatrix Bb2 = Bb.getSubMatrix(r + 1, n, 1, 0);

    final DoubleMatrix Q = A.createUnit(Ab11.getRowSize(), Ab11.getColumnSize());
    final DoubleMatrix R = A.createUnit(Ab21.getRowSize(), Ab21.getRowSize());
    // edit Q, R;
    final List<DoubleMatrix> tmp = Lqr.lqr(Ab11.transpose(), Ab21.transpose().unaryMinus(), Q, R);
    DoubleMatrix L = tmp.get(0);
    L = L.unaryMinus().transpose();

    final DoubleMatrix U = L.createUnit(r).appendRight(L.unaryMinus()).multiply(Tinv);
    final DoubleMatrix Ah = Ab11.subtract(L.multiply(Ab21));
    final DoubleMatrix Bh = Ah.multiply(L).add(Ab12).subtract(L.multiply(Ab22));
    final DoubleMatrix Ch = T.multiply(T.createUnit(r).appendDown(T.createZero(p, r)));
    final DoubleMatrix Dh = T.multiply(L.appendDown(L.createUnit(p)));
    final DoubleMatrix Jh = Bb1.subtract(L.multiply(Bb2));

    final DoubleNumber unit = A.getElement(1, 1);
    final DoubleNumber eps = unit.getMachineEpsilon().multiply(100);

    if ((((Ch.multiply(U).add(Dh.multiply(C).subtract(A.createUnit(Ch.getRowSize(), Ch.getRowSize()))))).absElementWise()).max().isGreaterThan(eps)) {
      throw new RuntimeException("Obsg: " + Messages.getString("Obsg.1")); //$NON-NLS-1$ //$NON-NLS-2$

    }

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {Ah, Bh, Ch, Dh, Jh}));
    //return new MatxList(new Object[] {Ah, Bh, Ch, Dh, Jh});
  }

  
  /**
   * システム
   * 
   * <pre><code>
   *  dx = Ax + Bu
   *  y = Cx
   *  </code></pre>
   * 
   * について、ゴピナスの方法で設計した最少次オブザーバ
   * 
   * <pre><code>
   * dz = Ah*z + Bh*y + Jh*u
   * xh = Ch*z + Dh*y
   *  </code></pre>
   * 
   * の係数行列を返します。ただし、オブザーバの極は <code>poles</code>です。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列A
   * @param B システム行列B
   * @param C システム行列C
   * @param poles オブザーバの極
   * @return {Ah, Bh, Ch, Dh, Jh } オブザーバのシステム行列
   */
  public static  <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>> List<RM> obsg(RM A, RM B, RM C, CM poles) {
    final int n = A.getRowSize();
    final int m = B.getColumnSize();
    final int p = C.getRowSize();
    final int r = n - p;
    
    if ( C.rank() == n) {
      throw new  IllegalArgumentException("Obsg: " + Messages.getString("Obsg.2")); //$NON-NLS-1$ //$NON-NLS-2$
    }
    
    if (r != poles.length()) {
      throw new  IllegalArgumentException("Obsg: " +  Messages.getString("Obsg.3")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    final RM Tinv = C.kernel().transpose().appendDown(C);
    final RM T = Tinv.inverse();
    final RM Ab = Tinv.multiply(A).multiply(T);
    final RM Bb = Tinv.multiply(B);
    // RM Cb = C.multiply(T);
    final RM Ab11 = Ab.getSubMatrix(1, r, 1, r);
    final RM Ab12 = Ab.getSubMatrix(1, r, r + 1, n);
    final RM Ab21 = Ab.getSubMatrix(r + 1, n, 1, r);
    final RM Ab22 = Ab.getSubMatrix(r + 1, n, r + 1, n);
    final RM Bb1 = Bb.getSubMatrix(1, r, 1, m);
    final RM Bb2 = Bb.getSubMatrix(r + 1, n, 1, m);

    final RM L = Pplace.pplace(Ab11.transpose(), Ab21.transpose().unaryMinus(), poles).transpose().unaryMinus();

    final RM U = L.createUnit(r).appendRight(L.unaryMinus()).multiply(Tinv);
    final RM Ah = Ab11.subtract(L.multiply(Ab21));
    final RM Bh = Ah.multiply(L).add(Ab12).subtract(L.multiply(Ab22));
    final RM Ch = T.multiply(T.createUnit(r).appendDown(T.createZero(p, r)));
    final RM Dh = T.multiply(L.appendDown(A.createUnit(p)));
    final RM Jh = Bb1.subtract(L.multiply(Bb2));

    RS sunit = A.getElement(1,1).createUnit();
    final RS eps = sunit.getMachineEpsilon().multiply(100);

    if ((((Ch.multiply(U).add(Dh.multiply(C).subtract(Ch.createUnit(Ch.getRowSize(), Ch.getRowSize()))))).absElementWise()).max().isGreaterThan(eps)) {
      throw new RuntimeException("Obsg: " + Messages.getString("Obsg.0")); //$NON-NLS-1$ //$NON-NLS-2$
    }

    final List<RM> observer = new ArrayList<>();
    observer.add(Ah);
    observer.add(Bh);
    observer.add(Ch);
    observer.add(Dh);
    observer.add(Jh);
    return observer;
  }

  /**
   * システム行列<code>A,B,C</code>から最小次元オブザーバを設計します。
   * 
   * <p>この時重み行列<code>Q,R</code>の値を入力しなければならない。(未完成) 最小次元オブザーバとは
   * 
   * <pre><code>
   * dz = Ah*z + Bh*y + Jh*u
   * xh = Ch*z + Dh*y
   * </code></pre>
   * 
   * のように表されます。
   * 
   * @param <RS> スカラーの型
   * @param <RM> 行列の型
   * @param <CS> 複素スカラーの型
   * @param <CM> 複素行列の型
   * @param A システム行列A
   * @param B システム行列B
   * @param C システム行列C
   * @return {Ah, Bh, Ch, Dh, Jh } オブザーバのシステム行列
   */
  public static <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>>List<RM> obsg(RM A, RM B, RM C) {
    final int n = A.getRowSize();
    final int p = C.getRowSize();
    final int r = n - p;
    
    if ( C.rank() == n) {
      throw new  IllegalArgumentException("Obsg: " + Messages.getString("Obsg.2")); //$NON-NLS-1$ //$NON-NLS-2$
    }
    
    final RM Tinv = C.kernel().transpose().appendDown(C);
    final RM T = Tinv.inverse();
    final RM Ab = Tinv.multiply(A).multiply(T);
    final RM Bb = Tinv.multiply(B);
    // RM Cb = C.multiply(T);
    final RM Ab11 = Ab.getSubMatrix(1, r, 1, r);
    final RM Ab12 = Ab.getSubMatrix(1, r, r + 1, n);
    final RM Ab21 = Ab.getSubMatrix(r + 1, n, 1, r);
    final RM Ab22 = Ab.getSubMatrix(r + 1, n, r + 1, n);
    final RM Bb1 = Bb.getSubMatrix(1, r, 1, 0);
    final RM Bb2 = Bb.getSubMatrix(r + 1, n, 1, 0);

    final RM Q = A.createUnit(Ab11.getRowSize(), Ab11.getColumnSize());
    final RM R = A.createUnit(Ab21.getRowSize(), Ab21.getRowSize());
    // edit Q, R;
    final List<RM> tmp = Lqr.lqr(Ab11.transpose(), Ab21.transpose().unaryMinus(), Q, R);
    RM L = tmp.get(0);
    L = L.unaryMinus().transpose();

    final RM U = L.createUnit(r).appendRight(L.unaryMinus()).multiply(Tinv);
    final RM Ah = Ab11.subtract(L.multiply(Ab21));
    final RM Bh = Ah.multiply(L).add(Ab12).subtract(L.multiply(Ab22));
    final RM Ch = T.multiply(T.createUnit(r).appendDown(T.createZero(p, r)));
    final RM Dh = T.multiply(L.appendDown(L.createUnit(p)));
    final RM Jh = Bb1.subtract(L.multiply(Bb2));

    RS sunit = A.getElement(1,1).createUnit();
    final RS eps = sunit.getMachineEpsilon().multiply(100);

    if ((((Ch.multiply(U).add(Dh.multiply(C).subtract(A.createUnit(Ch.getRowSize(), Ch.getRowSize()))))).absElementWise()).max().isGreaterThan(eps)) {
      throw new RuntimeException("Obsg: " + Messages.getString("Obsg.1")); //$NON-NLS-1$ //$NON-NLS-2$

    }

    final List<RM> observer = new ArrayList<>();
    observer.add(Ah);
    observer.add(Bh);
    observer.add(Ch);
    observer.add(Dh);
    observer.add(Jh);
    return observer;
  }
}