12 :
PolyEstimator(2, N, dt), dim_(dim), sum_ti_(0.0), sum_ti2_(0.0), sum_ti3_(0.0), sum_ti4_(0.0) {
18 sum_tixi_.resize(dim);
19 sum_ti2xi_.resize(dim);
20 for (
unsigned int i = 0; i < dim; ++i) {
22 sum_tixi_.at(i) = 0.0;
23 sum_ti2xi_.at(i) = 0.0;
29 Eigen::MatrixXd Tmat(
N_, 3);
30 Eigen::MatrixXd pinvTmat(3,
N_);
32 for (
unsigned int i = 0; i <
N_; ++i) {
33 Tmat(i, 2) = 0.5 * time * time;
43 pinv0_ =
new double[
N_];
44 pinv1_ =
new double[
N_];
45 pinv2_ =
new double[
N_];
49 c0_.assign(dim_, 0.0);
50 c1_.assign(dim_, 0.0);
51 c2_.assign(dim_, 0.0);
54 for (
unsigned int i = 0; i <
N_; ++i) {
55 pinv2_[i] = pinvTmat(2, i);
56 pinv1_[i] = pinvTmat(1, i);
57 pinv0_[i] = pinvTmat(0, i);
62 double QuadEstimator::getEsteeme() {
return coeff_(2); }
78 sum_ti3_ += t * t * t;
79 sum_ti4_ += t * t * t * t;
81 for (
unsigned int i = 0; i < esteem.size(); ++i) {
87 sum_tixi_.at(i) += t * x;
88 sum_ti2xi_.at(i) += t * t * x;
109 sum_ti3_ += t * t * t;
110 sum_ti4_ += t * t * t * t;
112 double den = 0.25 *
N_ * sum_ti3_ * sum_ti3_ - 0.5 * sum_ti3_ * sum_ti2_ * sum_ti_ +
113 0.25 * sum_ti2_ * sum_ti2_ * sum_ti2_ + 0.25 * sum_ti4_ * sum_ti_ * sum_ti_ -
114 0.25 *
N_ * sum_ti4_ * sum_ti2_;
115 double den2 = 1.0 / (2.0 * den);
122 for (
unsigned int i = 0; i < dim; ++i) {
126 sum_tixi_[i] += t * x;
127 sum_ti2xi_[i] += t * t * x;
130 (sum_ti2xi_[i] * (sum_ti_ * sum_ti_ -
N_ * sum_ti2_) + sum_tixi_[i] * (
N_ * sum_ti3_ - sum_ti2_ * sum_ti_) +
131 sum_xi_[i] * (sum_ti2_ * sum_ti2_ - sum_ti3_ * sum_ti_));
143 sum_tixi_[i] -= t_old * x;
144 sum_ti2xi_[i] -= t_old * t_old * x;
147 sum_ti2_ -= t_old * t_old;
148 sum_ti3_ -= t_old * t_old * t_old;
149 sum_ti4_ -= t_old * t_old * t_old * t_old;
154 void QuadEstimator::fit() {
156 double sum_ti2 = 0.0;
157 double sum_ti3 = 0.0;
158 double sum_ti4 = 0.0;
160 double sum_tixi = 0.0;
161 double sum_ti2xi = 0.0;
163 for (
unsigned int i = 0; i <
N_; ++i) {
165 sum_ti2 +=
t_[i] *
t_[i];
166 sum_ti3 +=
t_[i] *
t_[i] *
t_[i];
167 sum_ti4 +=
t_[i] *
t_[i] *
t_[i] *
t_[i];
169 sum_tixi +=
t_[i] *
x_[i];
170 sum_ti2xi +=
t_[i] *
t_[i] *
x_[i];
173 double den = 0.25 *
N_ * sum_ti3 * sum_ti3 - 0.5 * sum_ti3 * sum_ti2 * sum_ti + 0.25 * sum_ti2 * sum_ti2 * sum_ti2 +
174 0.25 * sum_ti4 * sum_ti * sum_ti - 0.25 *
N_ * sum_ti4 * sum_ti2;
175 double den4 = 1.0 / (4.0 * den);
177 coeff_(2) = den4 * (sum_ti2xi * (sum_ti * sum_ti -
N_ * sum_ti2) + sum_tixi * (
N_ * sum_ti3 - sum_ti2 * sum_ti) +
178 sum_xi * (sum_ti2 * sum_ti2 - sum_ti3 * sum_ti));
179 coeff_(1) = den4 * (sum_ti2xi * (
N_ * sum_ti3 - sum_ti2 * sum_ti) + sum_tixi * (sum_ti2 * sum_ti2 -
N_ * sum_ti4) +
180 sum_xi * (sum_ti4 * sum_ti - sum_ti3 * sum_ti2));
189 std::cerr <<
"Error: dt cannot be zero" << std::endl;
191 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
200 if ((
pt_ + 1) <
N_) {
203 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
216 for (
int i = 0; i < dim_; ++i) {
221 for (
unsigned int j = 0; j <
N_; ++j) {
223 if (idx >=
N_) idx -=
N_;
225 c0_[i] += x * pinv0_[j];
226 c1_[i] += x * pinv1_[j];
227 c2_[i] += x * pinv2_[j];
231 esteem[i] = 0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
238 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
242 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = c2_[i] * tmed_ + c1_[i];
246 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = c2_[i];
250 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;
std::vector< double > time_list_
Time vector corresponding to each element in elem_list_.
virtual void estimate(std::vector< double > &estimee, const std::vector< double > &el)
QuadEstimator(const unsigned int &N, const unsigned int &dim, const double &dt=0.0)
Eigen::VectorXd coeff_
Coefficients for the least squares solution.
bool dt_zero_
Indicate that dt is zero (dt is invalid)
virtual void getEstimateDerivative(std::vector< double > &estimeeDerivative, const unsigned int order)
double dt_
Sampling (control) time.
std::vector< std::vector< double > > elem_list_
All the data (N elements of size dim)
unsigned int N_
Window length.
virtual void estimateRecursive(std::vector< double > &estimee, const std::vector< double > &el, const double &time)
unsigned int pt_
Circular index to each data and time element.
void pinv(const Eigen::MatrixXd &matrix_in, Eigen::MatrixXd &pseudo_inv, const double &pinvtoler=1.0e-6)
std::vector< double > t_
Time vector setting the lowest time to zero (for numerical stability).