PreviousNext

A System Model in C++

After learning the basic principles of writing a MEX file with the handy Eigen wrappers provided by the Nonlinear Estimation Toolbox, let's program a first useful functionality in C++. We start with a general system model/measurement model. Implementing such a model in C++ requires two parts:

  • the usual MATLAB class that inherits from the system model/measurement model to be implemented and

  • the actual MEX file that implements the systemEquation()/measurementEquation() method.

Here, we consider the nonlinear system model from the Getting Started guide. Although this model can be written easily in MATLAB, it serves well as an illustrative example for a C++ implementation. First, we write the required MATLAB class:

TargetSysModelMex.m

classdef TargetSysModelMex < SystemModel
    methods
        function obj = TargetSysModelMex()
            obj.deltaT = 0.1;
            
            sysNoise = Gaussian(zeros(2, 1), [1e-1 1e-3]);
            
            obj.setNoise(sysNoise);
        end
        
        function predictedStates = systemEquation(obj, stateSamples, noiseSamples)
            predictedStates = targetSystemEquation(stateSamples, noiseSamples, obj.deltaT);
        end
    end
    
    properties (Access = 'private')
        deltaT;
    end
end

The only significant difference compared to the original TargetSysModel.m file is that the systemEquation() code is moved into a MEX file called targetSystemEquation(). Note also that we pass the step size \(\Delta t\) as a parameter. In doing so, we can change it from MATLAB without recompiling the MEX file. Next, we turn to the crucial part: the C++ implementation.

targetSystemEquation.cpp

#include <Mex/Mex.h>

void mexFunction(int numOutputs, mxArray *outputs[],
                 int numInputs, const mxArray *inputs[])
{
    try {
        // Check for proper number of arguments
        if (numInputs != 3) {
            throw std::invalid_argument("Three inputs are required.");
        }
        
        if (numOutputs != 1) {
            throw std::invalid_argument("One output is required.");
        }
        
        // First parameter contains the state samples
        Mex::ConstMatrix<double, 5, Eigen::Dynamic> stateSamples(inputs[0]);
        
        // Second parameter contains the noise samples
        Mex::ConstMatrix<double, 2, Eigen::Dynamic> noiseSamples(inputs[1]);
        
        // Check for the same number of state samples and noise samples
        if (stateSamples.cols() != noiseSamples.cols()) {
            throw std::invalid_argument("Different number of samples.");
        }
        
        // Third parameter is the discrete time step
        const double deltaT = mxGetScalar(inputs[2]);
        
        // Allocate memory for predicted state samples
        const int numSamples = stateSamples.cols();
        
        Mex::OutputMatrix<double, 5, Eigen::Dynamic> predictedStates(5, numSamples);
        
        // Compute a predicted state sample in each iteration
        for (int i = 0; i < numSamples; ++i) {
            const double px       = stateSamples(0, i);
            const double py       = stateSamples(1, i);
            const double dir      = stateSamples(2, i);
            const double speed    = stateSamples(3, i);
            const double turnRate = stateSamples(4, i);
            
            const double speedNoise    = noiseSamples(0, i);
            const double turnRateNoise = noiseSamples(1, i);
            
            const double predSpeed    = speed + speedNoise;
            const double predTurnRate = turnRate + turnRateNoise;
            const double predDir      = dir + deltaT * predTurnRate;
            
            predictedStates(0, i) = px + std::cos(predDir) * deltaT * predSpeed;
            predictedStates(1, i) = py + std::sin(predDir) * deltaT * predSpeed;
            predictedStates(2, i) = predDir;
            predictedStates(3, i) = predSpeed;
            predictedStates(4, i) = predTurnRate;
        }
        
        // Return the computed predicted state samples back to MATLAB
        outputs[0] = predictedStates;
    } catch (std::exception& ex) {
        mexErrMsgTxt(ex.what());
    }
}

The C++ code is structured as follows.

  • We first parse the input parameters and check that we get the same number of state samples and noise samples in order to avoid unintended out of bounds memory accesses. Note that we hard coded the dimensions of the system state and the system noise, as we know those in advance. However, we do not know the number of samples passed to the MEX file at compile time. Hence, we accept any number of samples by using the Eigen::Dynamic syntax.

  • Second, we allocate the output memory based on the passed number of samples.

  • Third, we perform the actual computation of the predicted state samples. For that, we iterate over all samples using a for loop. In each iteration, we compute a single predicted state sample and store it in the previously allocated memory.

  • Finally, we return the set of predicted state samples back to MATLAB.

Compile the MEX file with

>> compileMex('targetSystemEquation.cpp')

In order to switch to the new MEX-based system model, we only have to modify a single line in NonlinearEstimationExample.m, that is, we replace the line

sysModel = TargetSysModel();

with

sysModel = TargetSysModelMex();

Another Implementation Using Eigen's Capabilities

For those who are familiar with the Eigen linear algebra library might thought that the computation of the predicted state samples could be done without a for loop by using the matrix methods provided by the Eigen. Indeed, we can replace the for loop in targetSystemEquation.cpp with

// Predicted speed
predictedStates.row(3) = stateSamples.row(3) + noiseSamples.row(0);

// Predicted turn rate
predictedStates.row(4) = stateSamples.row(4) + noiseSamples.row(1);

// Predicted direction
predictedStates.row(2) = stateSamples.row(2) + deltaT * predictedStates.row(4);

// Note that we need the array() cast to perform element-wise cos()/sin() and element-wise multiplication

// Predicted x position
predictedStates.row(0) = stateSamples.row(0) + deltaT * (predictedStates.row(2).array().cos() *
                                                         predictedStates.row(3).array()).matrix();

// Predicted y position
predictedStates.row(1) = stateSamples.row(1) + deltaT * (predictedStates.row(2).array().sin() *
                                                         predictedStates.row(3).array()).matrix();

to get the same result. This looks now very similar to the original MATLAB implementation. However, the for loop allows for an easy multithreading as we will see later. Moreover, there might be situations where a for loop is the best way to write a complex system model/measurement model.

PreviousNext