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 RobotLocalization_FilterBase_h
00034 #define RobotLocalization_FilterBase_h
00035
00036 #include <Eigen/Dense>
00037
00038 #include <ostream>
00039 #include <vector>
00040 #include <set>
00041 #include <map>
00042 #include <queue>
00043
00044 namespace RobotLocalization
00045 {
00052 struct Measurement
00053 {
00054
00055
00056
00057 std::string topicName_;
00058
00059
00060 Eigen::VectorXd measurement_;
00061 Eigen::MatrixXd covariance_;
00062
00063
00064
00065
00066 std::vector<int> updateVector_;
00067
00068
00069
00070 double time_;
00071
00072
00073 bool operator()(const Measurement &a, const Measurement &b)
00074 {
00075 return a.time_ > b.time_;
00076 }
00077
00078 Measurement() :
00079 topicName_(""),
00080 time_(0)
00081 {
00082 }
00083 };
00084
00085 class FilterBase
00086 {
00087 public:
00088
00091 FilterBase();
00092
00095 ~FilterBase();
00096
00104 virtual void enqueueMeasurement(const std::string &topicName,
00105 const Eigen::VectorXd &measurement,
00106 const Eigen::MatrixXd &measurementCovariance,
00107 const std::vector<int> &updateVector,
00108 const double time);
00109
00114 virtual void integrateMeasurements(double currentTime,
00115 std::map<std::string, Eigen::VectorXd> &postUpdateStates);
00116
00121 bool getDebug();
00122
00127 const Eigen::MatrixXd& getEstimateErrorCovariance();
00128
00133 bool getInitializedStatus();
00134
00139 double getLastMeasurementTime();
00140
00145 double getLastUpdateTime();
00146
00151 const Eigen::MatrixXd& getProcessNoiseCovariance();
00152
00157 double getSensorTimeout();
00158
00163 const Eigen::VectorXd& getState();
00164
00175 void setDebug(const bool debug, std::ostream *outStream = NULL);
00176
00181 void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance);
00182
00187 void setLastMeasurementTime(const double lastMeasurementTime);
00188
00196 void setLastUpdateTime(const double lastUpdateTime);
00197
00206 void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance);
00207
00213 void setSensorTimeout(const double sensorTimeout);
00214
00219 void setState(const Eigen::VectorXd &state);
00220
00221 protected:
00222
00228 virtual void correct(const Measurement &measurement) = 0;
00229
00236 virtual void predict(const double delta) = 0;
00237
00242 virtual void processMeasurement(const Measurement &measurement);
00243
00248 void validateDelta(double &delta);
00249
00252 virtual void wrapStateAngles();
00253
00256 bool initialized_;
00257
00260 std::ostream *debugStream_;
00261
00265 Eigen::VectorXd state_;
00266
00280 Eigen::MatrixXd transferFunction_;
00281
00290 Eigen::MatrixXd transferFunctionJacobian_;
00291
00295 Eigen::MatrixXd estimateErrorCovariance_;
00296
00301 Eigen::MatrixXd covarianceEpsilon_;
00302
00313 Eigen::MatrixXd processNoiseCovariance_;
00314
00317 Eigen::MatrixXd identity_;
00318
00324 double sensorTimeout_;
00325
00328 const double pi_;
00329
00330 const double tau_;
00331
00338 std::priority_queue<Measurement, std::vector<Measurement>, Measurement> measurementQueue_;
00339
00349 double lastUpdateTime_;
00350
00356 double lastMeasurementTime_;
00357
00358 private:
00359
00362 bool debug_;
00363 };
00364 }
00365
00366
00367 std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat);
00368 std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec);
00369 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec);
00370 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec);
00371
00372 #endif