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

Riccati方程式を数値的に解く

最適制御を学んだ人なら、Riccati代数方程式を解くことは日常茶飯事だと思う。LQR的に定義されたJ値を最小化するための最適制御入力に必要だからだ。もちろんMatlabにはその求解関数が用意されていて、一行で実行できる。
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

テスト問題を解かせて正しく振る舞うことを検証済。