Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/quad-estimator.cpp	Lines:	0	158	0.0 %
Date:	2023-06-05 17:45:50	Branches:	0	108	0.0 %


/*
   Oscar Efrain RAMOS PONCE, LAAS-CNRS
   Date: 28/10/2014
   Object to estimate a polynomial of second order that fits some data.
*/

#include <iostream>
#include <sot/torque_control/utils/quad-estimator.hh>

QuadEstimator::QuadEstimator(const unsigned int& N, const unsigned int& dim,
                             const double& dt)
    : PolyEstimator(2, N, dt),
      dim_(dim),
      sum_ti_(0.0),
      sum_ti2_(0.0),
      sum_ti3_(0.0),
      sum_ti4_(0.0) {
  /* Number of coefficients for a quadratic estimator: 3 */
  coeff_.resize(3);

  /* Initialize sums for the recursive computation */
  sum_xi_.resize(dim);
  sum_tixi_.resize(dim);
  sum_ti2xi_.resize(dim);
  for (unsigned int i = 0; i < dim; ++i) {
    sum_xi_.at(i) = 0.0;
    sum_tixi_.at(i) = 0.0;
    sum_ti2xi_.at(i) = 0.0;
  }

  /* Initialize (pre-compute) pseudo inverse matrix that assumes that the sample
     time is constant (only if dt is not zero) */
  if (!dt_zero_) {
    Eigen::MatrixXd Tmat(N_, 3);
    Eigen::MatrixXd pinvTmat(3, N_);
    double time = 0.0;
    for (unsigned int i = 0; i < N_; ++i) {
      Tmat(i, 2) = 0.5 * time * time;
      Tmat(i, 1) = time;
      Tmat(i, 0) = 1.0;
      time += dt_;
    }
    /* Half time used to estimate velocity and position*/
    tmed_ = time * 0.5;

    /* Pseudo inverse */
    pinv(Tmat, pinvTmat);
    pinv0_ = new double[N_];
    pinv1_ = new double[N_];
    pinv2_ = new double[N_];
    c0_.resize(dim_);
    c1_.resize(dim_);
    c2_.resize(dim_);
    c0_.assign(dim_, 0.0);
    c1_.assign(dim_, 0.0);
    c2_.assign(dim_, 0.0);

    /* Copy the pseudo inverse to an array */
    for (unsigned int i = 0; i < N_; ++i) {
      pinv2_[i] = pinvTmat(2, i);
      pinv1_[i] = pinvTmat(1, i);
      pinv0_[i] = pinvTmat(0, i);
    }
  }
}

double QuadEstimator::getEsteeme() { return coeff_(2); }

