Dlqe.java

/*
 * $Id: Dlqe.java,v 1.10 2008/03/23 14:29:08 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.DoubleMatrix;


/**
 * 離散時間系のLQEを求めるクラスです。
 * 
 * <p>Discrete-time linear quadratic estimator
 * 
 * @author koga
 * @version $Revision: 1.10 $
 * @see org.mklab.tool.control.Lqe
 * @see org.mklab.tool.control.Dlqr
 */
public class Dlqe {

  /**
   * 離散時間システム
   * 
   * <pre><code> x[n+1] = Ax[n] + Bu[n] + Gw[n] z[n] = Cx[n] + Du[n] + v[n] </code></pre>
   * 
   * について、プロセス雑音と観測雑音の分散が
   * 
   * <pre><code> E[w] = E[v] = 0, E[ww'] = Q, E[vv'] = R </code></pre>
   * 
   * であるとします。
   * 
   * <p> 状態<code>x</code>のLQG最適推定を生成するカルマンフィルター
   * 
   * <pre><code>
   * 
   * xb[n+1] = A xh[n] + B u[n] xh[n] = xb[n] + L(z[n] - H xb[n] - D u[n]) </code></pre>
   * 
   * のゲイン行列<code>L</code>を返します。
   * 
   * <p> また、リカッティ方程式の解<code>M</code>と推定誤差の分散 <code>P</code>を返します。
   * 
   * @param A A行列
   * @param G G行列
   * @param C C行列
   * @param Q 状態外乱の分散
   * @param R 観測外乱の分散
   * @return {L, M, P} Linear quadratic estimator
   */
  public static List<DoubleMatrix> dlqe(DoubleMatrix A, DoubleMatrix G, DoubleMatrix C, DoubleMatrix Q, DoubleMatrix R) {
    List<DoubleMatrix> tmp = Dlqr.dlqr(A.conjugateTranspose(), C.conjugateTranspose(), G.multiply(Q).multiply(G.conjugateTranspose()), R);
    // DoubleMatrix K = tmp.getMatrix(0);
    DoubleMatrix S = tmp.get(1);
    DoubleMatrix M = S.conjugateTranspose();
    DoubleMatrix L = S.multiply(C.conjugateTranspose()).divide(C.multiply(S).multiply(C.conjugateTranspose()).add(R));
    DoubleMatrix P = A.leftDivide(M.subtract(G.multiply(Q).multiply(G.conjugateTranspose()))).divide(A.conjugateTranspose());

    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {L, M, P}));
    //return new MatxList(new Object[] {L, M, P});
  }

}