Head

GCC Code Coverage Report

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


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

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

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

  /* Initialize sums for the recursive computation */
  sum_xi_.resize(dim);
  sum_tixi_.resize(dim);
  for (unsigned int i = 0; i < dim; ++i) {
    sum_xi_.at(i) = 0.0;
    sum_tixi_.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_, 2);
    Eigen::MatrixXd pinvTmat(2, N_);
    double time = 0.0;
    for (unsigned int i = 0; i < N_; ++i) {
      Tmat(i, 1) = time;
      Tmat(i, 0) = 1.0;
      time += dt_;
    }
    /* Half time used to estimate the position */
    tmed_ = time * 0.5;

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

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

double LinEstimator::getEsteeme() { return coeff_(1); }

void LinEstimator::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;
      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;
      }
      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;
  double den = N_ * sum_ti2_ - sum_ti_ * sum_ti_;

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

    // the bias
    // coeff_(0) = (sum_xi*sum_titi - sum_ti*sum_tixi) / den;
    // the linear coefficient
    esteem[i] = (N_ * sum_tixi_[i] - sum_ti_ * sum_xi_[i]) / den;

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

  return;
}

void LinEstimator::fit() {
  double sum_ti = 0.0;
  double sum_titi = 0.0;
  double sum_xi = 0.0;
  double sum_tixi = 0.0;

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

  double den = N_ * sum_titi - sum_ti * sum_ti;

  // the bias
  coeff_(0) = (sum_xi * sum_titi - sum_ti * sum_tixi) / den;

  // the linear coefficient
  coeff_(1) = (N_ * sum_tixi - sum_ti * sum_xi) / den;

  return;
}

void LinEstimator::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;
    // 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];
    }

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

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

    case 1:
      for (int i = 0; i < dim_; ++i) estimateDerivative[i] = c1_[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 first order that fits some data.
5			*/
6
7			#include <iostream>
8			#include <sot/torque_control/utils/lin-estimator.hh>
9
10			LinEstimator::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
55			double LinEstimator::getEsteeme() { return coeff_(1); }
56
57			void 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_xisum_titi - sum_tisum_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
125			void 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
149			void 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
195			void LinEstimator::getEstimateDerivative(
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			}