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
27namespace 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 */
40KalmanFilter1D::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 */
56void
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 */
70void
71KalmanFilter1D::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 */
81float
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 */
92float
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 */
104float
105KalmanFilter1D::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 */
117float
118KalmanFilter1D::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.