1 |
|
|
/* |
2 |
|
|
Oscar Efrain RAMOS PONCE, LAAS-CNRS |
3 |
|
|
Date: 28/10/2014 |
4 |
|
|
Object to estimate a polynomial that fits some data. |
5 |
|
|
*/ |
6 |
|
|
|
7 |
|
|
#include <sot/torque_control/utils/poly-estimator.hh> |
8 |
|
|
|
9 |
|
|
void pinv(const Eigen::MatrixXd& matrix_in, Eigen::MatrixXd& pseudo_inv, |
10 |
|
|
const double& pinvtoler) { |
11 |
|
|
Eigen::JacobiSVD<Eigen::MatrixXd> svd( |
12 |
|
|
matrix_in, Eigen::ComputeThinU | Eigen::ComputeThinV); |
13 |
|
|
Eigen::VectorXd singular_values; |
14 |
|
|
Eigen::VectorXd singular_values_inv; |
15 |
|
|
singular_values = svd.singularValues(); |
16 |
|
|
singular_values_inv.setZero(singular_values.size()); |
17 |
|
|
|
18 |
|
|
for (int w = 0; w < singular_values.size(); ++w) |
19 |
|
|
if (singular_values(w) > pinvtoler) |
20 |
|
|
singular_values_inv(w) = 1 / singular_values(w); |
21 |
|
|
pseudo_inv = svd.matrixV() * singular_values_inv.asDiagonal() * |
22 |
|
|
svd.matrixU().transpose(); |
23 |
|
|
return; |
24 |
|
|
} |
25 |
|
|
|
26 |
|
|
PolyEstimator::PolyEstimator(const unsigned int& order, const unsigned int& N, |
27 |
|
|
const double& dt) |
28 |
|
|
: order_(order), N_(N), dt_(dt), dt_zero_(true), first_run_(true), pt_(0) { |
29 |
|
|
t_.resize(N_); |
30 |
|
|
x_.resize(N_); |
31 |
|
|
elem_list_.reserve(N_); |
32 |
|
|
elem_list_.resize(N_); |
33 |
|
|
time_list_.reserve(N_); |
34 |
|
|
time_list_.resize(N_); |
35 |
|
|
// Determine if the dt is valid |
36 |
|
|
if (dt_ > 1e-10) dt_zero_ = false; |
37 |
|
|
// Used only in the generic fit function |
38 |
|
|
R_.resize(N_, order_ + 1); |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
void PolyEstimator::estimate(std::vector<double>& esteem, |
42 |
|
|
const std::vector<double>& el, |
43 |
|
|
const double& time) { |
44 |
|
|
/* Feed Data */ |
45 |
|
|
elem_list_.at(pt_) = el; |
46 |
|
|
time_list_.at(pt_) = time; |
47 |
|
|
|
48 |
|
|
if (first_run_) { |
49 |
|
|
if ((pt_ + 1) < N_) { |
50 |
|
|
// Return input vector when not enough elements to compute |
51 |
|
|
pt_++; |
52 |
|
|
for (unsigned int i = 0; i < esteem.size(); ++i) esteem.at(i) = el[i]; |
53 |
|
|
return; |
54 |
|
|
} else { |
55 |
|
|
first_run_ = false; |
56 |
|
|
} |
57 |
|
|
} |
58 |
|
|
|
59 |
|
|
// Next pointer value |
60 |
|
|
pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0; |
61 |
|
|
|
62 |
|
|
// Get the time substracting the lowest one, so that the minimum time is |
63 |
|
|
// always zero |
64 |
|
|
unsigned int idx; |
65 |
|
|
for (unsigned int j = 0; j < N_; ++j) { |
66 |
|
|
idx = pt_ + j; |
67 |
|
|
if (idx >= N_) idx = idx - N_; |
68 |
|
|
t_[j] = time_list_[idx] - time_list_[pt_]; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
// Get size of first element: N dof (assuming all elements have same size) |
72 |
|
|
size_t dim = elem_list_.at(0).size(); |
73 |
|
|
// TODO: CHECK THAT SIZE OF ESTEEM = DIM |
74 |
|
|
|
75 |
|
|
// Cycle upon all elements |
76 |
|
|
for (unsigned int i = 0; i < dim; ++i) { |
77 |
|
|
// Retrieve the data vector |
78 |
|
|
for (unsigned int j = 0; j < N_; ++j) { |
79 |
|
|
idx = pt_ + j; |
80 |
|
|
if (idx >= N_) idx = idx - N_; |
81 |
|
|
x_[j] = elem_list_[idx][i]; |
82 |
|
|
} |
83 |
|
|
// Fit the vector |
84 |
|
|
fit(); |
85 |
|
|
esteem[i] = getEsteeme(); |
86 |
|
|
} |
87 |
|
|
|
88 |
|
|
return; |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
void PolyEstimator::fit() { |
92 |
|
|
for (unsigned int i = 0; i < N_; ++i) { |
93 |
|
|
double xtemp = t_[i]; |
94 |
|
|
R_(i, 0) = 1.0; |
95 |
|
|
|
96 |
|
|
for (unsigned int j = 1; j <= order_; ++j) { |
97 |
|
|
R_(i, j) = xtemp; |
98 |
|
|
xtemp *= xtemp; |
99 |
|
|
} |
100 |
|
|
} |
101 |
|
|
Eigen::Map<Eigen::VectorXd> ytemp(&x_[0], N_, 1); |
102 |
|
|
coeff_ = R_.householderQr().solve(ytemp); |
103 |
|
|
|
104 |
|
|
return; |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
void PolyEstimator::setWindowLength(const unsigned int& N) { N_ = N; } |
108 |
|
|
|
109 |
|
|
unsigned int PolyEstimator::getWindowLength() { return N_; } |