KalmanFilterAlg.h
Go to the documentation of this file.
1 /*
2  * KalmanFilterAlg.h
3  * Steven Block <sblock@udallas.edu>
4  * Aidan Medcalf <amedcalf@udallas.edu>
5  *
6  * A simplified one dimensional Kalman filter implementation - actually a single variable low pass filter ;-)
7  * Based on: http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/
8  */
9 
10 #ifndef _Kalman_h
11 #define _Kalman_h
12 
13 namespace beamlinereco {
14  class Kalman {
15  private:
16  /* Kalman filter variables */
17  double q; //process noise covariance
18  double r; //measurement noise covariance
19  double x; //value
20  double p; //estimation error covariance
21  double k; //kalman gain
22 
23  public:
24  Kalman(double process_noise, double sensor_noise, double estimated_error, double intial_value) {
25  /* The variables are x for the filtered value, q for the process noise,
26  r for the sensor noise, p for the estimated error and k for the Kalman Gain.
27  The state of the filter is defined by the values of these variables.
28 
29  The initial values for p is not very important since it is adjusted
30  during the process. It must be just high enough to narrow down.
31  The initial value for the readout is also not very important, since
32  it is updated during the process.
33  But tweaking the values for the process noise and sensor noise
34  is essential to get clear readouts.
35 
36  For large noise reduction, you can try to start from: (see http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/ )
37  q = 0.125
38  r = 32
39  p = 1023 //"large enough to narrow down"
40  e.g.
41  myVar = Kalman(0.125,32,1023,0);
42  */
43  this->q = process_noise;
44  this->r = sensor_noise;
45  this->p = estimated_error;
46  this->x = intial_value; //x will hold the iterated filtered value
47  }
48 
49  double GetFilteredValue(double measurement) {
50  /* Updates and gets the current measurement value */
51  //prediction update
52  //omit x = x
53  this->p = this->p + this->q;
54 
55  //measurement update
56  this->k = this->p / (this->p + this->r);
57  this->x = this->x + this->k * (measurement - this->x);
58  this->p = (1 - this->k) * this->p;
59 
60  return this->x;
61  }
62 
63  void SetParameters(double process_noise, double sensor_noise, double estimated_error) {
64  this->q = process_noise;
65  this->r = sensor_noise;
66  this->p = estimated_error;
67  }
68 
69  void SetParameters(double process_noise, double sensor_noise) {
70  this->q = process_noise;
71  this->r = sensor_noise;
72  }
73 
74  double GetProcessNoise() {
75  return this->q;
76  }
77 
78  double GetSensorNoise() {
79  return this->r;
80  }
81 
82  double GetEstimatedError() {
83  return this->p;
84  }
85  };
86 
87 } // namespace beamlinereco
88 
89 #endif
double GetFilteredValue(double measurement)
void SetParameters(double process_noise, double sensor_noise)
void SetParameters(double process_noise, double sensor_noise, double estimated_error)
Kalman(double process_noise, double sensor_noise, double estimated_error, double intial_value)