NimbRo ROS Soccer Package
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
KalmanFilter.h
1 #ifndef KALMANFILTER_H_
2 #define KALMANFILTER_H_
3 
4 #include <Eigen/Core>
5 
6 namespace margait_contrib
7 {
8 
9 /*
10 This is a simple Kalman Filter implementation that estimates the state of a one
11 dimensional process. The state consists of the position x, the velocity v, and
12 the acceleration a. Instantiate it, provide a time step parameter (default is
13 0.01 seconds), and provide smoothing parameters for the x, v, and a dimensions
14 (default 0.1, 0.1, 0.1). Keep calling the update(z) function periodically to
15 provide position measurements z. The output will be written into the public x,
16 v, and a variables.
17 
18 KalmanFilter kf;
19 kf.setTimeStep(0.01);
20 kf.setSmoothing(0.1, 0.1, 0.1);
21 
22 for_every_ten_milliseconds
23 {
24  double z = currentPositionMeasurement();
25  kf.update(z);
26 
27  double position = kf.x;
28  double velocity = kf.v;
29  double acceleration = kf.a;
30 }
31 
32 */
33 
34 class KalmanFilter
35 {
36  Eigen::Vector3d X;
37  Eigen::Vector3d Z;
38  Eigen::Matrix3d A;
39  Eigen::Matrix3d H;
40  Eigen::Matrix3d K;
41  Eigen::Matrix3d Q;
42  Eigen::Matrix3d R;
43  Eigen::Matrix3d P;
44  Eigen::Matrix3d I;
45 
46  double smoothing;
47  double timeStep;
48  double lastX;
49  double lastV;
50 
51  bool adaptiveGain;
52 
53 public:
54 
55  double x;
56  double v;
57  double a;
58  double px;
59  double pv;
60  double pa;
61 
62  KalmanFilter();
63  ~KalmanFilter(){};
64 
65  void init();
66  void setTimeStep(double t);
67  void setSmoothing(double s);
68  void reset(double xx=0, double vv=0, double aa=0);
69  void update(double z);
70 };
71 
72 }
73 
74 #endif // KALMANFILTER_H_