void QuadEstimator::estimateRecursive(std::vector<double>& esteem,
                                      const std::vector<double>& el,
                                      const double& time) {
  /* Feed Data */
  elem_list_.at(pt_) = el;
  time_list_.at(pt_) = time;

  double t, x;

  if (first_run_) {
    // Not enough elements in the window
    if ((pt_ + 1) < N_) {
      // t = time - time_list_.at(0);
      t = time;
      sum_ti_ += t;
      sum_ti2_ += t * t;
      sum_ti3_ += t * t * t;
      sum_ti4_ += t * t * t * t;

      for (unsigned int i = 0; i < esteem.size(); ++i) {
        // Return a zero vector
        esteem.at(i) = 0.0;
        x = el.at(i);
        // Fill the sumations
        sum_xi_.at(i) += x;
        sum_tixi_.at(i) += t * x;
        sum_ti2xi_.at(i) += t * t * x;
      }
      pt_++;
      return;
    } else {
      first_run_ = false;
    }
  }

  // Increase the 'circular' pointer
  pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
  // The pointer now points to the "oldest" element in the vector

  // Get size of first element: N dof (assuming all elements have same size)
  size_t dim = elem_list_.at(0).size();
  // TODO: CHECK THAT SIZE OF ESTEEM = DIM

  // t = time - time_list_.at(pt_);
  t = time;
  sum_ti_ += t;
  sum_ti2_ += t * t;
  sum_ti3_ += t * t * t;
  sum_ti4_ += t * t * t * t;

  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_ +
      0.25 * sum_ti4_ * sum_ti_ * sum_ti_ - 0.25 * N_ * sum_ti4_ * sum_ti2_;
  double den2 = 1.0 / (2.0 * den);
  // double den4 = 1.0/(4.0*den);

  double t_old = time_list_.at(pt_);
  double a;
  // double b;
  // unsigned int pt_med;
  for (unsigned int i = 0; i < dim; ++i) {
    x = el[i];
    // Fill the sumations
    sum_xi_[i] += x;
    sum_tixi_[i] += t * x;
    sum_ti2xi_[i] += t * t * x;

    a = den2 * (sum_ti2xi_[i] * (sum_ti_ * sum_ti_ - N_ * sum_ti2_) +
                sum_tixi_[i] * (N_ * sum_ti3_ - sum_ti2_ * sum_ti_) +
                sum_xi_[i] * (sum_ti2_ * sum_ti2_ - sum_ti3_ * sum_ti_));
    // b = den4*( sum_ti2xi_[i]*(N_*sum_ti3_-sum_ti2_*sum_ti_) +
    //            sum_tixi_[i]*(sum_ti2_*sum_ti2_-N_*sum_ti4_) +
    //            sum_xi_[i]*(sum_ti4_*sum_ti_-sum_ti3_*sum_ti2_) );

    esteem[i] = a;  // For acceleration

    // pt_med = (pt_+((N_-1)/2)) % N_;
    // esteem[i] = a*time_list_[pt_med] + b;  // For velocity

    x = elem_list_[pt_][i];
    sum_xi_[i] -= x;
    sum_tixi_[i] -= t_old * x;
    sum_ti2xi_[i] -= t_old * t_old * x;
  }
  sum_ti_ -= t_old;
  sum_ti2_ -= t_old * t_old;
  sum_ti3_ -= t_old * t_old * t_old;
  sum_ti4_ -= t_old * t_old * t_old * t_old;

  return;
}

void QuadEstimator::fit() {
  double sum_ti = 0.0;
  double sum_ti2 = 0.0;
  double sum_ti3 = 0.0;
  double sum_ti4 = 0.0;
  double sum_xi = 0.0;
  double sum_tixi = 0.0;
  double sum_ti2xi = 0.0;

  for (unsigned int i = 0; i < N_; ++i) {
    sum_ti += t_[i];
    sum_ti2 += t_[i] * t_[i];
    sum_ti3 += t_[i] * t_[i] * t_[i];
    sum_ti4 += t_[i] * t_[i] * t_[i] * t_[i];
    sum_xi += x_[i];
    sum_tixi += t_[i] * x_[i];
    sum_ti2xi += t_[i] * t_[i] * x_[i];
  }

  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 +
               0.25 * sum_ti4 * sum_ti * sum_ti - 0.25 * N_ * sum_ti4 * sum_ti2;
  double den4 = 1.0 / (4.0 * den);

  coeff_(2) = den4 * (sum_ti2xi * (sum_ti * sum_ti - N_ * sum_ti2) +
                      sum_tixi * (N_ * sum_ti3 - sum_ti2 * sum_ti) +
                      sum_xi * (sum_ti2 * sum_ti2 - sum_ti3 * sum_ti));
  coeff_(1) = den4 * (sum_ti2xi * (N_ * sum_ti3 - sum_ti2 * sum_ti) +
                      sum_tixi * (sum_ti2 * sum_ti2 - N_ * sum_ti4) +
                      sum_xi * (sum_ti4 * sum_ti - sum_ti3 * sum_ti2));
  // This has not been computed (because not needed for accel or velocity)
  coeff_(0) = 0;

  return;
}

