9 void pinv(
const Eigen::MatrixXd& matrix_in, Eigen::MatrixXd& pseudo_inv,
const double& pinvtoler) {
10 Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix_in, Eigen::ComputeThinU | Eigen::ComputeThinV);
11 Eigen::VectorXd singular_values;
12 Eigen::VectorXd singular_values_inv;
13 singular_values = svd.singularValues();
14 singular_values_inv.setZero(singular_values.size());
16 for (
int w = 0; w < singular_values.size(); ++w)
17 if (singular_values(w) > pinvtoler) singular_values_inv(w) = 1 / singular_values(w);
18 pseudo_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose();
23 : order_(order), N_(N), dt_(dt), dt_zero_(true), first_run_(true), pt_(0) {
45 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem.at(i) = el[i];
58 for (
unsigned int j = 0; j <
N_; ++j) {
60 if (idx >=
N_) idx = idx -
N_;
69 for (
unsigned int i = 0; i < dim; ++i) {
71 for (
unsigned int j = 0; j <
N_; ++j) {
73 if (idx >=
N_) idx = idx -
N_;
85 for (
unsigned int i = 0; i <
N_; ++i) {
89 for (
unsigned int j = 1; j <=
order_; ++j) {
94 Eigen::Map<Eigen::VectorXd> ytemp(&
x_[0],
N_, 1);
95 coeff_ =
R_.householderQr().solve(ytemp);
unsigned int getWindowLength()
std::vector< double > time_list_
Time vector corresponding to each element in elem_list_.
PolyEstimator(const unsigned int &order, const unsigned int &N, const double &dt)
Eigen::VectorXd coeff_
Coefficients for the least squares solution.
bool dt_zero_
Indicate that dt is zero (dt is invalid)
void pinv(const Eigen::MatrixXd &matrix_in, Eigen::MatrixXd &pseudo_inv, const double &pinvtoler)
double dt_
Sampling (control) time.
std::vector< std::vector< double > > elem_list_
All the data (N elements of size dim)
void setWindowLength(const unsigned int &N)
virtual double getEsteeme()=0
void estimate(std::vector< double > &estimee, const std::vector< double > &data_element, const double &time)
unsigned int N_
Window length.
unsigned int pt_
Circular index to each data and time element.
unsigned int order_
Order of the polynomial estimator.
std::vector< double > t_
Time vector setting the lowest time to zero (for numerical stability).