Fawkes API Fawkes Development Version
|
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