sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
lin-estimator.cpp
Go to the documentation of this file.
1/*
2 Oscar Efrain RAMOS PONCE, LAAS-CNRS
3 Date: 28/10/2014
4 Object to estimate a polynomial of first order that fits some data.
5*/
6
7#include <iostream>
9
10LinEstimator::LinEstimator(const unsigned int& N, const unsigned int& dim,
11 const double& dt)
12 : PolyEstimator(1, N, dt), dim_(dim), sum_ti_(0.0), sum_ti2_(0.0) {
13 /* Number of coefficients for a quadratic estimator: 2 */
14 coeff_.resize(2);
15
16 /* Initialize sums for the recursive computation */
17 sum_xi_.resize(dim);
18 sum_tixi_.resize(dim);
19 for (unsigned int i = 0; i < dim; ++i) {
20 sum_xi_.at(i) = 0.0;
21 sum_tixi_.at(i) = 0.0;
22 }
23
24 /* Initialize (pre-compute) pseudo inverse matrix that assumes that the sample
25 time is constant (only if dt is not zero) */
26 if (!dt_zero_) {
27 Eigen::MatrixXd Tmat(N_, 2);
28 Eigen::MatrixXd pinvTmat(2, N_);
29 double time = 0.0;
30 for (unsigned int i = 0; i < N_; ++i) {
31 Tmat(i, 1) = time;
32 Tmat(i, 0) = 1.0;
33 time += dt_;
34 }
35 /* Half time used to estimate the position */
36 tmed_ = time * 0.5;
37
38 /* Pseudo inverse */
39 pinv(Tmat, pinvTmat);
40 pinv0_ = new double[N_];
41 pinv1_ = new double[N_];
42 c0_.resize(dim_);
43 c1_.resize(dim_);
44 c0_.assign(dim_, 0.0);
45 c1_.assign(dim_, 0.0);
46
47 /* Copy the pseudo inverse to an array */
48 for (unsigned int i = 0; i < N_; ++i) {
49 pinv0_[i] = pinvTmat(0, i);
50 pinv1_[i] = pinvTmat(1, i);
51 }
52 }
53}
54
55double LinEstimator::getEsteeme() { return coeff_(1); }
56
57void LinEstimator::estimateRecursive(std::vector<double>& esteem,
58 const std::vector<double>& el,
59 const double& time) {
60 /* Feed Data */
61 elem_list_.at(pt_) = el;
62 time_list_.at(pt_) = time;
63
64 double t, x;
65
66 if (first_run_) {
67 // Not enough elements in the window
68 if ((pt_ + 1) < N_) {
69 // t = time - time_list_.at(0);
70 t = time;
71 sum_ti_ += t;
72 sum_ti2_ += t * t;
73 for (unsigned int i = 0; i < esteem.size(); ++i) {
74 // Return a zero vector
75 esteem.at(i) = 0.0;
76 x = el.at(i);
77 // Fill the sumations
78 sum_xi_.at(i) += x;
79 sum_tixi_.at(i) += t * x;
80 }
81 pt_++;
82 return;
83 } else {
84 first_run_ = false;
85 }
86 }
87
88 // Increase the 'circular' pointer
89 pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
90 // The pointer now points to the "oldest" element in the vector
91
92 // Get size of first element: N dof (assuming all elements have same size)
93 size_t dim = elem_list_.at(0).size();
94 // TODO: CHECK THAT SIZE OF ESTEEM = DIM
95
96 // t = time - time_list_.at(pt_);
97 t = time;
98 sum_ti_ += t;
99 sum_ti2_ += t * t;
100 double den = N_ * sum_ti2_ - sum_ti_ * sum_ti_;
101
102 // double t_old = time_list_.at(pt_);
103 double t_old = time_list_.at(pt_);
104 for (unsigned int i = 0; i < dim; ++i) {
105 x = el[i];
106 // Fill the sumations
107 sum_xi_[i] += x;
108 sum_tixi_[i] += t * x;
109
110 // the bias
111 // coeff_(0) = (sum_xi*sum_titi - sum_ti*sum_tixi) / den;
112 // the linear coefficient
113 esteem[i] = (N_ * sum_tixi_[i] - sum_ti_ * sum_xi_[i]) / den;
114
115 x = elem_list_[pt_][i];
116 sum_xi_[i] -= x;
117 sum_tixi_[i] -= t_old * x;
118 }
119 sum_ti_ -= t_old;
120 sum_ti2_ -= t_old * t_old;
121
122 return;
123}
124
125void LinEstimator::fit() {
126 double sum_ti = 0.0;
127 double sum_titi = 0.0;
128 double sum_xi = 0.0;
129 double sum_tixi = 0.0;
130
131 for (unsigned int i = 0; i < N_; ++i) {
132 sum_ti += t_[i];
133 sum_titi += t_[i] * t_[i];
134 sum_xi += x_[i];
135 sum_tixi += t_[i] * x_[i];
136 }
137
138 double den = N_ * sum_titi - sum_ti * sum_ti;
139
140 // the bias
141 coeff_(0) = (sum_xi * sum_titi - sum_ti * sum_tixi) / den;
142
143 // the linear coefficient
144 coeff_(1) = (N_ * sum_tixi - sum_ti * sum_xi) / den;
145
146 return;
147}
148
149void LinEstimator::estimate(std::vector<double>& esteem,
150 const std::vector<double>& el) {
151 if (dt_zero_) {
152 std::cerr << "Error: dt cannot be zero" << std::endl;
153 // Return a zero vector
154 for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
155 return;
156 }
157
158 /* Feed Data. Note that the time is not completed since it is assumed to be
159 constant */
160 elem_list_[pt_] = el;
161
162 if (first_run_) {
163 if ((pt_ + 1) < N_) {
164 // Return input vector when not enough elements to compute
165 pt_++;
166 for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
167 return;
168 } else
169 first_run_ = false;
170 }
171
172 // Next pointer value
173 pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
174
175 unsigned int idx;
176 double x;
177 // Cycle all the elements in the vector
178 for (int i = 0; i < dim_; ++i) {
179 c0_[i] = 0.0;
180 c1_[i] = 0.0;
181 // Retrieve the data in the window
182 for (unsigned int j = 0; j < N_; ++j) {
183 idx = (pt_ + j);
184 if (idx >= N_) idx -= N_;
185 x = elem_list_[idx][i];
186 c0_[i] += x * pinv0_[j];
187 c1_[i] += x * pinv1_[j];
188 }
189
190 // Polynomial (position)
191 esteem[i] = c1_[i] * tmed_ + c0_[i];
192 }
193}
194
196 std::vector<double>& estimateDerivative, const unsigned int order) {
197 switch (order) {
198 case 0:
199 for (int i = 0; i < dim_; ++i)
200 estimateDerivative[i] = c1_[i] * tmed_ + c0_[i];
201 return;
202
203 case 1:
204 for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[i];
205 return;
206
207 default:
208 for (int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;
209 }
210}
LinEstimator(const unsigned int &N, const unsigned int &dim, const double &dt=0.0)
virtual void estimateRecursive(std::vector< double > &estimee, const std::vector< double > &el, const double &time)
void getEstimateDerivative(std::vector< double > &estimeeDerivative, const unsigned int order)
virtual void estimate(std::vector< double > &estimee, const std::vector< double > &el)
unsigned int N_
Window length.
std::vector< double > time_list_
Time vector corresponding to each element in elem_list_.
double dt_
Sampling (control) time.
bool dt_zero_
Indicate that dt is zero (dt is invalid)
Eigen::VectorXd coeff_
Coefficients for the least squares solution.
std::vector< double > x_
unsigned int pt_
Circular index to each data and time element.
std::vector< double > t_
Time vector setting the lowest time to zero (for numerical stability).
std::vector< std::vector< double > > elem_list_
All the data (N elements of size dim)
void pinv(const Eigen::MatrixXd &matrix_in, Eigen::MatrixXd &pseudo_inv, const double &pinvtoler=1.0e-6)