C++とEigenで状態フィードバックの応答を求める。

目的

C++言語とEigenとで、状態フィードバックの応答を求める。

状態フィードバックの応答は、別途、
Python Controlで、状態フィードバックのゲインを求める。
で求めておくとする。

事前準備

Eigenをインストールしておく。

状態フィードバックとは

状態フィードバックとは、状態方程式で表したシステムにて

\dot{x}(t)=Ax(t)+Bu(t) \\
y(t)=Cx(t)+Du(t)

状態変数xをフィードバックする制御システムである。

u(t)=-Kx(t)

実装

Python Controlで、状態フィードバックのゲインを求める。
もしくは、Matlab・scilabなどツールより、フィードバックゲインKが求められているとすると、

C++とEigenで状態方程式を解く。の
時系列応答を求めるループに、u(t)=-Kx(t)を足せばよい。

    for (int i = 0; i < 1000; i++) {
        RungeKutta(dX, X, u, tt, dt, A, B, C, D);
        Y = C*X;
        u = -K*X;
        cout << tt << "," << u(0, 0) << "," << Y(0, 0) << endl;
        tt += dt;
    }

サンプルソース

#include "Eigen/Dense"
#include "Eigen/Core"
#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES

#include <math.h>
using namespace Eigen;
using namespace std;

void RungeKutta(MatrixXd dX, MatrixXd &X, MatrixXd u, double tt, double dt, MatrixXd A, MatrixXd B, MatrixXd C, MatrixXd D);


int main() {
    // Parameters defining the system
    double m = 250.0;         // system mass
    double  k = 40.0;          // spring constant
    double  b = 60.0;          // damping constant

    MatrixXd A(3, 3);
    A << 1, -1, 1, 1, -k / m, -b / m, 1, 1, 1;
    MatrixXd B(3, 1);
    B << 0, -1 / m, 1;

    MatrixXd C(1, 3);
    C << 1, 0, 1;

    MatrixXd D(1, 3);
    D << 1, 0, 0;

    MatrixXd K(1, 3);       //FeedBask Gain
    K << 15.80864382, -3.63298165, 7.85453193;

    double dt = 0.01;
    double tt = 0.0;
    MatrixXd X(3, 1);
    X << 10, 0, 0;
    MatrixXd dX(3, 1);
    dX << 0, 0, 0;

    MatrixXd u(1, 1);
    u << 0;
    MatrixXd Y(1, 1);
    Y << 0;

    ofstream ofs("test.csv");
    ofs << "time," << ",u,y,x0,x1,x2" << endl;
    for (int i = 0; i < 1000; i++) {
        RungeKutta(dX, X, u, tt, dt, A, B, C, D);
        Y = C*X;
        u = -K*X;
        ofs << tt << "," << u(0, 0) << "," << Y(0, 0) << "," << X(0, 0) << "," << X(1, 0) << "," << X(2, 0) << endl;
        tt += dt;
    }
    return 0;

}

以下のように、フィードバック制御ができていることを確認する。