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 #include <math.h>
25 
26 namespace fawkes {
27 
28 /** @class KalmanFilter1D <utils/kalman/kalman_1d.h>
29  * One-dimensional Kalman filter implementation for single-precision floats.
30  * @author Bahram Maleki-Fard
31  */
32 
33 /** Constructor.
34  * @param noise_x Transition noise, by default 1.0.
35  * @param noise_z Sensor noise, by default 1.0.
36  * @param mu Initial mu, by default 0.0.
37  * @param sig Standard deviation, by default 1.0.
38  */
39 KalmanFilter1D::KalmanFilter1D(float noise_x, float noise_z, float mu, float sig)
40 {
41  __noise_x = noise_x;
42  __noise_z = noise_z;
43  __mu = mu;
44  __sig = sig;
45 }
46 
47 /** Destructor. */
49 {
50 }
51 
52 /** Filters an observation. The internal mean and deviation are updated.
53  * @param observe The observation.
54  */
55 void
56 KalmanFilter1D::filter(float observe)
57 {
58  float help = __sig*__sig + __noise_x*__noise_x + __noise_z*__noise_z;
59  __mu = ((__sig*__sig + __noise_x*__noise_x) * observe + __noise_z*__noise_z*__mu) / help;
60  __sig = sqrt( (__sig*__sig + __noise_x*__noise_x)*__noise_z*__noise_z / help );
61 }
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 
88 /** Predicts the next position based on the past observations. Equivalent
89  * to predict(vel, 1, 0.0).
90  * @param vel The velocity of the object, 0.0 by default.
91  * @return predicted value
92  */
93 float
94 KalmanFilter1D::predict(float vel) const
95 {
96  return predict(vel, 1, 0.0);
97 }
98 
99 
100 /** Predicts the next position based on the past observations.
101  * @param vel The velocity of the object.
102  * @param steps The steps to look into the future.
103  * @param noise_z Sensor noise.
104  * @return predicted value
105  */
106 float
107 KalmanFilter1D::predict(float vel, int steps, float noise_z) const
108 {
109  return predict(__mu, vel, steps, noise_z);
110 }
111 
112 /** Predicts the next position based on the past observations.
113  * @param mu Explicitely
114  * @param vel The velocity of the object, 0.0 by default.
115  * @param steps The steps to look into the future, 1 by default.
116  * @param noise_z Sensor noise.
117  * @return predicted value
118  */
119 float
120 KalmanFilter1D::predict(float mu, float vel, int steps, float noise_z) const
121 {
122  return mu + steps * (vel + noise_z);
123 }
124 
125 }
126 
float predict() const
Predicts the next position based on the past observations.
Definition: kalman_1d.cpp:82
Fawkes library namespace.
~KalmanFilter1D()
Destructor.
Definition: kalman_1d.cpp:48
void filter(float observe)
Filters an observation.
Definition: kalman_1d.cpp:56
KalmanFilter1D(float noise_x=1.0, float noise_z=1.0, float mu=0.0, float sig=1.0)
Constructor.
Definition: kalman_1d.cpp:39