12 :
PolyEstimator(1, N, dt), dim_(dim), sum_ti_(0.0), sum_ti2_(0.0) {
18 sum_tixi_.resize(dim);
19 for (
unsigned int i = 0; i < dim; ++i) {
21 sum_tixi_.at(i) = 0.0;
27 Eigen::MatrixXd Tmat(
N_, 2);
28 Eigen::MatrixXd pinvTmat(2,
N_);
30 for (
unsigned int i = 0; i <
N_; ++i) {
40 pinv0_ =
new double[
N_];
41 pinv1_ =
new double[
N_];
44 c0_.assign(dim_, 0.0);
45 c1_.assign(dim_, 0.0);
48 for (
unsigned int i = 0; i <
N_; ++i) {
49 pinv0_[i] = pinvTmat(0, i);
50 pinv1_[i] = pinvTmat(1, i);
55 double LinEstimator::getEsteeme() {
return coeff_(1); }
71 for (
unsigned int i = 0; i < esteem.size(); ++i) {
77 sum_tixi_.at(i) += t * x;
98 double den =
N_ * sum_ti2_ - sum_ti_ * sum_ti_;
102 for (
unsigned int i = 0; i < dim; ++i) {
106 sum_tixi_[i] += t * x;
111 esteem[i] = (
N_ * sum_tixi_[i] - sum_ti_ * sum_xi_[i]) / den;
115 sum_tixi_[i] -= t_old * x;
118 sum_ti2_ -= t_old * t_old;
123 void LinEstimator::fit() {
125 double sum_titi = 0.0;
127 double sum_tixi = 0.0;
129 for (
unsigned int i = 0; i <
N_; ++i) {
131 sum_titi +=
t_[i] *
t_[i];
133 sum_tixi +=
t_[i] *
x_[i];
136 double den =
N_ * sum_titi - sum_ti * sum_ti;
139 coeff_(0) = (sum_xi * sum_titi - sum_ti * sum_tixi) / den;
142 coeff_(1) = (
N_ * sum_tixi - sum_ti * sum_xi) / den;
149 std::cerr <<
"Error: dt cannot be zero" << std::endl;
151 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
160 if ((
pt_ + 1) <
N_) {
163 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
175 for (
int i = 0; i < dim_; ++i) {
179 for (
unsigned int j = 0; j <
N_; ++j) {
181 if (idx >=
N_) idx -=
N_;
183 c0_[i] += x * pinv0_[j];
184 c1_[i] += x * pinv1_[j];
188 esteem[i] = c1_[i] * tmed_ + c0_[i];
195 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[i] * tmed_ + c0_[i];
199 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[i];
203 for (
int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;