MATLAB/SimulinkでS-FunctionをMEXで作成

SimulinkのS-FunctionをC言語やC++で作りたいときの資料があまり存在しないのでメモ．

たとえばシステムが線形で $\frac{dx}{dt} = Ax+Bu$ および $y = Cx+Du$ の形をしているとき，これをMEXでブロックにしたければ，下記のように書く（csfuncを参考）． Simulinkとのインターフェース部（test.cpp）と，運動方程式および出力方程式（motion.h，motion.cpp）の部分を切り分けている． C++での行列計算はEigenを導入するのが楽だが，速度に問題があればBLASに切り替える．

// test.cpp
#define S_FUNCTION_NAME test
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include "motion.h"
#include "Eigen/Dense"

#define U(element) (*uPtrs[element])  // Pointer to Input Port0

const int stateNum  = 2;
const int inputNum  = 2;
const int outputNum = 2;

// Function: mdlInitializeSizes ===============================================
// Abstract:
//    The sizes information is used by Simulink to determine the S-function
//    block's characteristics (number of inputs, outputs, states, etc.).
// ============================================================================
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, 0);   // Number of expected parameters
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return;                 // Parameter mismatch will be reported by Simulink
    }

    ssSetNumContStates(S, stateNum);    // Specify the number of continuous states that a block has
    ssSetNumDiscStates(S, 0);           // Specify the number of discrete states that a block has

    if (!ssSetNumInputPorts(S, 1)) {    // Specify the number of input ports that a block has
        return;
    }
    ssSetInputPortWidth(S, 0, inputNum);        // Specify the width of an input port
    ssSetInputPortDirectFeedThrough(S, 0, 1);   // Specify the direct feedthrough status of a block's ports

    if (!ssSetNumOutputPorts(S, 1)) {           // Specify the number of output ports that a block has
        return;
    }
    ssSetOutputPortWidth(S, 0, outputNum);      // Specify the width of an output port (int_T port, int_t, width)

    ssSetNumSampleTimes(S, 1);      // Specify the number of sample times that an S-Function block has
    ssSetNumRWork(S, 0);            // Specify the size of a block's floating-point work vector
    ssSetNumIWork(S, 0);            // Specify the size of a block's integer work vector
    ssSetNumPWork(S, 0);            // Specify the size of a block's pointer work vector
    ssSetNumModes(S, 0);            // Specify the size of the block's mode vector
    ssSetNumNonsampledZCs(S, 0);    // Specify the number of states for which a block detects zero crossings that occur between sample points
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
        // Specify the behavior of a Simulink S-function when saving and restoring the SimState of a model containing the S-function.

    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE); // Specify S-function options
}

// Function: mdlInitializeSampleTimes =========================================
// Abstract:
//    Specifiy that we have a continuous sample time.
// ============================================================================
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);      // Set the period of a sample time
    ssSetOffsetTime(S, 0, 0.0);                         // Set the offset time of a block
    ssSetModelReferenceSampleTimeDefaultInheritance(S);
        // Specify that a referenced model containing this S-function can inherit its sample time from its parent model
}

// Function: mdlInitializeConditions ==========================================
// Abstract:
//    Initialize all continuous states to zero.
// ============================================================================
#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
    real_T *x0 = ssGetContStates(S);    // Get a block's continuous states
    int_T  ii;

    for (ii = 0; ii < stateNum; ii++) { 
        *x0++ = 0.0; 
    }
}

// Function: mdlDerivatives ===================================================
// Abstract:
//      xdot = Ax + Bu
// ============================================================================
#define MDL_DERIVATIVES
static void mdlDerivatives(SimStruct *S)
{
    real_T            *dx   = ssGetdX(S);   // Get the derivatives of a block's continuous states
    real_T            *x    = ssGetContStates(S);   // Get a block's continuous states
    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);    // Get pointers to signals of type double connected to an input port

    //cast to Eigen vector
    Eigen::VectorXd dxV(2);
    Eigen::VectorXd xV(2);
    Eigen::VectorXd uV(2);
    xV << x[0], x[1];
    uV << U(0), U(1);
    
    // xdot = Ax + Bu
    calcDerivatives(dxV, xV, uV);
    
    // retrieval
    dx[0] = dxV(0);
    dx[1] = dxV(1);
}

// Function: mdlOutputs =======================================================
// Abstract:
//      y = Cx + Du 
// ============================================================================
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T            *y    = ssGetOutputPortRealSignal(S,0);   // Get a pointer to an output signal of type double (real_T)
    real_T            *x    = ssGetContStates(S);   // Get a block's continuous states
    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
        //Get pointers to signals of type double connected to an input port
 
    UNUSED_ARG(tid);    // not used in single tasking mode

    //cast to Eigen vector
    Eigen::VectorXd yV(2);
    Eigen::VectorXd xV(2);
    Eigen::VectorXd uV(2);
    xV << x[0], x[1];
    uV << U(0), U(1);
    
    // y = Cx + Du
    calcOutputs(yV, xV, uV);

    // retrieval
    y[0] = yV(0);
    y[1] = yV(1);    
}

// Function: mdlTerminate =====================================================
// Abstract:
//    No termination needed, but we are required to have this routine.
// ============================================================================
static void mdlTerminate(SimStruct *S)
{
    UNUSED_ARG(S);  // unused input argument
}

#ifdef  MATLAB_MEX_FILE     // Is this file being compiled as a MEX-file?
#include "simulink.c"       // MEX-file interface mechanism
#else
#include "cg_sfun.h"        // Code generation registration function
#endif

// motion.h
#ifndef __MOTION_H__
#define __MOTION_H__

#define S_FUNCTION_NAME test
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include "Eigen/Dense"

void calcDerivatives(Eigen::VectorXd& dx_, Eigen::VectorXd& x_, Eigen::VectorXd& u_);
void calcOutputs(Eigen::VectorXd& y_, Eigen::VectorXd& x_, Eigen::VectorXd& u_);

#endif

// motion.cpp
#include "motion.h"

void calcDerivatives(Eigen::VectorXd& dx_, Eigen::VectorXd& x_, Eigen::VectorXd& u_)
{

    Eigen::MatrixXd A(2, 2);
    Eigen::MatrixXd B(2, 2);
    
    A <<  1.0, 2.0, 3.0, 4.0;
    B <<  5.0, 6.0, 7.0, 8.0;

    dx_ = A * x_ + B * u_;
}

void calcOutputs(Eigen::VectorXd& y_, Eigen::VectorXd& x_, Eigen::VectorXd& u_)
{
    Eigen::MatrixXd C(2, 2);
    Eigen::MatrixXd D(2, 2);
    
    C <<  8.0, 7.0, 6.0, 5.0;
    D <<  4.0, 3.0, 2.0, 1.0;

    y_ = C * x_ + D * u_;
}

以上のファイルをmexコマンドでコンパイル．"test.mex***"というファイルが出来上がる（中身はDLL）．"-largeArrayDims"オプションは今後必須とのこと．"-v"は詳細表示，"-g"はデバッグ用．ライブラリブラウザを表示させたければ"simulink"コマンドを実行．

% compile.m
clear;

mex -v -largeArrayDims test.cpp motion.cpp
% mex -g -largeArrayDims test.cpp motion.cpp % debug

simulink('open');   % launch library browser
main;   % open main.slx

モデル（main.slk）は下図のようなものを作成．mexで作成したライブラリは"User-Defined Functions"から"S-Function"を選んで"test"を指定． f:id:hajimesakurai:20150821221125p:plain 実行すると問題なく動作した．