iCub-main
adaptWinPolyEstimator.cpp
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
4  * All rights reserved.
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 
iCub::ctrl::AWPolyEstimator::D
double D
Definition: adaptWinPolyEstimator.h:79
iCub::ctrl::AWPolyEstimator::fit
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,...
Definition: adaptWinPolyEstimator.cpp:55
iCub::ctrl::AWPolyEstimator::t
yarp::sig::Vector t
Definition: adaptWinPolyEstimator.h:81
iCub::ctrl::AWPolyEstimator::order
unsigned int order
Definition: adaptWinPolyEstimator.h:77
e
e
Definition: compute_ekf_fast.m:13
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::ctrl::AWPolyEstimator::estimate
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Definition: adaptWinPolyEstimator.cpp:100
iCub::ctrl::AWPolyEstimator::N
unsigned int N
Definition: adaptWinPolyEstimator.h:78
iCub::ctrl::AWPolyEstimator::x
yarp::sig::Vector x
Definition: adaptWinPolyEstimator.h:82
iCub::ctrl::AWPolyEstimator::elemList
AWPolyList elemList
Definition: adaptWinPolyEstimator.h:76
data
@ data
Definition: ST_M1_dataType.h:64
iCub::ctrl::AWPolyElement
Definition: adaptWinPolyEstimator.h:45
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::ctrl::AWPolyEstimator::coeff
yarp::sig::Vector coeff
Definition: adaptWinPolyEstimator.h:83
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
n
int n
Definition: debugFunctions.cpp:58
iCub::ctrl::AWPolyEstimator::reset
void reset()
Reinitialize the internal state.
Definition: adaptWinPolyEstimator.cpp:193
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
iCub::ctrl::AWPolyEstimator::firstRun
bool firstRun
Definition: adaptWinPolyEstimator.h:87
iCub::ctrl::AWPolyEstimator::eval
virtual double eval(double x)
Evaluate regressor at certain point.
Definition: adaptWinPolyEstimator.cpp:41
adaptWinPolyEstimator.h
iCub::ctrl::AWPolyEstimator::feedData
void feedData(const AWPolyElement &el)
Feed data into the algorithm.
Definition: adaptWinPolyEstimator.cpp:93
iCub::ctrl::AWPolyEstimator::getEsteeme
virtual double getEsteeme()=0
Return the current estimation.
iCub::ctrl::AWPolyEstimator::winLen
yarp::sig::Vector winLen
Definition: adaptWinPolyEstimator.h:84
iCub::ctrl::AWLinEstimator::fit
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.
Definition: adaptWinPolyEstimator.cpp:204
iCub::ctrl::AWPolyEstimator::mse
yarp::sig::Vector mse
Definition: adaptWinPolyEstimator.h:85