00001 /* 00002 * Copyright (c) 2015, Charles River Analytics, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions 00007 * are met: 00008 * 00009 * 1. Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * 2. Redistributions in binary form must reproduce the above 00012 * copyright notice, this list of conditions and the following 00013 * disclaimer in the documentation and/or other materials provided 00014 * with the distribution. 00015 * 3. Neither the name of the copyright holder nor the names of its 00016 * contributors may be used to endorse or promote products derived 00017 * from this software without specific prior written permission. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00020 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00021 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00022 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00023 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00024 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00025 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00026 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00027 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00028 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00029 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00030 * POSSIBILITY OF SUCH DAMAGE. 00031 */ 00032 00033 #ifndef RobotLocalization_FilterBase_h 00034 #define RobotLocalization_FilterBase_h 00035 00036 #include "robot_localization/filter_utilities.h" 00037 00038 #include <Eigen/Dense> 00039 00040 #include <ostream> 00041 #include <vector> 00042 #include <set> 00043 #include <map> 00044 #include <queue> 00045 #include <limits> 00046 #include <string> 00047 00048 namespace RobotLocalization 00049 { 00056 struct Measurement 00057 { 00058 // The topic name for this measurement. Needed 00059 // for capturing previous state values for new 00060 // measurements. 00061 std::string topicName_; 00062 00063 // The measurement and its associated covariance 00064 Eigen::VectorXd measurement_; 00065 Eigen::MatrixXd covariance_; 00066 00067 // This defines which variables within this measurement 00068 // actually get passed into the filter. std::vector<bool> 00069 // is generally frowned upon, so we use ints. 00070 std::vector<int> updateVector_; 00071 00072 // The real-valued time, in seconds, since some epoch 00073 // (presumably the start of execution, but any will do) 00074 double time_; 00075 00076 // The Mahalanobis distance threshold in number of sigmas 00077 double mahalanobisThresh_; 00078 00079 // We want earlier times to have greater priority 00080 bool operator()(const Measurement &a, const Measurement &b) 00081 { 00082 return a.time_ > b.time_; 00083 } 00084 00085 Measurement() : 00086 topicName_(""), 00087 time_(0), 00088 mahalanobisThresh_(std::numeric_limits<double>::max()) 00089 { 00090 } 00091 }; 00092 00093 class FilterBase 00094 { 00095 public: 00098 FilterBase(); 00099 00102 virtual ~FilterBase(); 00103 00109 virtual void correct(const Measurement &measurement) = 0; 00110 00115 bool getDebug(); 00116 00121 const Eigen::MatrixXd& getEstimateErrorCovariance(); 00122 00127 bool getInitializedStatus(); 00128 00133 double getLastMeasurementTime(); 00134 00140 double getLastUpdateTime(); 00141 00147 const Eigen::VectorXd& getPredictedState(); 00148 00153 const Eigen::MatrixXd& getProcessNoiseCovariance(); 00154 00159 double getSensorTimeout(); 00160 00165 const Eigen::VectorXd& getState(); 00166 00173 virtual void predict(const double delta) = 0; 00174 00179 virtual void processMeasurement(const Measurement &measurement); 00180 00191 void setDebug(const bool debug, std::ostream *outStream = NULL); 00192 00197 void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance); 00198 00203 void setLastMeasurementTime(const double lastMeasurementTime); 00204 00212 void setLastUpdateTime(const double lastUpdateTime); 00213 00222 void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance); 00223 00229 void setSensorTimeout(const double sensorTimeout); 00230 00235 void setState(const Eigen::VectorXd &state); 00236 00241 void validateDelta(double &delta); 00242 00243 protected: 00244 00247 virtual void wrapStateAngles(); 00248 00251 virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation, 00252 const Eigen::MatrixXd &invCovariance, 00253 const double nsigmas); 00254 00259 Eigen::MatrixXd covarianceEpsilon_; 00260 00263 std::ostream *debugStream_; 00264 00268 Eigen::MatrixXd estimateErrorCovariance_; 00269 00272 Eigen::MatrixXd identity_; 00273 00276 bool initialized_; 00277 00283 double lastMeasurementTime_; 00284 00294 double lastUpdateTime_; 00295 00298 Eigen::VectorXd predictedState_; 00299 00310 Eigen::MatrixXd processNoiseCovariance_; 00311 00317 double sensorTimeout_; 00318 00322 Eigen::VectorXd state_; 00323 00337 Eigen::MatrixXd transferFunction_; 00338 00347 Eigen::MatrixXd transferFunctionJacobian_; 00348 00349 private: 00350 00353 bool debug_; 00354 }; 00355 } 00356 00357 #endif