Fawkes API  Fawkes Development Version
kalman_1d.cpp
1 /***************************************************************************
2  * kalman_1d.cpp - Kalman filter (one dimensional)
3  *
4  * Created: Mon Nov 10 2008
5  * Copyright 2008 Bahram Maleki-Fard
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version. A runtime exception applies to
13  * this software (see LICENSE.GPL_WRE file mentioned below for details).
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21  */
22 
23 #include <utils/kalman/kalman_1d.h>
24 
25 #include <math.h>
26 
27 namespace fawkes {
28 
29 /** @class KalmanFilter1D <utils/kalman/kalman_1d.h>
30  * One-dimensional Kalman filter implementation for single-precision floats.
31  * @author Bahram Maleki-Fard
32  */
33 
34 /** Constructor.
35  * @param noise_x Transition noise, by default 1.0.
36  * @param noise_z Sensor noise, by default 1.0.
37  * @param mu Initial mu, by default 0.0.
38  * @param sig Standard deviation, by default 1.0.
39  */
40 KalmanFilter1D::KalmanFilter1D(float noise_x, float noise_z, float mu, float sig)
41 {
42  noise_x_ = noise_x;
43  noise_z_ = noise_z;
44  mu_ = mu;
45  sig_ = sig;
46 }
47 
48 /** Destructor. */
50 {
51 }
52 
53 /** Filters an observation. The internal mean and deviation are updated.
54  * @param observe The observation.
55  */
56 void
57 KalmanFilter1D::filter(float observe)
58 {
59  float help = sig_ * sig_ + noise_x_ * noise_x_ + noise_z_ * noise_z_;
60  mu_ = ((sig_ * sig_ + noise_x_ * noise_x_) * observe + noise_z_ * noise_z_ * mu_) / help;
61  sig_ = sqrt((sig_ * sig_ + noise_x_ * noise_x_) * noise_z_ * noise_z_ / help);
62 }
63 
64 /** Filters an observation. The resulting mu and sig are not only stored
65  * internally, but also in the given parameters mean and deviation.
66  * @param observe The observation.
67  * @param mu The mean (out parameter).
68  * @param sig The deviation (out parameter)
69  */
70 void
71 KalmanFilter1D::filter(float observe, float &mu, float &sig)
72 {
73  mu = mu_;
74  sig = sig_;
75 }
76 
77 /** Predicts the next position based on the past observations. Equivalent
78  * to predict(0.0), i.e. velocity 0.0.
79  * @return predicted value
80  */
81 float
83 {
84  return predict(0.0);
85 }
86 
87 /** Predicts the next position based on the past observations. Equivalent
88  * to predict(vel, 1, 0.0).
89  * @param vel The velocity of the object, 0.0 by default.
90  * @return predicted value
91  */
92 float
93 KalmanFilter1D::predict(float vel) const
94 {
95  return predict(vel, 1, 0.0);
96 }
97 
98 /** Predicts the next position based on the past observations.
99  * @param vel The velocity of the object.
100  * @param steps The steps to look into the future.
101  * @param noise_z Sensor noise.
102  * @return predicted value
103  */
104 float
105 KalmanFilter1D::predict(float vel, int steps, float noise_z) const
106 {
107  return predict(mu_, vel, steps, noise_z);
108 }
109 
110 /** Predicts the next position based on the past observations.
111  * @param mu Explicitely
112  * @param vel The velocity of the object, 0.0 by default.
113  * @param steps The steps to look into the future, 1 by default.
114  * @param noise_z Sensor noise.
115  * @return predicted value
116  */
117 float
118 KalmanFilter1D::predict(float mu, float vel, int steps, float noise_z) const
119 {
120  return mu + steps * (vel + noise_z);
121 }
122 
123 } // namespace fawkes
float predict() const
Predicts the next position based on the past observations.
Definition: kalman_1d.cpp:82
void filter(float observe)
Filters an observation.
Definition: kalman_1d.cpp:57
KalmanFilter1D(float noise_x=1.0, float noise_z=1.0, float mu=0.0, float sig=1.0)
Constructor.
Definition: kalman_1d.cpp:40
~KalmanFilter1D()
Destructor.
Definition: kalman_1d.cpp:49
Fawkes library namespace.