Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "robot_localization/filter_base.h"
00034 #include "robot_localization/filter_common.h"
00035
00036 #include <sstream>
00037 #include <iomanip>
00038 #include <limits>
00039 #include <iostream>
00040
00041 namespace RobotLocalization
00042 {
00043 FilterBase::FilterBase() :
00044 state_(STATE_SIZE),
00045 predictedState_(STATE_SIZE),
00046 transferFunction_(STATE_SIZE, STATE_SIZE),
00047 transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
00048 estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
00049 covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
00050 processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
00051 identity_(STATE_SIZE, STATE_SIZE),
00052 debug_(false),
00053 debugStream_(NULL)
00054 {
00055 initialized_ = false;
00056
00057
00058 state_.setZero();
00059 predictedState_.setZero();
00060
00061
00062
00063 transferFunction_.setIdentity();
00064
00065
00066 transferFunctionJacobian_.setZero();
00067
00068
00069
00070
00071 estimateErrorCovariance_.setIdentity();
00072 estimateErrorCovariance_ *= 1e-9;
00073
00074
00075 identity_.setIdentity();
00076
00077
00078
00079 covarianceEpsilon_.setIdentity();
00080 covarianceEpsilon_ *= 0.001;
00081
00082
00083 sensorTimeout_ = 0.033333333;
00084
00085
00086 lastUpdateTime_ = 0;
00087 lastMeasurementTime_ = 0;
00088
00089
00090
00091 processNoiseCovariance_.setZero();
00092 processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
00093 processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
00094 processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
00095 processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
00096 processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
00097 processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
00098 processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
00099 processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
00100 processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
00101 processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
00102 processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
00103 processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
00104 processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
00105 processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
00106 processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;
00107 }
00108
00109 FilterBase::~FilterBase()
00110 {
00111 }
00112
00113 bool FilterBase::getDebug()
00114 {
00115 return debug_;
00116 }
00117
00118 const Eigen::MatrixXd& FilterBase::getEstimateErrorCovariance()
00119 {
00120 return estimateErrorCovariance_;
00121 }
00122
00123 bool FilterBase::getInitializedStatus()
00124 {
00125 return initialized_;
00126 }
00127
00128 double FilterBase::getLastMeasurementTime()
00129 {
00130 return lastMeasurementTime_;
00131 }
00132
00133 double FilterBase::getLastUpdateTime()
00134 {
00135 return lastUpdateTime_;
00136 }
00137
00138 const Eigen::VectorXd& FilterBase::getPredictedState()
00139 {
00140 return predictedState_;
00141 }
00142
00143 const Eigen::MatrixXd& FilterBase::getProcessNoiseCovariance()
00144 {
00145 return processNoiseCovariance_;
00146 }
00147
00148 double FilterBase::getSensorTimeout()
00149 {
00150 return sensorTimeout_;
00151 }
00152
00153 const Eigen::VectorXd& FilterBase::getState()
00154 {
00155 return state_;
00156 }
00157
00158 void FilterBase::processMeasurement(const Measurement &measurement)
00159 {
00160 FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
00161
00162 double delta = 0.0;
00163
00164
00165
00166
00167 if (initialized_)
00168 {
00169
00170 delta = measurement.time_ - lastMeasurementTime_;
00171
00172 FB_DEBUG("Filter is already initialized. Carrying out predict/correct loop...\n"
00173 "Measurement time is " << std::setprecision(20) << measurement.time_ <<
00174 ", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n");
00175
00176
00177
00178 if (delta > 0)
00179 {
00180 validateDelta(delta);
00181
00182 predict(delta);
00183
00184
00185 predictedState_ = state_;
00186 }
00187
00188 correct(measurement);
00189 }
00190 else
00191 {
00192 FB_DEBUG("First measurement. Initializing filter.\n");
00193
00194
00195 size_t measurementLength = measurement.updateVector_.size();
00196 for(size_t i = 0; i < measurementLength; ++i)
00197 {
00198 state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]);
00199 }
00200
00201
00202 for(size_t i = 0; i < measurementLength; ++i)
00203 {
00204 for(size_t j = 0; j < measurementLength; ++j)
00205 {
00206 estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ?
00207 measurement.covariance_(i, j) :
00208 estimateErrorCovariance_(i, j));
00209 }
00210 }
00211
00212 initialized_ = true;
00213 }
00214
00215 if(delta >= 0.0)
00216 {
00217
00218
00219
00220
00221
00222
00223
00224 lastMeasurementTime_ = measurement.time_;
00225 }
00226
00227 FB_DEBUG("------ /FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
00228 }
00229
00230 void FilterBase::setDebug(const bool debug, std::ostream *outStream)
00231 {
00232 if (debug)
00233 {
00234 if (outStream != NULL)
00235 {
00236 debugStream_ = outStream;
00237 debug_ = true;
00238 }
00239 else
00240 {
00241 debug_ = false;
00242 }
00243 }
00244 else
00245 {
00246 debug_ = false;
00247 }
00248 }
00249
00250 void FilterBase::setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance)
00251 {
00252 estimateErrorCovariance_ = estimateErrorCovariance;
00253 }
00254
00255 void FilterBase::setLastMeasurementTime(const double lastMeasurementTime)
00256 {
00257 lastMeasurementTime_ = lastMeasurementTime;
00258 }
00259
00260 void FilterBase::setLastUpdateTime(const double lastUpdateTime)
00261 {
00262 lastUpdateTime_ = lastUpdateTime;
00263 }
00264
00265 void FilterBase::setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance)
00266 {
00267 processNoiseCovariance_ = processNoiseCovariance;
00268 }
00269
00270 void FilterBase::setSensorTimeout(const double sensorTimeout)
00271 {
00272 sensorTimeout_ = sensorTimeout;
00273 }
00274
00275 void FilterBase::setState(const Eigen::VectorXd &state)
00276 {
00277 state_ = state;
00278 }
00279
00280 void FilterBase::validateDelta(double &delta)
00281 {
00282
00283 if (delta > 100000.0)
00284 {
00285 FB_DEBUG("Delta was very large. Suspect playing from bag file. Setting to 0.01\n");
00286
00287 delta = 0.01;
00288 }
00289 }
00290
00291 void FilterBase::wrapStateAngles()
00292 {
00293 state_(StateMemberRoll) = FilterUtilities::clampRotation(state_(StateMemberRoll));
00294 state_(StateMemberPitch) = FilterUtilities::clampRotation(state_(StateMemberPitch));
00295 state_(StateMemberYaw) = FilterUtilities::clampRotation(state_(StateMemberYaw));
00296 }
00297
00298 bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
00299 const Eigen::MatrixXd &invCovariance,
00300 const double nsigmas)
00301 {
00302 double sqMahalanobis = innovation.dot(invCovariance * innovation);
00303 double threshold = nsigmas * nsigmas;
00304
00305 if (sqMahalanobis >= threshold)
00306 {
00307 FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
00308 "Threshold is: " << threshold << "\n" <<
00309 "Innovation is: " << innovation << "\n" <<
00310 "Innovation covariance is:\n" << invCovariance << "\n");
00311
00312 return false;
00313 }
00314
00315 return true;
00316 }
00317 }