C++とEigenで状態方程式を解いてopenFrameworksで表示する。

19804 ワード
C++ 制御工学 openFrameworks ルンゲクッタ法 Eigen C++ テキストリンク

目的

C++言語＋Eigenとで、バネ～マス～ダンパ系の状態方程式の応答を求め、
openFrameworksのaddon ofxGraphで応答波形をリアルタイムに表示する。
また、ofxGuiでバネ～マス～ダンパのパラメータをリアルタイムに変更できるようにする。

参考：C++とEigenで状態方程式を解く。

　

事前準備

以下をインストールしておく。
・openFrameworks
　→上記ページからファイルをダウンロードし、任意のフォルダに展開する。
・ofxGraph
　→上記ページからファイルをダウンロードし、openFrameworksのaddonディレクトリ以下に配置する。
・Eigen
→上記ページからヘッダファイルをダウンロードし、openFrameworksのプロジェクトのsrcディレクトリ以下に配置する。

実装

以下のバネ～マス～ダンパ系の状態方程式を解くとする。
u(t)には外乱としてホワイトノイズを注入する。

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

x(t)=
\begin{bmatrix}
x_{1}  \\
x_{2}  
\end{bmatrix}

,
u(t)=f
,

A=
\begin{bmatrix}
0 & 1 \\
-k/m & -c/m
\end{bmatrix}
,
B=
\begin{bmatrix}
0 \\
1/m 
\end{bmatrix}
,
C=
\begin{bmatrix}
1 & 0 
\end{bmatrix}
,
D=[0]

サンプルソース

ofApp.cpp

#include "ofApp.h"

void RungeKutta(MatrixXd dX, MatrixXd &X, MatrixXd u, double tt, double dt, MatrixXd A, MatrixXd B, MatrixXd C, MatrixXd D) {
    MatrixXd k1 = A*X + B*u;
    MatrixXd k2 = A*(X + 0.5*k1*dt) + B*u;
    MatrixXd k3 = A*(X + 0.5*k2*dt) + B*u;
    MatrixXd k4 = A*(X + k3*dt) + B*u;
    MatrixXd k = (k1 + 2.0*k2 + 2.0*k3 + k4)*dt / 6.0;
    X = X + k;
}
void ofApp::setup(){
    ofSetVerticalSync(true); 
    ofxGuiSetFont(ofToDataPath("ofxGraph/DIN Alternate Bold.ttf"), 10);
    graph.setup(100, 100, 800, 400);
    graph.setName("spring-mass-damper system wave"); 
    graph.setDx(2.0);
    graph.setColor(ofColor::white); 
    graph.setBufSize(1000);  
    graph.setLabel({ "y0","u" });
    frame = 0;

    gui.setup("parametor");
    gui.add(kk.setup("K", 1.0, 0.0, 5.0));
    gui.add(mm.setup("M", 0.1, 0.0, 1.0));
    gui.add(cc.setup("C", 0.1, 0.0, 1.0));

    AA.resize(2, 2);
    AA << 0, 1, -kk / mm, -cc / mm;
    BB.resize(2, 1);
    BB << 1, 1 / mm;
    CC.resize(1, 2);
    CC << 1, 0;
    DD.resize(1, 1);
    DD << 1;

    XX.resize(2, 1);
    XX(0, 0) = 0;
    XX(1, 0) = 0;
    dXX.resize(2, 1);
    dXX(0, 0) = 0;
    dXX(1, 0) = 0;

    uu.resize(1, 1);
    uu << 0;
    YY.resize(1, 1);
    YY(0, 0) = 0;

    dt = 0.01;
    tt = 0.0;
}

//--------------------------------------------------------------
void ofApp::update() {
    frame++;

    AA << 0, 1, -kk / mm, -cc / mm;
    BB << 1, 1 / mm;

    double TT = 100.0;
    uu(0, 0) = ofRandom(-1,1);

    RungeKutta(dXX, XX, uu, tt, dt, AA, BB, CC, DD);
    YY = CC*XX;
    tt += dt;

    vector<float>value;
    value.push_back(YY(0, 0));
    value.push_back(uu(0,0));
    graph.add(value);

}

void ofApp::draw(){
    ofBackground(50, 50, 50);
    gui.draw();
    graph.draw();
}

ofApp.h

#include "ofMain.h"
#include "ofxGraph.h"
#include "ofxGui.h"

#define _USE_MATH_DEFINES
#include <math.h>

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

using namespace Eigen;
using namespace std;

class ofApp : public ofBaseApp{
    public:
        void setup();
        void update();
        void draw();
        ofxGraph graph;
        long frame;
        ofxPanel gui;
        ofxFloatSlider kk,mm,cc;
        MatrixXd AA;
        MatrixXd BB; 
        MatrixXd CC;
        MatrixXd DD;
        MatrixXd XX;
        MatrixXd dXX;
        MatrixXd uu;
        MatrixXd YY;
        double dt ;
        double tt ;
};

ソースは以下にも格納済み。
https://github.com/nnn112358/180217_system_control