nonholonomicEKFilter.h
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
32 #ifndef _NONHOLONOMICKALMANFILTER_H_
33 #define _NONHOLONOMICKALMANFILTER_H_
34 
35 #include <algorithm>
36 #include <vector>
37 #include <iostream>
38 
39 #include <ros/ros.h>
40 
41 #include <boost/shared_ptr.hpp>
42 #include <boost/format.hpp>
43 
44 #include <Eigen/Dense>
45 #include <Eigen/Cholesky>
46 
47 #include <kfilter/ekfilter.hpp>
48 
49 using Eigen::Matrix4d;
50 using Eigen::Matrix2d;
51 using Eigen::Vector2d;
52 using Eigen::Vector4d;
53 using Eigen::MatrixXd;
54 using Eigen::VectorXd;
55 
56 using namespace Kalman;
57 using namespace ros;
58 using namespace std;
59 
60 typedef Eigen::Matrix<double, 5, 1> Vector5d;
61 typedef Eigen::Matrix<double, 3, 1> Vector3d;
62 typedef Eigen::Matrix<double, 5, 5> Matrix5d;
63 
64 
65 typedef EKFilter<double,0,false,false,false>::Vector kVector;
66 typedef EKFilter<double,0,false,false,false>::Matrix kMatrix;
67 
68 class nonholonomicEKFilter;
69 typedef boost::shared_ptr<nonholonomicEKFilter> nonholonomicEKFilterPtr;
70 
75 class nonholonomicEKFilter:public EKFilter<double,0,false,false,false>
76 {
77  public:
78 
88  void SetIdentity(kMatrix& M,int size,double value=1)
89  {
90  for(int l=0;l<size;l++)
91  for(int c=0;c<size;c++)
92  if(c==l)
93  M(l,c)=value;
94  else
95  M(l,c)=0;
96  }
97 
98 
107  void SetZero(kMatrix& M,int size)
108  {
109  for(int l=0;l<size;l++)
110  for(int c=0;c<size;c++)
111  M(l,c)=0;
112  }
113 
120  void InitFilter(Vector5d& x_init)
121  {
122  setDim(5,0,5,3,3);
123  dt=1./50.;
124  lt=Time::now();
125 
126  Vector x_0(5);
127 
128  x_0(0)=x_init(0);//x
129  x_0(1)=x_init(1);//y
130  x_0(2)=x_init(2);//v
131  x_0(3)=x_init(3);//t
132  x_0(4)=x_init(4);//f
133 
134  Matrix P_0(5,5);
135 
136  miss_associations=0;
137 
138  //Large initial uncertainty
139  SetIdentity(P_0,5,4.0);
140 
141  init(x_0,P_0);
142  }
143 
144  void InitFilter(Vector5d& x_init,Matrix5d& P_init)
145  {
146  setDim(5,0,5,3,3);
147  dt=1./50.;
148  lt=Time::now();
149 
150  Vector x_0(5);
151 
152  x_0(0)=x_init(0);//x
153  x_0(1)=x_init(1);//y
154  x_0(2)=x_init(2);//v
155  x_0(3)=x_init(3);//t
156  x_0(4)=x_init(4);//f
157 
158  Matrix P_0(5,5);
159 
160  P_0(0,0)=P_init(0,0);
161  P_0(0,1)=P_init(0,1);
162  P_0(0,2)=P_init(0,2);
163  P_0(0,3)=P_init(0,3);
164  P_0(0,4)=P_init(0,4);
165 
166  P_0(1,0)=P_init(1,0);
167  P_0(1,1)=P_init(1,1);
168  P_0(1,2)=P_init(1,2);
169  P_0(1,3)=P_init(1,3);
170  P_0(1,4)=P_init(1,4);
171 
172  P_0(2,0)=P_init(2,0);
173  P_0(2,1)=P_init(2,1);
174  P_0(2,2)=P_init(2,2);
175  P_0(2,3)=P_init(2,3);
176  P_0(2,4)=P_init(2,4);
177 
178  P_0(3,0)=P_init(3,0);
179  P_0(3,1)=P_init(3,1);
180  P_0(3,2)=P_init(3,2);
181  P_0(3,3)=P_init(3,3);
182  P_0(3,4)=P_init(3,4);
183 
184  P_0(4,0)=P_init(4,0);
185  P_0(4,1)=P_init(4,1);
186  P_0(4,2)=P_init(4,2);
187  P_0(4,3)=P_init(4,3);
188  P_0(4,4)=P_init(4,4);
189 
190  init(x_0,P_0);
191 
192  }
193 
194  int miss_associations;
195  int life_time;
196 
197  Vector5d x_predicted;
198  Vector3d z_measured;
199  Vector3d inovation_error;
200 
201  protected:
202 
204  double dt;
206  Time lt;
207 
208  double l;
209 
215  void makeCommonProcess()
216  {
217  dt=1./50.;
218  l=2.5;
219  }
220 
224  void makeA()
225  {
226  double v=x(2);
227  double t=x(3);
228  double f=x(4);
229 
230  A(0,0)=1.0;
231  A(0,1)=0.0;
232  A(0,2)=cos(t)*cos(f)*dt;
233  A(0,3)=-sin(t)*cos(f)*v*dt;
234  A(0,4)=cos(t)*(-sin(f))*v*dt;
235 
236  A(1,0)=0.0;
237  A(1,1)=1.0;
238  A(1,2)=sin(t)*cos(f)*dt;
239  A(1,3)=cos(t)*cos(f)*v*dt;
240  A(1,4)=sin(t)*(-sin(f))*v*dt;
241 
242  A(2,0)=0.0;
243  A(2,1)=0.0;
244  A(2,2)=1.0;
245  A(2,3)=0.0;
246  A(2,4)=0.0;
247 
248  A(3,0)=0.0;
249  A(3,1)=0.0;
250  A(3,2)=sin(f)*dt/l;
251  A(3,3)=1.0;
252  A(3,4)=cos(f)/l*v*dt;
253 
254  A(4,0)=0.0;
255  A(4,1)=0.0;
256  A(4,2)=0.0;
257  A(4,3)=0.0;
258  A(4,4)=1.0;
259  }
260 
264  void makeH()
265  {
266 // if(miss_associations==0)
267 // {
268  H(0,0) = 1;
269  H(0,1) = 0;
270  H(0,2) = 0;
271  H(0,3) = 0;
272  H(0,4) = 0;
273 
274  H(1,0) = 0;
275  H(1,1) = 1;
276  H(1,2) = 0;
277  H(1,3) = 0;
278  H(1,4) = 0;
279 
280  H(2,0) = 0;
281  H(2,1) = 0;
282  H(2,2) = 0;
283  H(2,3) = 1;
284  H(2,4) = 0;
285 // }else
286 // {
287 // H(0,0) = 0;
288 // H(0,1) = 0;
289 // H(0,2) = 0;
290 // H(0,3) = 0;
291 // H(0,4) = 0;
292 //
293 // H(1,0) = 0;
294 // H(1,1) = 0;
295 // H(1,2) = 0;
296 // H(1,3) = 0;
297 // H(1,4) = 0;
298 //
299 // H(2,0) = 0;
300 // H(2,1) = 0;
301 // H(2,2) = 0;
302 // H(2,3) = 0;
303 // H(2,4) = 0;
304 // }
305 //
306 
307  }
308 
312  void makeBaseW()
313  {
314  SetIdentity(W,5,1.0);
315  }
316 
320  void makeBaseV()
321  {
322  SetIdentity(V,3,1.0);
323  }
324 
328  void makeR()
329  {
330  Matrix2d eigval;
331 
332  double inovation_factor = 5.0;
333  double distortion_y = 0.1;
334 
335  double x_min = 0.2;
336  double y_min = 0.2;
337 
338  double x_max = 6.0;
339  double y_max = 6.0;
340 
341  double x_size = inovation_error.norm()*inovation_factor;
342  double y_size = inovation_error.norm()*inovation_factor*distortion_y;
343 
344  double dir = atan2(inovation_error(1),inovation_error(0));
345 
346  cout<<"miss_associations: "<<miss_associations<<endl;
347 
348  if(miss_associations>0)
349  {
350  x_size+=20;
351  y_size+=20;
352  }
353 
354  if(y_size<y_min)
355  y_size = y_min;
356 
357  if(x_size<x_min)
358  x_size = x_min;
359 
360  if(y_size>y_max)
361  y_size = y_max;
362 
363  if(x_size>x_max)
364  x_size = x_max;
365 
366  eigval << x_size, 0,
367  0, y_size;
368 
369  Matrix2d rot;
370  rot << cos(dir), -sin(dir),
371  sin(dir), cos(dir);
372 
373  Matrix2d cov = rot*eigval*rot.inverse();
374 
375  R(0,0) = cov(0,0);
376  R(0,1) = cov(0,1);
377  //
378  R(1,0) = cov(1,0);
379  R(1,1) = cov(1,1);
380 
381 // R(0,0) = 0.200;//x
382 // R(0,1) = 0.000;
383  R(0,2) = 0.000;
384 
385 // R(1,0) = 0.000;//y
386 // R(1,1) = 0.200;
387  R(1,2) = 0.000;
388 
389  R(2,0) = 0.000;//t
390  R(2,1) = 0.000;
391  R(2,2) = 0.010;
392  }
393 
397  void makeQ()
398  {
399  double q=2.0;
400 
401  //X
402  Q(0,0) = q*0.1;
403  Q(0,1) = 0.0;
404  Q(0,2) = 0.0;
405  Q(0,3) = 0.0;
406  Q(0,4) = 0.0;
407 
408  //Y
409  Q(1,0) = 0.0;
410  Q(1,1) = q*0.1;
411  Q(1,2) = 0.0;
412  Q(1,3) = 0.0;
413  Q(1,4) = 0.0;
414 
415  //Vl
416  Q(2,0) = 0.0;
417  Q(2,1) = 0.0;
418  Q(2,2) = q*20.;
419  Q(2,3) = 0.0;
420  Q(2,4) = 0.0;
421 
422  //Theta
423  Q(3,0) = 0.0;
424  Q(3,1) = 0.0;
425  Q(3,2) = 0.0;
426  Q(3,3) = q*0.01;
427  Q(3,4) = 0.0;
428 
429  //Fi
430  Q(4,0) = 0.0;
431  Q(4,1) = 0.0;
432  Q(4,2) = 0.0;
433  Q(4,3) = 0.0;
434  Q(4,4) = q*0.01;
435 
436  }
437 
441  void makeProcess()
442  {
443 
444  // x(0) -> x
445  // x(1) -> y
446  // x(2) -> v
447  // x(3) -> t
448  // x(4) -> f
449 
450  Vector x_(x.size());
451 
452  x_(0) = x(0) + cos(x(3))*cos(x(4))*x(2)*dt;
453  x_(1) = x(1) + sin(x(3))*cos(x(4))*x(2)*dt;
454  x_(2) = x(2);
455  x_(3) = x(3) + sin(x(4))*x(2)*dt/l;
456  x_(4) = x(4);
457 
458 
459  x.swap(x_);
460  }
461 
465  void makeMeasure()
466  {
467  z(0)=x(0);//x
468  z(1)=x(1);//y
469  z(2)=x(3);//t
470  }
471 };
472 
473 #endif
nonholonomicEKFilter::dt
double dt
Time interval between measurements.
Definition: nonholonomicEKFilter.h:204
Matrix
EKFilter< double, 0, false, false, false >::Matrix Matrix
EKfilter matrix type.
Definition: types_declaration.h:183
kMatrix
EKFilter< double, 0, false, false, false >::Matrix kMatrix
EKfilter matrix type.
Definition: mht_types.h:86
nonholonomicEKFilter::InitFilter
void InitFilter(Vector5d &x_init)
Init filter.
Definition: nonholonomicEKFilter.h:120
nonholonomicEKFilter::makeBaseW
void makeBaseW()
Make process noise sensitivity matrix.
Definition: nonholonomicEKFilter.h:312
nonholonomicEKFilter
nonholonomic Kalman filter
Definition: nonholonomicEKFilter.h:75
nonholonomicEKFilter::makeH
void makeH()
Make measurement sensitivity matrix.
Definition: nonholonomicEKFilter.h:264
nonholonomicEKFilter::SetZero
void SetZero(kMatrix &M, int size)
Set a matrix to zero.
Definition: nonholonomicEKFilter.h:107
nonholonomicEKFilter::SetIdentity
void SetIdentity(kMatrix &M, int size, double value=1)
Set a matrix to identity.
Definition: nonholonomicEKFilter.h:88
nonholonomicEKFilter::makeA
void makeA()
Make the process Jacobian matrix.
Definition: nonholonomicEKFilter.h:224
Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
Definition: nonholonomicEKFilter.h:61
nonholonomicEKFilter::makeQ
void makeQ()
Make process noise covariance matrix.
Definition: nonholonomicEKFilter.h:397
nonholonomicEKFilter::lt
Time lt
Time of the last call to the makeCommonProcess function.
Definition: nonholonomicEKFilter.h:206
t
Timer t
Definition: k_best.cpp:34
nonholonomicEKFilter::InitFilter
void InitFilter(Vector5d &x_init, Matrix5d &P_init)
Definition: nonholonomicEKFilter.h:144
Matrix
Definition: matrix.h:32
nonholonomicEKFilter::makeProcess
void makeProcess()
Make process, model iteration.
Definition: nonholonomicEKFilter.h:441
nonholonomicEKFilter::makeCommonProcess
void makeCommonProcess()
This function is called before all other make something functions.
Definition: nonholonomicEKFilter.h:215
nonholonomicEKFilterPtr
boost::shared_ptr< nonholonomicEKFilter > nonholonomicEKFilterPtr
Definition: nonholonomicEKFilter.h:68
kVector
EKFilter< double, 0, false, false, false >::Vector kVector
Definition: nonholonomicEKFilter.h:65
kMatrix
EKFilter< double, 0, false, false, false >::Matrix kMatrix
Definition: nonholonomicEKFilter.h:66
nonholonomicEKFilter::makeBaseV
void makeBaseV()
Make measurement noise sensitivity matrix.
Definition: nonholonomicEKFilter.h:320
Matrix5d
Eigen::Matrix< double, 5, 5 > Matrix5d
Definition: nonholonomicEKFilter.h:62
Vector
EKFilter< double, 0, false, false, false >::Vector Vector
EKfilter vector type.
Definition: types_declaration.h:176
nonholonomicEKFilter::makeMeasure
void makeMeasure()
Make measurement, used when measurement is not possible (i'm not using it now)
Definition: nonholonomicEKFilter.h:465
Vector5d
Eigen::Matrix< double, 5, 1 > Vector5d
Definition: nonholonomicEKFilter.h:60
nonholonomicEKFilter::makeR
void makeR()
Make measurement noise covariance matrix.
Definition: nonholonomicEKFilter.h:328

mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18