void QuadEstimator::estimate(std::vector<double>& esteem,
                             const std::vector<double>& el) {
  if (dt_zero_) {
    std::cerr << "Error: dt cannot be zero" << std::endl;
    // Return a zero vector
    for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
    return;
  }

  /* Feed Data. Note that the time is not completed since it is assumed to be
     constant */
  elem_list_[pt_] = el;

  if (first_run_) {
    if ((pt_ + 1) < N_) {
      // Return input vector when not enough elements to compute
      pt_++;
      for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
      return;
    } else
      first_run_ = false;
  }

  // Next pointer value
  pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;

  unsigned int idx;

  double x;
  // Cycle all the elements in the vector
  for (int i = 0; i < dim_; ++i) {
    c0_[i] = 0.0;
    c1_[i] = 0.0;
    c2_[i] = 0.0;
    // Retrieve the data in the window
    for (unsigned int j = 0; j < N_; ++j) {
      idx = (pt_ + j);
      if (idx >= N_) idx -= N_;
      x = elem_list_[idx][i];
      c0_[i] += x * pinv0_[j];
      c1_[i] += x * pinv1_[j];
      c2_[i] += x * pinv2_[j];
    }

    // Polynomial (position)
    esteem[i] = 0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
  }
}

void QuadEstimator::getEstimateDerivative(
    std::vector<double>& estimateDerivative, const unsigned int order) {
  switch (order) {
    case 0:
      for (int i = 0; i < dim_; ++i)
        estimateDerivative[i] =
            0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
      return;

    case 1:
      for (int i = 0; i < dim_; ++i)
        estimateDerivative[i] = c2_[i] * tmed_ + c1_[i];
      return;

    case 2:
      for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c2_[i];
      return;

    default:
      for (int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;
  }
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			Oscar Efrain RAMOS PONCE, LAAS-CNRS
3			Date: 28/10/2014
4			Object to estimate a polynomial of second order that fits some data.
5			*/
6
7			#include <iostream>
8			#include <sot/torque_control/utils/quad-estimator.hh>
9
10			QuadEstimator::QuadEstimator(const unsigned int& N, const unsigned int& dim,
11			const double& dt)
12			: PolyEstimator(2, N, dt),
13			dim_(dim),
14			sum_ti_(0.0),
15			sum_ti2_(0.0),
16			sum_ti3_(0.0),
17			sum_ti4_(0.0) {
18			/* Number of coefficients for a quadratic estimator: 3 */
19			coeff_.resize(3);
20
21			/* Initialize sums for the recursive computation */
22			sum_xi_.resize(dim);
23			sum_tixi_.resize(dim);
24			sum_ti2xi_.resize(dim);
25			for (unsigned int i = 0; i < dim; ++i) {
26			sum_xi_.at(i) = 0.0;
27			sum_tixi_.at(i) = 0.0;
28			sum_ti2xi_.at(i) = 0.0;
29			}
30
31			/* Initialize (pre-compute) pseudo inverse matrix that assumes that the sample
32			time is constant (only if dt is not zero) */
33			if (!dt_zero_) {
34			Eigen::MatrixXd Tmat(N_, 3);
35			Eigen::MatrixXd pinvTmat(3, N_);
36			double time = 0.0;
37			for (unsigned int i = 0; i < N_; ++i) {
38			Tmat(i, 2) = 0.5 * time * time;
39			Tmat(i, 1) = time;
40			Tmat(i, 0) = 1.0;
41			time += dt_;
42			}
43			/* Half time used to estimate velocity and position*/
44			tmed_ = time * 0.5;
45
46			/* Pseudo inverse */
47			pinv(Tmat, pinvTmat);
48			pinv0_ = new double[N_];
49			pinv1_ = new double[N_];
50			pinv2_ = new double[N_];
51			c0_.resize(dim_);
52			c1_.resize(dim_);
53			c2_.resize(dim_);
54			c0_.assign(dim_, 0.0);
55			c1_.assign(dim_, 0.0);
56			c2_.assign(dim_, 0.0);
57
58			/* Copy the pseudo inverse to an array */
59			for (unsigned int i = 0; i < N_; ++i) {
60			pinv2_[i] = pinvTmat(2, i);
61			pinv1_[i] = pinvTmat(1, i);
62			pinv0_[i] = pinvTmat(0, i);
63			}
64			}
65			}
66
67			double QuadEstimator::getEsteeme() { return coeff_(2); }
68
69			void QuadEstimator::estimateRecursive(std::vector<double>& esteem,
70			const std::vector<double>& el,
71			const double& time) {
72			/* Feed Data */
73			elem_list_.at(pt_) = el;
74			time_list_.at(pt_) = time;
75
76			double t, x;
77
78			if (first_run_) {
79			// Not enough elements in the window
80			if ((pt_ + 1) < N_) {
81			// t = time - time_list_.at(0);
82			t = time;
83			sum_ti_ += t;
84			sum_ti2_ += t * t;
85			sum_ti3_ += t * t * t;
86			sum_ti4_ += t * t * t * t;
87
88			for (unsigned int i = 0; i < esteem.size(); ++i) {
89			// Return a zero vector
90			esteem.at(i) = 0.0;
91			x = el.at(i);
92			// Fill the sumations
93			sum_xi_.at(i) += x;
94			sum_tixi_.at(i) += t * x;
95			sum_ti2xi_.at(i) += t * t * x;
96			}
97			pt_++;
98			return;
99			} else {
100			first_run_ = false;
101			}
102			}
103
104			// Increase the 'circular' pointer
105			pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
106			// The pointer now points to the "oldest" element in the vector
107
108			// Get size of first element: N dof (assuming all elements have same size)
109			size_t dim = elem_list_.at(0).size();
110			// TODO: CHECK THAT SIZE OF ESTEEM = DIM
111
112			// t = time - time_list_.at(pt_);
113			t = time;
114			sum_ti_ += t;
115			sum_ti2_ += t * t;
116			sum_ti3_ += t * t * t;
117			sum_ti4_ += t * t * t * t;
118
119			double den =
120			0.25 * N_ * sum_ti3_ * sum_ti3_ - 0.5 * sum_ti3_ * sum_ti2_ * sum_ti_ +
121			0.25 * sum_ti2_ * sum_ti2_ * sum_ti2_ +
122			0.25 * sum_ti4_ * sum_ti_ * sum_ti_ - 0.25 * N_ * sum_ti4_ * sum_ti2_;
123			double den2 = 1.0 / (2.0 * den);
124			// double den4 = 1.0/(4.0*den);
125
126			double t_old = time_list_.at(pt_);
127			double a;
128			// double b;
129			// unsigned int pt_med;
130			for (unsigned int i = 0; i < dim; ++i) {
131			x = el[i];
132			// Fill the sumations
133			sum_xi_[i] += x;
134			sum_tixi_[i] += t * x;
135			sum_ti2xi_[i] += t * t * x;
136
137			a = den2 * (sum_ti2xi_[i] * (sum_ti_ * sum_ti_ - N_ * sum_ti2_) +
138			sum_tixi_[i] * (N_ * sum_ti3_ - sum_ti2_ * sum_ti_) +
139			sum_xi_[i] * (sum_ti2_ * sum_ti2_ - sum_ti3_ * sum_ti_));
140			// b = den4( sum_ti2xi_[i](N_sum_ti3_-sum_ti2_sum_ti_) +
141			// sum_tixi_[i](sum_ti2_sum_ti2_-N_*sum_ti4_) +
142			// sum_xi_[i](sum_ti4_sum_ti_-sum_ti3_*sum_ti2_) );
143
144			esteem[i] = a; // For acceleration
145
146			// pt_med = (pt_+((N_-1)/2)) % N_;
147			// esteem[i] = a*time_list_[pt_med] + b; // For velocity
148
149			x = elem_list_[pt_][i];
150			sum_xi_[i] -= x;
151			sum_tixi_[i] -= t_old * x;
152			sum_ti2xi_[i] -= t_old * t_old * x;
153			}
154			sum_ti_ -= t_old;
155			sum_ti2_ -= t_old * t_old;
156			sum_ti3_ -= t_old * t_old * t_old;
157			sum_ti4_ -= t_old * t_old * t_old * t_old;
158
159			return;
160			}
161
162			void QuadEstimator::fit() {
163			double sum_ti = 0.0;
164			double sum_ti2 = 0.0;
165			double sum_ti3 = 0.0;
166			double sum_ti4 = 0.0;
167			double sum_xi = 0.0;
168			double sum_tixi = 0.0;
169			double sum_ti2xi = 0.0;
170
171			for (unsigned int i = 0; i < N_; ++i) {
172			sum_ti += t_[i];
173			sum_ti2 += t_[i] * t_[i];
174			sum_ti3 += t_[i] * t_[i] * t_[i];
175			sum_ti4 += t_[i] * t_[i] * t_[i] * t_[i];
176			sum_xi += x_[i];
177			sum_tixi += t_[i] * x_[i];
178			sum_ti2xi += t_[i] * t_[i] * x_[i];
179			}
180
181			double den = 0.25 * N_ * sum_ti3 * sum_ti3 -
182			0.5 * sum_ti3 * sum_ti2 * sum_ti +
183			0.25 * sum_ti2 * sum_ti2 * sum_ti2 +
184			0.25 * sum_ti4 * sum_ti * sum_ti - 0.25 * N_ * sum_ti4 * sum_ti2;
185			double den4 = 1.0 / (4.0 * den);
186
187			coeff_(2) = den4 * (sum_ti2xi * (sum_ti * sum_ti - N_ * sum_ti2) +
188			sum_tixi * (N_ * sum_ti3 - sum_ti2 * sum_ti) +
189			sum_xi * (sum_ti2 * sum_ti2 - sum_ti3 * sum_ti));
190			coeff_(1) = den4 * (sum_ti2xi * (N_ * sum_ti3 - sum_ti2 * sum_ti) +
191			sum_tixi * (sum_ti2 * sum_ti2 - N_ * sum_ti4) +
192			sum_xi * (sum_ti4 * sum_ti - sum_ti3 * sum_ti2));
193			// This has not been computed (because not needed for accel or velocity)
194			coeff_(0) = 0;
195
196			return;
197			}
198
199			void QuadEstimator::estimate(std::vector<double>& esteem,
200			const std::vector<double>& el) {
201			if (dt_zero_) {
202			std::cerr << "Error: dt cannot be zero" << std::endl;
203			// Return a zero vector
204			for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = 0.0;
205			return;
206			}
207
208			/* Feed Data. Note that the time is not completed since it is assumed to be
209			constant */
210			elem_list_[pt_] = el;
211
212			if (first_run_) {
213			if ((pt_ + 1) < N_) {
214			// Return input vector when not enough elements to compute
215			pt_++;
216			for (unsigned int i = 0; i < esteem.size(); ++i) esteem[i] = el[i];
217			return;
218			} else
219			first_run_ = false;
220			}
221
222			// Next pointer value
223			pt_ = (pt_ + 1) < N_ ? (pt_ + 1) : 0;
224
225			unsigned int idx;
226
227			double x;
228			// Cycle all the elements in the vector
229			for (int i = 0; i < dim_; ++i) {
230			c0_[i] = 0.0;
231			c1_[i] = 0.0;
232			c2_[i] = 0.0;
233			// Retrieve the data in the window
234			for (unsigned int j = 0; j < N_; ++j) {
235			idx = (pt_ + j);
236			if (idx >= N_) idx -= N_;
237			x = elem_list_[idx][i];
238			c0_[i] += x * pinv0_[j];
239			c1_[i] += x * pinv1_[j];
240			c2_[i] += x * pinv2_[j];
241			}
242
243			// Polynomial (position)
244			esteem[i] = 0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
245			}
246			}
247
248			void QuadEstimator::getEstimateDerivative(
249			std::vector<double>& estimateDerivative, const unsigned int order) {
250			switch (order) {
251			case 0:
252			for (int i = 0; i < dim_; ++i)
253			estimateDerivative[i] =
254			0.5 * c2_[i] * tmed_ * tmed_ + c1_[i] * tmed_ + c0_[i];
255			return;
256
257			case 1:
258			for (int i = 0; i < dim_; ++i)
259			estimateDerivative[i] = c2_[i] * tmed_ + c1_[i];
260			return;
261
262			case 2:
263			for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c2_[i];
264			return;
265
266			default:
267			for (int i = 0; i < dim_; ++i) estimateDerivative[i] = 0.0;
268			}
269			}