ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
testKalmanAcceleration.cpp
1 /****************************************************************************
2  *
3  * $Id: testKalmanAcceleration.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Tests some vpLinearKalmanFilterInstantiation functionalities.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
49 #include <visp/vpLinearKalmanFilterInstantiation.h>
50 #include <iostream>
51 #include <fstream>
52 
53 int
54 main()
55 {
56  unsigned int nsignal = 1; // Number of signal to filter
57  unsigned int niter = 100;
58 
59  std::string filename = "/tmp/log.dat";
60  std::ofstream flog(filename.c_str());
61 
63 
66  kalman.setStateModel(model);
67 
68  unsigned int size_state_vector = kalman.getStateSize()*nsignal;
69  unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
70 
71  vpColVector sigma_measure(size_measure_vector);
72  for (unsigned int signal=0; signal < nsignal; signal ++)
73  sigma_measure = 0.0001;
74  vpColVector sigma_state(size_state_vector);
75  for (unsigned int signal=0; signal < nsignal; signal ++) {
76  sigma_state[3*signal] = 0.; // not used
77  sigma_state[3*signal+1] = 0.000001;
78  sigma_state[3*signal+2] = 0.000001;
79  }
80 
81  vpColVector velocity_measure(size_measure_vector);
82 
83  double rho = 0.9; // correlation
84  double dt = 0.2; // sampling period
85 
86  for (unsigned int signal=0; signal < nsignal; signal ++)
87  velocity_measure[signal] = 3+2*signal;
88 
89  kalman.verbose(false);
90  kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
91 
92 
93  for (unsigned int iter=0; iter <= niter; iter++) {
94  std::cout << "-------- iter " << iter << " ------------" << std::endl;
95  for (unsigned int signal=0; signal < nsignal; signal ++) {
96  velocity_measure[signal] = 3+2*signal
97  + 0.3*sin(vpMath::rad(360./niter*iter));
98  }
99  std::cout << "measure : " << velocity_measure.t() << std::endl;
100 
101  flog << velocity_measure.t();
102 
103  // kalman.prediction();
104  kalman.filter(velocity_measure);
105  flog << kalman.Xest.t();
106  flog << kalman.Xpre.t();
107 
108  std::cout << "Xest: " << kalman.Xest.t() << std::endl;
109  std::cout << "Xpre: " << kalman.Xpre.t() << std::endl;
110 
111  flog << std::endl;
112  }
113 
114  flog.close();
115  return 0;
116 }
static double rad(double deg)
Definition: vpMath.h:100
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
This class provides an implementation of some specific linear Kalman filters.