PreviousNext

An Additive Noise Measurement Model in C++

The special case of a model that suffers from pure additive noise is handled in the same way as before: we need a MATLAB class that inherits from the respective model and a MEX file that implements the actual systemEquation()/measurementEquation() method.

Here, we consider the nonlinear measurement 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:

PolarMeasModelMex.m

classdef PolarMeasModelMex < AdditiveNoiseMeasurementModel
    methods
        function obj = PolarMeasModelMex()
            measNoise = Gaussian(zeros(2, 1), [1e-2 1e-4]);
            
            obj.setNoise(measNoise);
        end
        
        function measurements = measurementEquation(obj, stateSamples)
            measurements = polarMeasurementEquation(stateSamples);
        end
    end
end

The only significant difference compared to the original PolarMeasModel.m file is that the measurementEquation() code is moved into a MEX file called polarMeasurementEquation(). In contrast to the previous example, it takes only state samples.

polarMeasurementEquation.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 != 1) {
            throw std::invalid_argument("One input is 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]);
        
        // Allocate memory for measurement samples
        const int numSamples = stateSamples.cols();
        
        Mex::OutputMatrix<double, 2, Eigen::Dynamic> measurements(2, numSamples);
        
        // Compute a measurement sample in each iteration
        for (int i = 0; i < numSamples; ++i) {
            const double px = stateSamples(0, i);
            const double py = stateSamples(1, i);
            
            measurements.col(i) = Eigen::Vector2d(std::sqrt(px * px + py * py),
                                                  std::atan2(py, px));
        }
        
        // Return the computed measurement samples back to MATLAB
        outputs[0] = measurements;
    } catch (std::exception& ex) {
        mexErrMsgTxt(ex.what());
    }
}

The above C++ code is structured analogously to the nonlinear system model example.

  • We first parse the input parameter. Note that we hard coded the dimension of the system state, as we know it in advance. However, we do not know the number of state 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 measurement samples. For that, we iterate over all state samples using a for loop. In each iteration, we compute a single measurement sample and store it in the previously allocated memory.

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

Compile the MEX file with

>> compileMex('polarMeasurementEquation.cpp')

In order to switch to the new MEX-based measurement model, we only have to set

measModel = PolarMeasModelMex();

in NonlinearEstimationExample.m.

Keep in mind that the likelihood implementation of an AdditiveNoiseMeasurementModel also relies on the measurementEquation(). Hence, writing it in C++ also boosts the likelihood evaluation. Nevertheless, we can also implement the entire likelihood in C++ as well, which will be discussed next.

PreviousNext