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_common.h>
00034 #include <robot_localization/ekf.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 transferFunction_(STATE_SIZE, STATE_SIZE),
00046 transferFunctionJacobian_(STATE_SIZE, STATE_SIZE),
00047 estimateErrorCovariance_(STATE_SIZE, STATE_SIZE),
00048 covarianceEpsilon_(STATE_SIZE, STATE_SIZE),
00049 processNoiseCovariance_(STATE_SIZE, STATE_SIZE),
00050 identity_(STATE_SIZE, STATE_SIZE),
00051 pi_(3.141592653589793),
00052 tau_(6.283185307179586),
00053 debug_(false),
00054 debugStream_(NULL)
00055 {
00056 initialized_ = false;
00057
00058
00059 state_.setZero();
00060
00061
00062
00063 transferFunction_.setIdentity();
00064
00065
00066 transferFunctionJacobian_.setZero();
00067
00068
00069
00070 estimateErrorCovariance_.setIdentity();
00071 estimateErrorCovariance_ *= 1e-6;
00072
00073
00074 identity_.setIdentity();
00075
00076
00077
00078 covarianceEpsilon_.setIdentity();
00079 covarianceEpsilon_ *= 0.001;
00080
00081
00082 sensorTimeout_ = 0.033333333;
00083
00084
00085 lastUpdateTime_ = 0;
00086 lastMeasurementTime_ = 0;
00087
00088
00089
00090 processNoiseCovariance_.setZero();
00091 processNoiseCovariance_(StateMemberX, StateMemberX) = 0.03;
00092 processNoiseCovariance_(StateMemberY, StateMemberY) = 0.03;
00093 processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.4;
00094 processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
00095 processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
00096 processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
00097 processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
00098 processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
00099 processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.05;
00100 processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.002;
00101 processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.002;
00102 processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.004;
00103 }
00104
00105 FilterBase::~FilterBase()
00106 {
00107 }
00108
00109 void FilterBase::enqueueMeasurement(const std::string &topicName,
00110 const Eigen::VectorXd &measurement,
00111 const Eigen::MatrixXd &measurementCovariance,
00112 const std::vector<int> &updateVector,
00113 const double time)
00114 {
00115 Measurement meas;
00116
00117 meas.topicName_ = topicName;
00118 meas.measurement_ = measurement;
00119 meas.covariance_ = measurementCovariance;
00120 meas.updateVector_ = updateVector;
00121 meas.time_ = time;
00122
00123 measurementQueue_.push(meas);
00124 }
00125
00126 void FilterBase::integrateMeasurements(double currentTime,
00127 std::map<std::string, Eigen::VectorXd> &postUpdateStates)
00128 {
00129 if (debug_)
00130 {
00131 *debugStream_ << "------ FilterBase::integrateMeasurements ------\n\n";
00132 *debugStream_ << "Integration time is " << std::setprecision(20) << currentTime << "\n";
00133 }
00134
00135 postUpdateStates.clear();
00136
00137 if (debug_)
00138 {
00139 *debugStream_ << measurementQueue_.size() << " measurements in queue.\n";
00140 }
00141
00142
00143 if (!measurementQueue_.empty())
00144 {
00145 while (!measurementQueue_.empty())
00146 {
00147 Measurement measurement = measurementQueue_.top();
00148 measurementQueue_.pop();
00149
00150 processMeasurement(measurement);
00151
00152 postUpdateStates.insert(std::pair<std::string, Eigen::VectorXd>(measurement.topicName_, state_));
00153 }
00154
00155 lastUpdateTime_ = currentTime;
00156 }
00157 else if (initialized_)
00158 {
00159
00160
00161
00162 double lastUpdateDelta = currentTime - lastUpdateTime_;
00163
00164
00165 if(lastUpdateDelta >= sensorTimeout_)
00166 {
00167 double projectTime = sensorTimeout_ * std::floor(lastUpdateDelta / sensorTimeout_);
00168
00169 if (debug_)
00170 {
00171 *debugStream_ << "Sensor timeout! Last measurement was " << std::setprecision(10) << lastMeasurementTime_ << ", current time is " <<
00172 currentTime << ", delta is " << lastUpdateDelta << ", projection time is " << projectTime << "\n";
00173 }
00174
00175 validateDelta(projectTime);
00176
00177 predict(projectTime);
00178
00179
00180 lastMeasurementTime_ += projectTime;
00181 lastUpdateTime_ += projectTime;
00182 }
00183
00184 }
00185 else
00186 {
00187 if (debug_)
00188 {
00189 *debugStream_ << "Filter not yet initialized.\n";
00190 }
00191 }
00192
00193 if (debug_)
00194 {
00195 *debugStream_ << "\n----- /FilterBase::integrateMeasurements ------\n";
00196 }
00197 }
00198
00199 bool FilterBase::getDebug()
00200 {
00201 return debug_;
00202 }
00203
00204 const Eigen::MatrixXd& FilterBase::getEstimateErrorCovariance()
00205 {
00206 return estimateErrorCovariance_;
00207 }
00208
00209 bool FilterBase::getInitializedStatus()
00210 {
00211 return initialized_;
00212 }
00213
00214 double FilterBase::getLastMeasurementTime()
00215 {
00216 return lastMeasurementTime_;
00217 }
00218
00219 double FilterBase::getLastUpdateTime()
00220 {
00221 return lastUpdateTime_;
00222 }
00223
00224 const Eigen::MatrixXd& FilterBase::getProcessNoiseCovariance()
00225 {
00226 return processNoiseCovariance_;
00227 }
00228
00229 double FilterBase::getSensorTimeout()
00230 {
00231 return sensorTimeout_;
00232 }
00233
00234 const Eigen::VectorXd& FilterBase::getState()
00235 {
00236 return state_;
00237 }
00238
00239 void FilterBase::processMeasurement(const Measurement &measurement)
00240 {
00241 if (debug_)
00242 {
00243 *debugStream_ << "------ FilterBase::processMeasurement ------\n";
00244 }
00245
00246 double delta = 0.0;
00247
00248
00249
00250
00251 if (initialized_)
00252 {
00253
00254 delta = measurement.time_ - lastMeasurementTime_;
00255
00256 if (debug_)
00257 {
00258 *debugStream_ << "Filter is already initialized. Carrying out EKF loop...\n";
00259 *debugStream_ << "Measurement time is " << std::setprecision(20) << measurement.time_ <<
00260 ", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n";
00261 }
00262
00263
00264
00265 if (delta > 0)
00266 {
00267 validateDelta(delta);
00268
00269 predict(delta);
00270 }
00271
00272 correct(measurement);
00273 }
00274 else
00275 {
00276 if (debug_)
00277 {
00278 *debugStream_ << "First measurement. Initializing filter.\n";
00279 }
00280
00281
00282 size_t measurementLength = measurement.updateVector_.size();
00283 for(size_t i = 0; i < measurementLength; ++i)
00284 {
00285 state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]);
00286 }
00287
00288
00289 for(size_t i = 0; i < measurementLength; ++i)
00290 {
00291 for(size_t j = 0; j < measurementLength; ++j)
00292 {
00293 estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ?
00294 measurement.covariance_(i, j) :
00295 estimateErrorCovariance_(i, j));
00296 }
00297 }
00298
00299 initialized_ = true;
00300 }
00301
00302 if(delta >= 0.0)
00303 {
00304
00305
00306
00307
00308
00309
00310
00311 lastMeasurementTime_ = measurement.time_;
00312 }
00313
00314 if (debug_)
00315 {
00316 *debugStream_ << "\n----- /FilterBase::processMeasurement ------\n";
00317 }
00318 }
00319
00320 void FilterBase::setDebug(const bool debug, std::ostream *outStream)
00321 {
00322 if (debug)
00323 {
00324 if (outStream != NULL)
00325 {
00326 debugStream_ = outStream;
00327 debug_ = true;
00328 }
00329 else
00330 {
00331 debug_ = false;
00332 }
00333 }
00334 else
00335 {
00336 debug_ = false;
00337 }
00338 }
00339
00340 void FilterBase::setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance)
00341 {
00342 estimateErrorCovariance_ = estimateErrorCovariance;
00343 }
00344
00345 void FilterBase::setLastMeasurementTime(const double lastMeasurementTime)
00346 {
00347 lastMeasurementTime_ = lastMeasurementTime;
00348 }
00349
00350 void FilterBase::setLastUpdateTime(const double lastUpdateTime)
00351 {
00352 lastUpdateTime_ = lastUpdateTime;
00353 }
00354
00355 void FilterBase::setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance)
00356 {
00357 processNoiseCovariance_ = processNoiseCovariance;
00358 }
00359
00360 void FilterBase::setSensorTimeout(const double sensorTimeout)
00361 {
00362 sensorTimeout_ = sensorTimeout;
00363 }
00364
00365 void FilterBase::setState(const Eigen::VectorXd &state)
00366 {
00367 state_ = state;
00368 }
00369
00370 void FilterBase::validateDelta(double &delta)
00371 {
00372
00373 if (delta > 100000.0)
00374 {
00375 if (debug_)
00376 {
00377 *debugStream_ << "Delta was very large. Suspect playing from bag file. Setting to 0.01\n";
00378 }
00379
00380 delta = 0.01;
00381 }
00382 }
00383
00384 void FilterBase::wrapStateAngles()
00385 {
00386 while (state_(StateMemberRoll) < -pi_)
00387 {
00388 state_(StateMemberRoll) += tau_;
00389 }
00390 while (state_(StateMemberRoll) > pi_)
00391 {
00392 state_(StateMemberRoll) -= tau_;
00393 }
00394
00395 while (state_(StateMemberPitch) < -pi_)
00396 {
00397 state_(StateMemberPitch) += tau_;
00398 }
00399 while (state_(StateMemberPitch) > pi_)
00400 {
00401 state_(StateMemberPitch) -= tau_;
00402 }
00403
00404 while (state_(StateMemberYaw) < -pi_)
00405 {
00406 state_(StateMemberYaw) += tau_;
00407 }
00408 while (state_(StateMemberYaw) > pi_)
00409 {
00410 state_(StateMemberYaw) -= tau_;
00411 }
00412 }
00413 }
00414
00415 std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat)
00416 {
00417 os << "[";
00418
00419 int rowCount = static_cast<int>(mat.rows());
00420
00421 for (int row = 0; row < rowCount; ++row)
00422 {
00423 if (row > 0)
00424 {
00425 os << " ";
00426 }
00427
00428 for (int col = 0; col < mat.cols(); ++col)
00429 {
00430 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col);
00431 }
00432
00433 if (row < rowCount - 1)
00434 {
00435 os << "\n";
00436 }
00437 }
00438
00439 os << "]\n";
00440
00441 return os;
00442 }
00443
00444 std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec)
00445 {
00446 os << "[";
00447 for (int dim = 0; dim < vec.rows(); ++dim)
00448 {
00449 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim);
00450 }
00451 os << "]\n";
00452
00453 return os;
00454 }
00455
00456 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec)
00457 {
00458 os << "[";
00459 for (size_t dim = 0; dim < vec.size(); ++dim)
00460 {
00461 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim];
00462 }
00463 os << "]\n";
00464
00465 return os;
00466 }
00467
00468 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec)
00469 {
00470 os << "[";
00471 for (size_t dim = 0; dim < vec.size(); ++dim)
00472 {
00473 os << std::setiosflags(std::ios::left) << std::setw(12) << (vec[dim] ? "true " : "false");
00474 }
00475 os << "]\n";
00476
00477 return os;
00478 }
00479