iCub-main
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10
11 #include <cmath>
12 #include <algorithm>
13
14 #include <yarp/os/LogStream.h>
15 #include <yarp/math/Math.h>
16 #include <yarp/math/SVD.h>
18
19 using namespace std;
20 using namespace yarp::os;
21 using namespace yarp::sig;
22 using namespace yarp::math;
23 using namespace iCub::ctrl;
24
25
26 /***************************************************************************/
27 AWPolyEstimator::AWPolyEstimator(unsigned int _order, unsigned int _N, const double _D) :
28  order(_order), N(_N), D(_D)
29 {
30  order=std::max(order,1U);
31  coeff.resize(order+1);
32  N=N<=order ? N+1 : N;
33  t.resize(N);
34  x.resize(N);
35
36  firstRun=true;
37 }
38
39
40 /***************************************************************************/
41 double AWPolyEstimator::eval(double x)
42 {
43  double y=coeff[0];
44  for (unsigned int i=1; i<=order; i++)
45  {
46  y+=coeff[i]*x;
47  x*=x;
48  }
49
50  return y;
51 }
52
53
54 /***************************************************************************/
55 Vector AWPolyEstimator::fit(const Vector &x, const Vector &y, const unsigned int n)
56 {
57  unsigned int i1=0;
58  unsigned int i2=(unsigned int)std::min(x.length(),y.length());
59  unsigned int M=i2;
60
61  if (n>0)
62  {
63  i1=i2-n;
64  M=n;
65  }
66
67  Matrix R(M,order+1);
68  Vector _y(M);
69
70  for (unsigned int i=i1; i<i2; i++)
71  {
72  double _x=x[i];
73
74  R(i-i1,0)=1.0;
75
76  for (unsigned int j=1; j<=order; j++)
77  {
78  R(i-i1,j)=_x;
79  _x*=_x;
80  }
81
82  _y[i-i1]=y[i];
83  }
84
85  if (R.rows()>=R.cols())
86  return pinv(R)*_y;
87  else
88  return pinv(R.transposed()).transposed()*_y;
89 }
90
91
92 /***************************************************************************/
94 {
95  elemList.push_back(el);
96 }
97
98
99 /***************************************************************************/
101 {
102  yAssert(elemList.size()>0);
103
104  size_t dim=elemList[0].data.length();
105  Vector esteem(dim,0.0);
106
107  if (firstRun)
108  {
109  winLen.resize(dim,N);
110  mse.resize(dim,0.0);
111  firstRun=false;
112  }
113
114  size_t L=elemList.size();
115  int delta=(int)L-(int)N;
116
117  if (delta<0)
118  return esteem;
119
120  // retrieve the time vector
121  // starting from t=0 (numeric stability reason)
122  t[0]=0.0;
123  for (unsigned int j=1; j<N; j++)
124  {
125  t[j]=elemList[delta+j].time-elemList[delta].time;
126
127  // enforce condition on time vector
128  if (t[j]<=0.0)
129  {
130  yWarning()<<"Provided non-increasing time vector";
131  return esteem;
132  }
133  }
134
135  // cycle upon all elements
136  for (unsigned int i=0; i<dim; i++)
137  {
138  // retrieve the data vector
139  for (unsigned int j=0; j<N; j++)
140  x[j]=elemList[delta+j].data[i];
141
142  // change the window length of two units, back and forth
143  unsigned int n1=(unsigned int)((winLen[i]>(order+1))?(winLen[i]-1):(order+1));
144  unsigned int n2=(unsigned int)((winLen[i]<N)?(winLen[i]+1):N);
145
146  // cycle upon all possibile window's length
147  for (unsigned int n=n1; n<=n2; n++)
148  {
149  // find the regressor's coefficients
150  coeff=fit(t,x,n);
151  bool _stop=false;
152
153  // test the regressor upon all the elements
154  // belonging to the actual window
155  mse[i]=0.0;
156  for (unsigned int k=N-n; k<N; k++)
157  {
158  double e=x[k]-eval(t[k]);
159  _stop|=(fabs(e)>D);
160  mse[i]+=e*e;
161  }
162  mse[i]/=n;
163
164  // set the new window's length in case of
165  // crossing of max deviation threshold
166  if (_stop)
167  {
168  winLen[i]=n;
169  break;
170  }
171  }
172
173  esteem[i]=getEsteeme();
174  }
175
176  int margin=delta-10;
177  if (margin>0)
178  elemList.erase(elemList.begin(),elemList.begin()+margin);
179
180  return esteem;
181 }
182
183
184 /***************************************************************************/
186 {
187  feedData(el);
188  return estimate();
189 }
190
191
192 /***************************************************************************/
194 {
195  if (elemList.size()>0)
196  {
197  winLen.resize(elemList[0].data.length(),N);
198  elemList.clear();
199  }
200 }
201
202
203 /***************************************************************************/
204 Vector AWLinEstimator::fit(const Vector &x, const Vector &y, const unsigned int n)
205 {
206  unsigned int i1=0;
207  unsigned int i2=(unsigned int)std::min(x.length(),y.length());
208  unsigned int M=i2;
209
210  if (n>0)
211  {
212  i1=i2-n;
213  M=n;
214  }
215
216  double sum_xi =0.0;
217  double sum_xixi=0.0;
218  double sum_yi =0.0;
219  double sum_xiyi=0.0;
220
221  for (unsigned int i=i1; i<i2; i++)
222  {
223  sum_xi +=x[i];
224  sum_xixi+=x[i]*x[i];
225  sum_yi +=y[i];
226  sum_xiyi+=x[i]*y[i];
227  }
228
229  double den=M*sum_xixi-sum_xi*sum_xi;
230
231  Vector c(2);
232
233  // the bias
234  c[0]=(sum_yi*sum_xixi-sum_xi*sum_xiyi) / den;
235
236  // the linear coefficient
237  c[1]=(M*sum_xiyi-sum_xi*sum_yi) / den;
238
239  return c;
240 }
241
242
243
@ data
virtual yarp::sig::Vector fit(const yarp::sig::Vector &x, const yarp::sig::Vector &y, const unsigned int n=0)
Redefine method to improve computation just for first-order estimator.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
void feedData(const AWPolyElement &el)
Feed data into the algorithm.
virtual double eval(double x)
Evaluate regressor at certain point.
void reset()
Reinitialize the internal state.
virtual double getEsteeme()=0
Return the current estimation.
virtual yarp::sig::Vector fit(const yarp::sig::Vector &x, const yarp::sig::Vector &y, const unsigned int n=0)
Find the regressor which best fits in least square sense the last n data sample couples,...
int n
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49