kalman_1d.cpp

00001 /***************************************************************************
00002  *  kalman_1d.cpp - Kalman filter (one dimensional)
00003  *
00004  *  Created: Mon Nov 10 2008
00005  *  Copyright  2008  Bahram Maleki-Fard
00006  *
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version. A runtime exception applies to
00013  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00021  */
00022 
00023 #include <utils/kalman/kalman_1d.h>
00024 #include <math.h>
00025 
00026 namespace fawkes {
00027 
00028 /** @class KalmanFilter1D <utils/kalman/kalman_1d.h>
00029  * One-dimensional Kalman filter implementation for single-precision floats.
00030  * @author Bahram Maleki-Fard
00031  */
00032 
00033 /** Constructor.
00034  * @param noise_x Transition noise, by default 1.0.
00035  * @param noise_z Sensor noise, by default 1.0.
00036  * @param mu Initial mu, by default 0.0.
00037  * @param sig Standard deviation, by default 1.0.
00038  */
00039 KalmanFilter1D::KalmanFilter1D(float noise_x, float noise_z, float mu, float sig)
00040 {
00041   __noise_x = noise_x;
00042   __noise_z = noise_z;
00043   __mu      = mu;
00044   __sig     = sig;
00045 }
00046 
00047 /** Destructor. */
00048 KalmanFilter1D::~KalmanFilter1D()
00049 {
00050 }
00051 
00052 /** Filters an observation. The internal mean and deviation are updated.
00053  * @param observe The observation.
00054  */
00055 void
00056 KalmanFilter1D::filter(float observe)
00057 {
00058   float help = __sig*__sig + __noise_x*__noise_x + __noise_z*__noise_z;
00059   __mu  = ((__sig*__sig + __noise_x*__noise_x) * observe + __noise_z*__noise_z*__mu) / help;
00060   __sig = sqrt( (__sig*__sig + __noise_x*__noise_x)*__noise_z*__noise_z / help );
00061 }
00062 
00063 
00064 /** Filters an observation. The resulting mu and sig are not only stored
00065  * internally, but also in the given parameters mean and deviation.
00066  * @param observe The observation.
00067  * @param mu The mean (out parameter).
00068  * @param sig The deviation (out parameter)
00069  */
00070 void
00071 KalmanFilter1D::filter(float observe, float& mu, float& sig)
00072 {
00073   mu  = __mu;
00074   sig = __sig;
00075 }
00076 
00077 /** Predicts the next position based on the past observations. Equivalent
00078  * to predict(0.0), i.e. velocity 0.0.
00079  * @return predicted value
00080  */
00081 float
00082 KalmanFilter1D::predict() const
00083 {
00084   return predict(0.0);
00085 }
00086 
00087 
00088 /** Predicts the next position based on the past observations. Equivalent
00089  * to predict(vel, 1, 0.0). 
00090  * @param vel The velocity of the object, 0.0 by default.
00091  * @return predicted value
00092  */
00093 float
00094 KalmanFilter1D::predict(float vel) const
00095 {
00096   return predict(vel, 1, 0.0);
00097 }
00098 
00099 
00100 /** Predicts the next position based on the past observations.
00101  * @param vel The velocity of the object.
00102  * @param steps The steps to look into the future.
00103  * @param noise_z Sensor noise.
00104  * @return predicted value
00105  */
00106 float
00107 KalmanFilter1D::predict(float vel, int steps, float noise_z) const
00108 {
00109   return predict(__mu, vel, steps, noise_z);
00110 }
00111 
00112 /** Predicts the next position based on the past observations.
00113  * @param mu Explicitely 
00114  * @param vel The velocity of the object, 0.0 by default.
00115  * @param steps The steps to look into the future, 1 by default.
00116  * @param noise_z Sensor noise.
00117  * @return predicted value
00118  */
00119 float
00120 KalmanFilter1D::predict(float mu, float vel, int steps, float noise_z) const
00121 {
00122   return mu + steps * (vel + noise_z);
00123 }
00124 
00125 }
00126 

Generated on Tue Feb 22 13:32:28 2011 for Fawkes API by  doxygen 1.4.7