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 accelerationGains_(TWIST_SIZE, 0.0),
00045 accelerationLimits_(TWIST_SIZE, 0.0),
00046 decelerationGains_(TWIST_SIZE, 0.0),
00047 decelerationLimits_(TWIST_SIZE, 0.0),
00048 controlAcceleration_(TWIST_SIZE),
00049 controlTimeout_(0.0),
00050 controlUpdateVector_(TWIST_SIZE, 0),
00051 dynamicProcessNoiseCovariance_(STATE_SIZE, STATE_SIZE),
00052 latestControlTime_(0.0),
00053 state_(STATE_SIZE),
00054 predictedState_(STATE_SIZE),
00055 transferFunction_(STATE_SIZE, STATE_SIZE),
00056 transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
00057 estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
00058 covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
00059 processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
00060 identity_(STATE_SIZE, STATE_SIZE),
00061 debug_(false),
00062 debugStream_(NULL),
00063 useControl_(false),
00064 useDynamicProcessNoiseCovariance_(false)
00065 {
00066 initialized_ = false;
00067
00068
00069 state_.setZero();
00070 predictedState_.setZero();
00071 controlAcceleration_.setZero();
00072
00073
00074
00075 transferFunction_.setIdentity();
00076
00077
00078 transferFunctionJacobian_.setZero();
00079
00080
00081
00082
00083 estimateErrorCovariance_.setIdentity();
00084 estimateErrorCovariance_ *= 1e-9;
00085
00086
00087 identity_.setIdentity();
00088
00089
00090
00091 covarianceEpsilon_.setIdentity();
00092 covarianceEpsilon_ *= 0.001;
00093
00094
00095 sensorTimeout_ = 0.033333333;
00096
00097
00098 lastUpdateTime_ = 0;
00099 lastMeasurementTime_ = 0;
00100
00101
00102
00103 processNoiseCovariance_.setZero();
00104 processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
00105 processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
00106 processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
00107 processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
00108 processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
00109 processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
00110 processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
00111 processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
00112 processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
00113 processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
00114 processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
00115 processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
00116 processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
00117 processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
00118 processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;
00119
00120 dynamicProcessNoiseCovariance_ = processNoiseCovariance_;
00121 }
00122
00123 FilterBase::~FilterBase()
00124 {
00125 }
00126
00127 void FilterBase::computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta)
00128 {
00129
00130
00131
00132
00133
00134
00135
00136 Eigen::MatrixXd velocityMatrix(TWIST_SIZE, TWIST_SIZE);
00137 velocityMatrix.setIdentity();
00138 velocityMatrix.diagonal() *= state.segment(POSITION_V_OFFSET, TWIST_SIZE).norm();
00139
00140 dynamicProcessNoiseCovariance_.block<TWIST_SIZE, TWIST_SIZE>(POSITION_OFFSET, POSITION_OFFSET) =
00141 velocityMatrix *
00142 processNoiseCovariance_.block<TWIST_SIZE, TWIST_SIZE>(POSITION_OFFSET, POSITION_OFFSET) *
00143 velocityMatrix.transpose();
00144 }
00145
00146 const Eigen::VectorXd& FilterBase::getControl()
00147 {
00148 return latestControl_;
00149 }
00150
00151 double FilterBase::getControlTime()
00152 {
00153 return latestControlTime_;
00154 }
00155
00156 bool FilterBase::getDebug()
00157 {
00158 return debug_;
00159 }
00160
00161 const Eigen::MatrixXd& FilterBase::getEstimateErrorCovariance()
00162 {
00163 return estimateErrorCovariance_;
00164 }
00165
00166 bool FilterBase::getInitializedStatus()
00167 {
00168 return initialized_;
00169 }
00170
00171 double FilterBase::getLastMeasurementTime()
00172 {
00173 return lastMeasurementTime_;
00174 }
00175
00176 double FilterBase::getLastUpdateTime()
00177 {
00178 return lastUpdateTime_;
00179 }
00180
00181 const Eigen::VectorXd& FilterBase::getPredictedState()
00182 {
00183 return predictedState_;
00184 }
00185
00186 const Eigen::MatrixXd& FilterBase::getProcessNoiseCovariance()
00187 {
00188 return processNoiseCovariance_;
00189 }
00190
00191 double FilterBase::getSensorTimeout()
00192 {
00193 return sensorTimeout_;
00194 }
00195
00196 const Eigen::VectorXd& FilterBase::getState()
00197 {
00198 return state_;
00199 }
00200
00201 void FilterBase::processMeasurement(const Measurement &measurement)
00202 {
00203 FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
00204
00205 double delta = 0.0;
00206
00207
00208
00209
00210 if (initialized_)
00211 {
00212
00213 delta = measurement.time_ - lastMeasurementTime_;
00214
00215 FB_DEBUG("Filter is already initialized. Carrying out predict/correct loop...\n"
00216 "Measurement time is " << std::setprecision(20) << measurement.time_ <<
00217 ", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n");
00218
00219
00220
00221 if (delta > 0)
00222 {
00223 validateDelta(delta);
00224 predict(measurement.time_, delta);
00225
00226
00227 predictedState_ = state_;
00228 }
00229
00230 correct(measurement);
00231 }
00232 else
00233 {
00234 FB_DEBUG("First measurement. Initializing filter.\n");
00235
00236
00237 size_t measurementLength = measurement.updateVector_.size();
00238 for (size_t i = 0; i < measurementLength; ++i)
00239 {
00240 state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]);
00241 }
00242
00243
00244 for (size_t i = 0; i < measurementLength; ++i)
00245 {
00246 for (size_t j = 0; j < measurementLength; ++j)
00247 {
00248 estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ?
00249 measurement.covariance_(i, j) :
00250 estimateErrorCovariance_(i, j));
00251 }
00252 }
00253
00254 initialized_ = true;
00255 }
00256
00257 if (delta >= 0.0)
00258 {
00259
00260
00261
00262
00263
00264
00265
00266 lastMeasurementTime_ = measurement.time_;
00267 }
00268
00269 FB_DEBUG("------ /FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n");
00270 }
00271
00272 void FilterBase::setControl(const Eigen::VectorXd &control, const double controlTime)
00273 {
00274 latestControl_ = control;
00275 latestControlTime_ = controlTime;
00276 }
00277
00278 void FilterBase::setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
00279 const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
00280 const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains)
00281 {
00282 useControl_ = true;
00283 controlUpdateVector_ = updateVector;
00284 controlTimeout_ = controlTimeout;
00285 accelerationLimits_ = accelerationLimits;
00286 accelerationGains_ = accelerationGains;
00287 decelerationLimits_ = decelerationLimits;
00288 decelerationGains_ = decelerationGains;
00289 }
00290
00291 void FilterBase::setDebug(const bool debug, std::ostream *outStream)
00292 {
00293 if (debug)
00294 {
00295 if (outStream != NULL)
00296 {
00297 debugStream_ = outStream;
00298 debug_ = true;
00299 }
00300 else
00301 {
00302 debug_ = false;
00303 }
00304 }
00305 else
00306 {
00307 debug_ = false;
00308 }
00309 }
00310
00311 void FilterBase::setUseDynamicProcessNoiseCovariance(const bool useDynamicProcessNoiseCovariance)
00312 {
00313 useDynamicProcessNoiseCovariance_ = useDynamicProcessNoiseCovariance;
00314 }
00315
00316 void FilterBase::setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance)
00317 {
00318 estimateErrorCovariance_ = estimateErrorCovariance;
00319 }
00320
00321 void FilterBase::setLastMeasurementTime(const double lastMeasurementTime)
00322 {
00323 lastMeasurementTime_ = lastMeasurementTime;
00324 }
00325
00326 void FilterBase::setLastUpdateTime(const double lastUpdateTime)
00327 {
00328 lastUpdateTime_ = lastUpdateTime;
00329 }
00330
00331 void FilterBase::setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance)
00332 {
00333 processNoiseCovariance_ = processNoiseCovariance;
00334 }
00335
00336 void FilterBase::setSensorTimeout(const double sensorTimeout)
00337 {
00338 sensorTimeout_ = sensorTimeout;
00339 }
00340
00341 void FilterBase::setState(const Eigen::VectorXd &state)
00342 {
00343 state_ = state;
00344 }
00345
00346 void FilterBase::validateDelta(double &delta)
00347 {
00348
00349 if (delta > 100000.0)
00350 {
00351 FB_DEBUG("Delta was very large. Suspect playing from bag file. Setting to 0.01\n");
00352
00353 delta = 0.01;
00354 }
00355 }
00356
00357
00358 void FilterBase::prepareControl(const double referenceTime, const double predictionDelta)
00359 {
00360 controlAcceleration_.setZero();
00361
00362 if (useControl_)
00363 {
00364 bool timedOut = ::fabs(referenceTime - latestControlTime_) >= controlTimeout_;
00365
00366 if(timedOut)
00367 {
00368 FB_DEBUG("Control timed out. Reference time was " << referenceTime << ", latest control time was " <<
00369 latestControlTime_ << ", control timeout was " << controlTimeout_ << "\n");
00370 }
00371
00372 for(size_t controlInd = 0; controlInd < TWIST_SIZE; ++controlInd)
00373 {
00374 if(controlUpdateVector_[controlInd])
00375 {
00376 controlAcceleration_(controlInd) = computeControlAcceleration(state_(controlInd + POSITION_V_OFFSET),
00377 (timedOut ? 0.0 : latestControl_(controlInd)), accelerationLimits_[controlInd],
00378 accelerationGains_[controlInd], decelerationLimits_[controlInd], decelerationGains_[controlInd]);
00379 }
00380 }
00381 }
00382 }
00383
00384 void FilterBase::wrapStateAngles()
00385 {
00386 state_(StateMemberRoll) = FilterUtilities::clampRotation(state_(StateMemberRoll));
00387 state_(StateMemberPitch) = FilterUtilities::clampRotation(state_(StateMemberPitch));
00388 state_(StateMemberYaw) = FilterUtilities::clampRotation(state_(StateMemberYaw));
00389 }
00390
00391 bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
00392 const Eigen::MatrixXd &invCovariance,
00393 const double nsigmas)
00394 {
00395 double sqMahalanobis = innovation.dot(invCovariance * innovation);
00396 double threshold = nsigmas * nsigmas;
00397
00398 if (sqMahalanobis >= threshold)
00399 {
00400 FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
00401 "Threshold is: " << threshold << "\n" <<
00402 "Innovation is: " << innovation << "\n" <<
00403 "Innovation covariance is:\n" << invCovariance << "\n");
00404
00405 return false;
00406 }
00407
00408 return true;
00409 }
00410 }