SmoothDerivative.cpp

Go to the documentation of this file.

#include "robodyn_utilities/SmoothDerivative.h"
#include "nasa_common_logging/Logger.h"
#include <string.h>
#include <stdlib.h>



SmoothDerivative::SmoothDerivative()
{
    isInitialized = false;
}



SmoothDerivative::SmoothDerivative(std::vector<double> xA, std::vector<double> smoothingA, double frequencyA)
{
    isInitialized = Initialize(xA, smoothingA, frequencyA);
}



SmoothDerivative::~SmoothDerivative()
{
}



bool SmoothDerivative::Initialize(std::vector<double> xA, std::vector<double> smoothingA, double frequencyA)
{
    isInitialized = false;

    //check that the sizes are equal
    if (xA.size() != smoothingA.size())
    {
        return false;
    }

    if (xDotCalc.size() != xA.size())
    {
        xDotCalc.clear();
        xDotCalc.resize(xA.size());
    }

    m_freq = frequencyA;
    m_smoothing = smoothingA;

    for (unsigned int iL = 0; iL < smoothingA.size(); iL++)
    {
        xDotCalc[iL].m_xPrevSample     = xA[iL];
        xDotCalc[iL].m_xPrevTransition = xA[iL];
        xDotCalc[iL].m_smooth          = (smoothingA[iL] < 0.0) ? 0.0 : (smoothingA[iL] > 1.0) ? 1.0 : smoothingA[iL];
        xDotCalc[iL].m_innovation      = (1.0 - xDotCalc[iL].m_smooth) * frequencyA;
        xDotCalc[iL].m_regression      = xDotCalc[iL].m_smooth;
        xDotCalc[iL].m_numTicks        = 0;
        xDotCalc[iL].m_delta           = 0;
        xDotCalc[iL].m_numTransitions  = 0;
        xDotCalc[iL].m_xDot            = 0;
    }

    isInitialized = true;
    return true;
}


void SmoothDerivative::OnExecute(std::vector<double> xA, std::vector<double>& xDotA)
{
    if (!isInitialized)
    {
        // you muse initialize before you use
        std::stringstream err;
        err << "You must initilize SmoothDerivative before calling OnExecute";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.SmoothDerivative", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    double       diffL;
    unsigned int iL;

    if (xA.size() != xDotCalc.size())
    {
        std::stringstream err;
        err << "xA should be of size " << xDotCalc.size() << " but it is " << xA.size() << ".";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.SmoothDerivative", log4cpp::Priority::ERROR, err.str());
        //Initialize(xA, m_smoothing, m_freq );
        throw std::runtime_error(err.str());
    }

    if (xA.size() != xDotA.size())
    {
        xDotA.resize(xA.size());
    }

    for (iL = 0; iL < xA.size(); iL++)
    {
        ++xDotCalc[iL].m_numTicks;
//        std::cout<<"num Ticks: "<<xDotCalc[iL].m_numTicks<<std::endl;

        diffL = xDotCalc[iL].m_xPrevSample - xA[iL];
//        std::cout<<"diffL: "<<diffL<<std::endl;
//        std::cout<<"numTransitions: "<<xDotCalc[iL].m_numTransitions;

        if (diffL != 0.)     //  SIGNAL VALUE CHANGED
        {
            xDotCalc[iL].m_numTicksPrev    = xDotCalc[iL].m_numTicks;
            xDotCalc[iL].m_xPrevTransition = xDotCalc[iL].m_xPrevSample;
            xDotCalc[iL].m_xPrevSample     = xA[iL];
            xDotCalc[iL].m_delta           = xDotCalc[iL].m_delta * 0.8 + xDotCalc[iL].m_numTicks * 0.2;     //  FILTER THE EFFECTIVE DELTA TIME
            ++xDotCalc[iL].m_numTransitions;
            xDotCalc[iL].m_numTicks = 0;
        }
        else
        {
            if (xDotCalc[iL].m_numTicks > xDotCalc[iL].m_numTicksPrev)
            {
                xDotCalc[iL].m_delta = xDotCalc[iL].m_delta * 0.8 + xDotCalc[iL].m_numTicks * 0.2;
            }
        }

        if (xDotCalc[iL].m_numTransitions > 2 && xDotCalc[iL].m_delta != 0)
        {
            xDotCalc[iL].m_xDot = xDotCalc[iL].m_innovation * (xA[iL] - xDotCalc[iL].m_xPrevTransition) / xDotCalc[iL].m_delta + xDotCalc[iL].m_regression * xDotCalc[iL].m_xDot;
        }
        else
        {
            xDotCalc[iL].m_xDot = 0.0;
        }

        xDotA[iL] = xDotCalc[iL].m_xDot;
    }
}