adaptWinPolyEstimator.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __ADAPTWINPOLYESTIMATOR_H__
#define __ADAPTWINPOLYESTIMATOR_H__
 
#include <deque>
 
#include <yarp/sig/Vector.h>
#include <iCub/ctrl/math.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class AWPolyElement
{
public:
    yarp::sig::Vector data;
    double time;
 
    AWPolyElement() {}
 
    AWPolyElement(const yarp::sig::Vector &d, const double t): data(d), time(t) { }
};
 
 
typedef std::deque<AWPolyElement> AWPolyList;
 
class AWPolyEstimator
{
protected:
    AWPolyList elemList;
    unsigned int order;
    unsigned int N;
    double D;
 
    yarp::sig::Vector t;
    yarp::sig::Vector x;
    yarp::sig::Vector coeff;
    yarp::sig::Vector winLen;
    yarp::sig::Vector mse;
 
    bool firstRun;
 
    virtual yarp::sig::Vector fit(const yarp::sig::Vector &x,
                                  const yarp::sig::Vector &y, const unsigned int n=0);
 
    virtual double eval(double x);
 
    virtual double getEsteeme() = 0;
 
public:
    AWPolyEstimator(unsigned int _order, unsigned int _N, const double _D);
 
    AWPolyList &getList() { return elemList; }
 
    void feedData(const AWPolyElement &el);
 
    yarp::sig::Vector getWinLen() { return winLen; }
 
    yarp::sig::Vector getMSE() { return mse; }
 
    yarp::sig::Vector estimate();
 
    yarp::sig::Vector estimate(const AWPolyElement &el);
 
    void reset();
 
    virtual ~AWPolyEstimator() { }
};
 
 
class AWLinEstimator : public AWPolyEstimator
{
protected:
    virtual yarp::sig::Vector fit(const yarp::sig::Vector &x,
                                  const yarp::sig::Vector &y, const unsigned int n=0);
 
    virtual double getEsteeme() { return coeff[1]; }
 
public:
    AWLinEstimator(unsigned int _N, const double _D) : AWPolyEstimator(1,_N,_D) { }
};
 
 
class AWQuadEstimator : public AWPolyEstimator
{
protected:
    virtual double getEsteeme() { return 2.0*coeff[2]; }
 
public:
    AWQuadEstimator(unsigned int _N, const double _D) : AWPolyEstimator(2,_N,_D) { }
};
 
}
 
}
 
#endif