UnscentedTransform.cpp

Go to the documentation of this file.

// *****************************************************************
// Filename:    UnscentedTransform.cpp
// Copyright:   Kai Welke, Chair Prof. Dillmann (IAIM),
//              Institute for Anthropomatics (IFA),
//              Humanoids and Intelligence Systems Lab,
//              Karlsruhe Institute of Technology. All rights reserved.
// Author:      Kai Welke
// Date:        20.01.2010
// *****************************************************************
 
 
#include "UnscentedTransform.h"
 
#include <cstdio>
#include <iostream>
 
UnscentedTransform::UnscentedTransform(float fAlpha, float fKappa, float fBeta)
{
    m_fAlpha = fAlpha;
    m_fBeta = fBeta;
    m_fKappa = fKappa;
}
 
Eigen::MatrixXd
UnscentedTransform::getSigmaPoints(const Gaussian& gaussian)
{
    // ctreate required matrices
    int nNumberDimensions = gaussian.getDimensions();
    int nNumberSigmaPoints = 2 * nNumberDimensions + 1;
    Eigen::MatrixXd sigmaPoints(nNumberDimensions, nNumberSigmaPoints);
    Eigen::VectorXd w_m(nNumberSigmaPoints);
    Eigen::VectorXd w_c(nNumberSigmaPoints);
 
    // calculate lamda factor
    float fLamda = m_fAlpha * m_fAlpha * (nNumberDimensions - m_fKappa) - nNumberDimensions;
 
    // first sigmapoint is mean
    sigmaPoints.block(0, 0, sigmaPoints.rows(), 1) = gaussian.getMean();
    w_m(0) = fLamda / (nNumberDimensions + fLamda);
    w_c(0) = fLamda / (nNumberDimensions + fLamda) + (1 - m_fAlpha * m_fAlpha + m_fBeta);
 
    // precalculate square root of matrix
    Eigen::MatrixXd diff(nNumberDimensions, nNumberDimensions);
    diff = (nNumberDimensions + fLamda) * gaussian.getCovariance();
 
    // square root of diagonal matrix
    diff = squareRoot(diff);
 
    // generate 2n sigmapoints
    Eigen::VectorXd point1(nNumberDimensions);
    Eigen::VectorXd point2(nNumberDimensions);
 
    float fWeight;
 
    for (int i = 0; i < nNumberDimensions; i++)
    {
        // points
        point1 = gaussian.getMean() + diff.block(i, 0, 1, diff.cols()).transpose();
        point2 = gaussian.getMean() - diff.block(i, 0, 1, diff.cols()).transpose();
        sigmaPoints.block(0, 2 * i + 1, sigmaPoints.rows(), 1) = point1;
        sigmaPoints.block(0, 2 * i + 2, sigmaPoints.rows(), 1) = point2;
 
        // calculate weight
        fWeight = 1.0f / (2 * (nNumberDimensions + fLamda));
        w_m(2 * i + 1) = fWeight;
        w_m(2 * i + 2) = fWeight;
        w_c(2 * i + 1) = fWeight;
        w_c(2 * i + 2) = fWeight;
    }
 
    m_weights_m = w_m;
    m_weights_c = w_c;
 
    return sigmaPoints;
}
 
Gaussian
UnscentedTransform::extractGaussian(Eigen::MatrixXd processedSigmaPoints)
{
    Gaussian result(processedSigmaPoints.rows());
 
    // assemble mean
    for (int i = 0; i < m_weights_c.rows(); i++)
    {
        result.setMean(result.getMean() +
                       m_weights_m(i) *
                           processedSigmaPoints.block(0, i, processedSigmaPoints.rows(), 1));
    }
 
    // assemble cov
    Eigen::MatrixXd cov = result.getCovariance();
    cov.setIdentity();
    result.setCovariance(cov);
 
    for (int i = 0; i < m_weights_c.rows(); i++)
    {
        result.setCovariance(
            result.getCovariance() +
            m_weights_c(i) * ((processedSigmaPoints.block(0, i, processedSigmaPoints.rows(), 1) -
                               result.getMean()) *
                              ((processedSigmaPoints.block(0, i, processedSigmaPoints.rows(), 1) -
                                result.getMean())
                                   .transpose())));
    }
 
    return result;
}
 
Eigen::MatrixXd
UnscentedTransform::squareRoot(Eigen::MatrixXd input)
{
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(input);
 
    Eigen::MatrixXd sqrtA = es.operatorSqrt();
 
    return sqrtA;
}