PreviousNext

A Likelihood in C++

Like the system models/measurement models, also a likelihood can be computed in C++ based on the MATLAB class and MEX file combination.

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

PolarMeasLikelihoodMex.m

classdef PolarMeasLikelihoodMex < Likelihood
    methods
        function obj = PolarMeasLikelihoodMex()
            % Inverse covariance matrix of time-invariant,
            % zero-mean Gaussian measurement noise
            obj.invNoiseCov = diag(1 ./ [1e-2 1e-4]);
        end
        
        function logValues = logLikelihood(obj, stateSamples, measurement)
            logValues = polarMeasLogLikelihood(stateSamples, obj.invNoiseCov, measurement);
        end
    end
    
    properties (Access = 'private')
        invNoiseCov;
    end
end

It is important to note that, compared to the original PolarMeasLikelihood.m file, we explicitly compute and store the inverse of the measurement noise covariance matrix. The reason for this is that in this example the measurement noise is time-invariant. Thus, we have to compute the inverse covariance matrix only once and not for each likelihood evaluation (note that this also holds for the likelihood implementation of the AdditiveNoiseMeasurementModel). The inverse covariance matrix is then passed to the polarMeasLogLikelihood() MEX file in the logLikelihood() method. In doing so, we can change it without recompiling the MEX file. Of course, if the measurement noise changes over time, we can also directly pass the covariance matrix to the MEX file and compute its inverse using the Eigen library. The C++ code of the log-likelihood MEX file is listed next:

polarMeasLogLikelihood.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 inverse noise covariance matrix
        Mex::ConstMatrix<double, 2, 2> invNoiseCov(inputs[1]);
        
        // Thrid parameter contains the actual measurement
        Mex::ConstMatrix<double, 2, 1> measurement(inputs[2]);
        
        // Allocate memory for log-likelihood values
        const int numSamples = stateSamples.cols();
        
        Mex::OutputMatrix<double, 1, Eigen::Dynamic> logValues(1, numSamples);
        
        // Compute a log-likelihood value in each iteration
        for (int i = 0; i < numSamples; ++i) {
            const double px = stateSamples(0, i);
            const double py = stateSamples(1, i);
            
            Eigen::Vector2d vec(std::sqrt(px * px + py * py),
                                std::atan2(py, px));
            
            vec -= measurement;
            
            const double logExpVal = -0.5 * vec.transpose() * invNoiseCov * vec;
            
            // Note that the computed likelihood values are only correct up to proportionality
            // as we omit the normalization part of the Gaussian PDF!
            logValues(i) = logExpVal;
        }
        
        // Return the computed log-likelihood values back to MATLAB
        outputs[0] = logValues;
    } catch (std::exception& ex) {
        mexErrMsgTxt(ex.what());
    }
}

The code is structured analogously to the previous examples.

  • We first parse the input parameters. Note that we hard coded the dimensions of the system state, the measurement noise covariance matrix, and the actual measurement, as we know those 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 log-likelihood values. For that, we iterate over all state samples using a for loop. In each iteration, we compute a single log-likelihood value and store it in the previously allocated memory.

  • Finally, we return the set of log-likelihood values back to MATLAB.

It should be noted that for estimators relying on a likelihood function, e.g., particle filters, it is sufficient that likelihood values are only correct up to proportionality, which is the case for the above implementation to avoid unnecessary computations! That is, we omitted the constant/normalization part of the Gaussian PDF. This is the reason for not computing the determinant of the measurement noise covariance matrix in the constructor and passing it to the MEX file.

Compile the MEX file with

>> compileMex('polarMeasLogLikelihood.cpp')

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

measModel = PolarMeasLikelihoodMex();

in NonlinearEstimationExample.m.

PreviousNext