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 #ifndef ROBOT_LOCALIZATION_FILTER_BASE_H
00034 #define ROBOT_LOCALIZATION_FILTER_BASE_H
00035
00036 #include "robot_localization/filter_utilities.h"
00037 #include "robot_localization/filter_common.h"
00038
00039 #include <Eigen/Dense>
00040
00041 #include <ostream>
00042 #include <vector>
00043 #include <set>
00044 #include <map>
00045 #include <queue>
00046 #include <limits>
00047 #include <string>
00048
00049 #include <boost/shared_ptr.hpp>
00050
00051 namespace RobotLocalization
00052 {
00053
00060 struct Measurement
00061 {
00062
00063
00064
00065 std::string topicName_;
00066
00067
00068 Eigen::VectorXd measurement_;
00069 Eigen::MatrixXd covariance_;
00070
00071
00072
00073
00074 std::vector<int> updateVector_;
00075
00076
00077
00078 double time_;
00079
00080
00081 double mahalanobisThresh_;
00082
00083
00084 Eigen::VectorXd latestControl_;
00085
00086
00087 double latestControlTime_;
00088
00089
00090 bool operator()(const boost::shared_ptr<Measurement> &a, const boost::shared_ptr<Measurement> &b)
00091 {
00092 return (*this)(*(a.get()), *(b.get()));
00093 }
00094
00095 bool operator()(const Measurement &a, const Measurement &b)
00096 {
00097 return a.time_ > b.time_;
00098 }
00099
00100 Measurement() :
00101 topicName_(""),
00102 latestControl_(),
00103 latestControlTime_(0.0),
00104 time_(0.0),
00105 mahalanobisThresh_(std::numeric_limits<double>::max())
00106 {
00107 }
00108 };
00109 typedef boost::shared_ptr<Measurement> MeasurementPtr;
00110
00117 struct FilterState
00118 {
00119
00120 Eigen::VectorXd state_;
00121
00122
00123 Eigen::MatrixXd estimateErrorCovariance_;
00124
00125
00126 Eigen::VectorXd latestControl_;
00127
00128
00129 double lastMeasurementTime_;
00130
00131
00132 double latestControlTime_;
00133
00134
00135 bool operator()(const FilterState &a, const FilterState &b)
00136 {
00137 return a.lastMeasurementTime_ < b.lastMeasurementTime_;
00138 }
00139
00140 FilterState() :
00141 state_(),
00142 estimateErrorCovariance_(),
00143 latestControl_(),
00144 lastMeasurementTime_(0.0),
00145 latestControlTime_(0.0)
00146 {}
00147 };
00148 typedef boost::shared_ptr<FilterState> FilterStatePtr;
00149
00150 class FilterBase
00151 {
00152 public:
00155 FilterBase();
00156
00159 virtual ~FilterBase();
00160
00167 void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta);
00168
00174 virtual void correct(const Measurement &measurement) = 0;
00175
00180 const Eigen::VectorXd& getControl();
00181
00186 double getControlTime();
00187
00192 bool getDebug();
00193
00198 const Eigen::MatrixXd& getEstimateErrorCovariance();
00199
00204 bool getInitializedStatus();
00205
00210 double getLastMeasurementTime();
00211
00217 double getLastUpdateTime();
00218
00224 const Eigen::VectorXd& getPredictedState();
00225
00230 const Eigen::MatrixXd& getProcessNoiseCovariance();
00231
00236 double getSensorTimeout();
00237
00242 const Eigen::VectorXd& getState();
00243
00251 virtual void predict(const double referenceTime, const double delta) = 0;
00252
00257 virtual void processMeasurement(const Measurement &measurement);
00258
00264 void setControl(const Eigen::VectorXd &control, const double controlTime);
00265
00275 void setControlParams(const std::vector<int> &updateVector, const double controlTimeout,
00276 const std::vector<double> &accelerationLimits, const std::vector<double> &accelerationGains,
00277 const std::vector<double> &decelerationLimits, const std::vector<double> &decelerationGains);
00278
00289 void setDebug(const bool debug, std::ostream *outStream = NULL);
00290
00295 void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance);
00296
00301 void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance);
00302
00307 void setLastMeasurementTime(const double lastMeasurementTime);
00308
00316 void setLastUpdateTime(const double lastUpdateTime);
00317
00326 void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance);
00327
00333 void setSensorTimeout(const double sensorTimeout);
00334
00339 void setState(const Eigen::VectorXd &state);
00340
00345 void validateDelta(double &delta);
00346
00347 protected:
00357 inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit,
00358 const double accelerationGain, const double decelerationLimit, const double decelerationGain)
00359 {
00360 FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n");
00361
00362 const double error = control - state;
00363 const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01);
00364 const double setPoint = (sameSign ? control : 0.0);
00365 const bool decelerating = ::fabs(setPoint) < ::fabs(state);
00366 double limit = accelerationLimit;
00367 double gain = accelerationGain;
00368
00369 if(decelerating)
00370 {
00371 limit = decelerationLimit;
00372 gain = decelerationGain;
00373 }
00374
00375 const double finalAccel = std::min(std::max(gain * error, -limit), limit);
00376
00377 FB_DEBUG("Control value: " << control << "\n" <<
00378 "State value: " << state << "\n" <<
00379 "Error: " << error << "\n" <<
00380 "Same sign: " << (sameSign ? "true" : "false") << "\n" <<
00381 "Set point: " << setPoint << "\n" <<
00382 "Decelerating: " << (decelerating ? "true" : "false") << "\n" <<
00383 "Limit: " << limit << "\n" <<
00384 "Gain: " << gain << "\n" <<
00385 "Final is " << finalAccel << "\n");
00386
00387 return finalAccel;
00388 }
00389
00392 virtual void wrapStateAngles();
00393
00399 virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
00400 const Eigen::MatrixXd &invCovariance,
00401 const double nsigmas);
00402
00407 void prepareControl(const double referenceTime, const double predictionDelta);
00408
00411 std::vector<double> accelerationGains_;
00412
00415 std::vector<double> accelerationLimits_;
00416
00419 Eigen::VectorXd controlAcceleration_;
00420
00423 std::vector<double> decelerationGains_;
00424
00427 std::vector<double> decelerationLimits_;
00428
00431 Eigen::VectorXd latestControl_;
00432
00435 std::vector<int> controlUpdateVector_;
00436
00439 double controlTimeout_;
00440
00445 Eigen::MatrixXd covarianceEpsilon_;
00446
00449 std::ostream *debugStream_;
00450
00453 Eigen::MatrixXd dynamicProcessNoiseCovariance_;
00454
00458 Eigen::MatrixXd estimateErrorCovariance_;
00459
00462 Eigen::MatrixXd identity_;
00463
00466 bool initialized_;
00467
00473 double lastMeasurementTime_;
00474
00484 double lastUpdateTime_;
00485
00488 double latestControlTime_;
00489
00492 Eigen::VectorXd predictedState_;
00493
00504 Eigen::MatrixXd processNoiseCovariance_;
00505
00511 double sensorTimeout_;
00512
00516 Eigen::VectorXd state_;
00517
00531 Eigen::MatrixXd transferFunction_;
00532
00541 Eigen::MatrixXd transferFunctionJacobian_;
00542
00545 bool useControl_;
00546
00550 bool useDynamicProcessNoiseCovariance_;
00551
00552 private:
00555 bool debug_;
00556 };
00557
00558 }
00559
00560 #endif // ROBOT_LOCALIZATION_FILTER_BASE_H