Butterfly_Effect( )

僕はとにかく地面と別れる方法を探しているんだ

C++&EigenでRiccati方程式の解を求める

Riccati方程式を数値的に解く

最適制御を学んだことがある人なら、Riccati代数方程式を解くことの重要性が分かると思う。このプロセスは、LQR的に定義されたJ値を最小化するための最適制御入力の構成に必要になる。言うまでもなく、Matlab様にはRiccati代数方程式の求解関数が用意されていて、誰でも一行で実行できる。
https://jp.mathworks.com/help/control/linear-algebra-for-control-design.html

でも僕はMatlabを持っていないので、C++で書いてみることにした。
数値解法には有本/ポッター法を用いる。行列の固有値固有ベクトルを計算する問題に落とし込めるので明快だ。
コーディングと検証にあたっては以下のページを参考にした。
http://www.maizuru-ct.ac.jp/control/kawata/study/book3/matlab/chap08/doc/sec842.html

C++ Code

#include <iostream>
#include <vector>
#include "Eigen/Dense"

using namespace std;
using namespace Eigen;

//リッカチ方程式を解く
MatrixXd RiccatiSolver(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R){
    
    int n = (int)A.rows();
    
    //ハミルトン行列を生成する
    MatrixXd H(2*n, 2*n);
    H << A, -B*R.inverse()*B.transpose(), -Q, -A.transpose();
    
    //固有値と固有ベクトルを求める
    EigenSolver<Matrix<double,4,4>> es(H);
    if (es.info() != Success) abort();
    
    //ベクトルセットu,vを書き出す
    vector<VectorXd> v_set, u_set;
    for(int i=0;i<2*n;i++){
        if (es.eigenvalues().real()(i) < 0){
            v_set.push_back( es.eigenvectors().real().block(0,i,n,1) );
            u_set.push_back( es.eigenvectors().real().block(n,i,n,1) );
        }
    }
    
    int num = (int)v_set.size();
    MatrixXd v(n,num), u(n,num);
    for(int i=0;i<num;i++){
        v.block(0,i,n,1) = v_set[i];
        u.block(0,i,n,1) = u_set[i];
    }
    
    //解Pを求める
    MatrixXd P = u * v.inverse();
    return P;
}

int main(int argc, const char * argv[]) {
    MatrixXd A(2,2), B(2,1), R(1,1), Q(2,2);
    A << 0, 1, -10, -1;
    B << 0, 1;
    Q << 300, 0, 0, 60;
    R << 1;

    MatrixXd P = RiccatiSolver(A,B,Q,R);
    cout << "P : " << endl << P << endl;
    return EXIT_SUCCESS;
}

実行結果

P : 
170  10
 10   8
Program ended with exit code: 0

参照したページと照らし合わせて、正確に計算できていることが確認できた。