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
テスト問題を解かせて正しく振る舞うことを検証済。