Lqe.java

/*
 * $Id: Lqe.java,v 1.12 2008/03/23 15:08:16 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>Continuous-time linear quadratic estimator
 * 
 * @author koga
 * @version $Revision: 1.12 $
 * @see org.mklab.tool.control.Lqr
 * @see org.mklab.tool.control.Dlqe
 */
public class Lqe {

  /**
   * 連続時間システム
   * 
   * <pre><code> . x = Ax + Bu + Gw z = Cx + Du + v </code></pre>
   * 
   * について、プロセス雑音と観測雑音の分散が
   * 
   * <pre><code> E[w] = E[v] = 0, E[ww#] = Q, E[vv#] = R </code></pre>
   * 
   * であるとします。 状態<code>x</code>のLQG最適推定を生成するカルマンフィルター
   * 
   * <pre><code> . x = Ax + Bu + L(z - Hx - Du) </code></pre>
   * 
   * のゲイン行列<code>L</code>を返します。
   * 
   * <p> また、リカッティ方程式の解<code>P</code>(推定誤差の分散) <code>P</code>を返します。
   * 
   * @param A システム行列
   * @param B 入力行列
   * @param C 出力行列
   * @param Q 状態雑音の分散
   * @param R 観測雑音の分散
   * @return {L, P} estimator
   */
  public static List<DoubleMatrix> lqe(DoubleMatrix A, DoubleMatrix B, DoubleMatrix C, DoubleMatrix Q, DoubleMatrix R) {
    List<DoubleMatrix> LP = Lqr.lqr(A.conjugateTranspose(), C.conjugateTranspose(), B.multiply(Q).multiply(B.conjugateTranspose()), R);
    DoubleMatrix L = LP.get(0).conjugateTranspose();
    DoubleMatrix P = LP.get(1).conjugateTranspose();
    return new ArrayList<>(Arrays.asList(new DoubleMatrix[] {L, P}));
  }

}