#include <sot/torque_control/utils/poly-estimator.hh>
Public Member Functions | |
PolyEstimator (const unsigned int &order, const unsigned int &N, const double &dt) | |
virtual void | estimate (std::vector< double > &estimee, const std::vector< double > &data_element)=0 |
void | estimate (std::vector< double > &estimee, const std::vector< double > &data_element, const double &time) |
virtual void | estimateRecursive (std::vector< double > &estimee, const std::vector< double > &data_element, const double &time)=0 |
virtual void | getEstimateDerivative (std::vector< double > &estimeeDerivative, const unsigned int order)=0 |
unsigned int | getWindowLength () |
void | setWindowLength (const unsigned int &N) |
Protected Member Functions | |
virtual void | fit () |
virtual double | getEsteeme ()=0 |
Protected Attributes | |
Eigen::VectorXd | coeff_ |
Coefficients for the least squares solution. More... | |
double | dt_ |
Sampling (control) time. More... | |
bool | dt_zero_ |
Indicate that dt is zero (dt is invalid) More... | |
std::vector< std::vector< double > > | elem_list_ |
All the data (N elements of size dim) More... | |
bool | first_run_ |
unsigned int | N_ |
Window length. More... | |
unsigned int | order_ |
Order of the polynomial estimator. More... | |
unsigned int | pt_ |
Circular index to each data and time element. More... | |
Eigen::MatrixXd | R_ |
std::vector< double > | t_ |
Time vector setting the lowest time to zero (for numerical stability). More... | |
std::vector< double > | time_list_ |
Time vector corresponding to each element in elem_list_. More... | |
std::vector< double > | x_ |
Object to fit a polynomial of a given order. It provides a generic fitting polynomial of any order. However, it cannot be used by itself since the proper coefficient of the polynomial (which represents the estimation) needs to be specified. Moreover, the derived classes implement a faster computation based on the specific case.
Definition at line 29 of file poly-estimator.hh.
PolyEstimator | ( | const unsigned int & | order, |
const unsigned int & | N, | ||
const double & | dt | ||
) |
Create a polynomial estimator on a window of length N
order | is the order of the polynomial estimator. |
N | is the window length. |
dt | is the control (sampling) time |
Definition at line 22 of file poly-estimator.cpp.
|
pure virtual |
Estimate the polynomial given a new element assuming a constant time difference. This constant time difference between consecutive samples is given by dt (specified in the constructor).
Note: This function will only work if dt is different to zero.
[out] | estimee | is the calculated estimation. |
[in] | data_element | is the new data vector. |
Implemented in QuadEstimator, and LinEstimator.
void estimate | ( | std::vector< double > & | estimee, |
const std::vector< double > & | data_element, | ||
const double & | time | ||
) |
Estimate the generic polynomial given a new element. The order of the polynomial is specified in the constructor. Note that this function can be slow if no specialization of fit() has been done, since the generic algorithm would be used.
[out] | estimee | is the calculated estimation |
[in] | data_element | is the new data vector. |
[in] | time | is the time stamp corresponding to the new data. |
Definition at line 36 of file poly-estimator.cpp.
|
pure virtual |
Estimate the polynomial given a new element using a recursive algorithm. This method is faster. However, it takes the time as it is (it does not set the lowest time to zero), which can create "ill-conditions".
[out] | estimee | is the calculated estimation |
[in] | data_element | is the new data. |
[in] | time | is the time stamp corresponding to the new data. |
Implemented in QuadEstimator, and LinEstimator.
|
protectedvirtual |
Find the regressor which best fits in least square sense the last N data sample couples. The order of the regressor is given in the constructor.
Definition at line 84 of file poly-estimator.cpp.
|
protectedpure virtual |
Get the estimation when using the generic fit function (in poly-estimator)
|
pure virtual |
Get the time derivative of the estimated polynomial.
[out] | estimeeDerivative | is the calculated time derivative. |
[in] | order | The order of the derivative (e.g. 1 means the first derivative). |
Implemented in QuadEstimator, and LinEstimator.
unsigned int getWindowLength | ( | ) |
void setWindowLength | ( | const unsigned int & | N | ) |
Set the size of the filter window.
[in] | N | size |
Definition at line 100 of file poly-estimator.cpp.
|
protected |
Coefficients for the least squares solution.
Definition at line 128 of file poly-estimator.hh.
|
protected |
Sampling (control) time.
Definition at line 109 of file poly-estimator.hh.
|
protected |
Indicate that dt is zero (dt is invalid)
Definition at line 112 of file poly-estimator.hh.
|
protected |
All the data (N elements of size dim)
Definition at line 119 of file poly-estimator.hh.
|
protected |
Indicate that there are not enough elements to compute. The reason is that it is one of the first runs, and the estimate will be zero.
Definition at line 116 of file poly-estimator.hh.
|
protected |
Window length.
Definition at line 106 of file poly-estimator.hh.
|
protected |
Order of the polynomial estimator.
Definition at line 103 of file poly-estimator.hh.
|
protected |
Circular index to each data and time element.
Definition at line 125 of file poly-estimator.hh.
|
protected |
Matrix containing time components. It is only used for the generic fit computation, such that the estimation is \(c = R^{\#} x\), where \(x\) is the data vector.
Definition at line 140 of file poly-estimator.hh.
|
protected |
Time vector setting the lowest time to zero (for numerical stability).
Definition at line 131 of file poly-estimator.hh.
|
protected |
Time vector corresponding to each element in elem_list_.
Definition at line 122 of file poly-estimator.hh.
|
protected |
Data vector for a single dimension (a single dof). It is only one 'column' of the elem_list_ 'matrix'
Definition at line 135 of file poly-estimator.hh.