diff --git a/include/robot_localization/filter_base.h b/include/robot_localization/filter_base.h index 9b4cbffa4..b07d5a7eb 100644 --- a/include/robot_localization/filter_base.h +++ b/include/robot_localization/filter_base.h @@ -91,6 +91,31 @@ struct Measurement } }; +struct FilterState +{ + + Eigen::VectorXd state_; + + Eigen::MatrixXd estimate_error_covariance_; + + double last_update_time_; + + double time_; + + // We want the queue to be sorted from latest to earliest timestamps. + bool operator()(const FilterState &a, const FilterState &b) + { + return a.time_ < b.time_; + } + + FilterState() : + state_(), + estimate_error_covariance_(), + last_update_time_(0), + time_(0) + {} +}; + class FilterBase { public: @@ -242,6 +267,8 @@ class FilterBase void validateDelta(double &delta); protected: + + //! @brief Keeps the state Euler angles in the range [-pi, pi] //! virtual void wrapStateAngles(); diff --git a/include/robot_localization/ros_filter.h b/include/robot_localization/ros_filter.h index 9d0bd23b7..4b0a9b921 100644 --- a/include/robot_localization/ros_filter.h +++ b/include/robot_localization/ros_filter.h @@ -63,6 +63,7 @@ #include #include #include +#include // Some typedefs for message filter shared pointers typedef boost::shared_ptr< message_filters::Subscriber > poseMFSubPtr; @@ -75,6 +76,8 @@ namespace RobotLocalization { typedef std::priority_queue, Measurement> MeasurementQueue; +typedef std::deque MeasurementHistoryDeque; +typedef std::deque FilterStateHistoryDeque; template class RosFilter { @@ -255,6 +258,19 @@ template class RosFilter const double mahalanobisThresh); protected: + //! Finds the latest filter state before the given timestamp and makes it + //! the current state again. It also inserts all measurements between the + //! older filter timestamp and now into the measurements queue. + //! @return True if restoring the filter succeeded. False if not. + bool restoreFilterAndMeasurementsBefore(const double t); + + //! Saves the current filter state on in the queue of previous filter states + //! to be used in backwards smoothing in case older measurements come in. + void saveFilterState(FilterBase &filter); + + //! Removes measurements and filter states older than the given timestamp. + void removeHistoryStatesAndMeasurementsOlderThan(const double t); + //! @brief Adds a diagnostic message to the accumulating map and updates the error level //! @param[in] errLevel - The error level of the diagnostic //! @param[in] topicAndClass - The sensor topic (if relevant) and class of diagnostic @@ -544,6 +560,12 @@ template class RosFilter //! bool twoDMode_; + //! @brief: The smoothing window time in milliseconds. + //! + //! This is the guaranteed minimum buffer size for which previous + //! states and measurements are kept. + int filterHistoryInterval_; + //! @brief Message that contains our latest transform (i.e., state) //! //! We use the vehicle's latest state in a number of places, and often @@ -555,6 +577,19 @@ template class RosFilter //! @brief tf frame name that is the parent frame of the transform that this node will calculate and broadcast. //! std::string worldFrameId_; + + + //! @brief An implicitly time ordered queue of past filter states used for smoothing. + // + // front() refers to the filter state with the earliest timestamp. + // back() referst to the filter state with the latest timestamp. + FilterStateHistoryDeque filterStateHistory_; + + //! @brief A deque of previous measurements which is implicitly ordered from earliest to latest measurement. + // when popped from the measurement priority queue. + // front() refers to the measurement with the earliest timestamp. + // back() referst to the measurement with the latest timestamp. + MeasurementHistoryDeque measurementHistory_; }; } // namespace RobotLocalization diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index b9d6c5fca..14002937f 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -57,7 +57,8 @@ namespace RobotLocalization messageFiltersEmpty_(true), nhLocal_("~"), printDiagnostics_(true), - twoDMode_(false) + twoDMode_(false), + filterHistoryInterval_(0) { stateVariableNames_.push_back("X"); stateVariableNames_.push_back("Y"); @@ -392,13 +393,45 @@ namespace RobotLocalization // If we have any measurements in the queue, process them if (!measurementQueue_.empty()) { + // If smoothing is enabled and the earliest measurement is older than + // the current filter state try to reset filter and measurement queue + // to a time earlier than the earliest measurement. + const Measurement& oldest_measurement = measurementQueue_.top(); + if (filterHistoryInterval_ > 0 && + oldest_measurement.time_ < filter_.getLastMeasurementTime()) + { + const double time_difference = (oldest_measurement.time_ - filter_.getLastMeasurementTime()); + RF_DEBUG("Smoothing: Measurement" << oldest_measurement.topicName_ << " is " << -time_difference << " seconds behind filter state. Begin smoothing update.\n"); + const bool success = restoreFilterAndMeasurementsBefore(oldest_measurement.time_); + if (!success) + { + RF_DEBUG("FATAL ERROR: Time difference too big to reset filter! Diff is : " << time_difference << "\n"); + } + } + + while (!measurementQueue_.empty()) { Measurement measurement = measurementQueue_.top(); measurementQueue_.pop(); + if (filterHistoryInterval_ > 0) + { + // Invariant still holds: measurementHistoryDeque_.back().time_ < measurementQueue_.top().time_ + measurementHistory_.push_back(measurement); + } + // This will call predict and, if necessary, correct filter_.processMeasurement(measurement); + + // Only save one filter state for exactly one timestamp each. + // So wait with saving the state until all measurements with the same + // timestamp have been processed. + if (filterHistoryInterval_ > 0 && + (measurementQueue_.empty() || measurementQueue_.top().time_ > filter_.getLastMeasurementTime())) + { + saveFilterState(filter_); + } } filter_.setLastUpdateTime(currentTime); @@ -557,6 +590,15 @@ namespace RobotLocalization // Determine if we're in 2D mode nhLocal_.param("two_d_mode", twoDMode_, false); + // Smoothing window size. + // + // Internally stored in ms because checking double values to be exactly zero is unreliable. + { + double filter_history_interval_seconds = 0.; + nhLocal_.param("filter_history_interval", filter_history_interval_seconds, 0.); + filterHistoryInterval_ = int(filter_history_interval_seconds*1000); + } + // Debugging writes to file RF_DEBUG("tf_prefix is " << tfPrefix << "\nmap_frame is " << mapFrameId_ << @@ -567,6 +609,7 @@ namespace RobotLocalization "\nfrequency is " << frequency_ << "\nsensor_timeout is " << filter_.getSensorTimeout() << "\ntwo_d_mode is " << (twoDMode_ ? "true" : "false") << + "\nfilter_history_interval is " << filterHistoryInterval_ << "\nprint_diagnostics is " << (printDiagnostics_ ? "true" : "false") << "\n"); // Create a subscriber for manually setting/resetting pose @@ -1541,6 +1584,14 @@ namespace RobotLocalization while (ros::ok()) { + // Each second we remove old states and measurements from the queue. + static int flush_counter = 0; + flush_counter++; + if (flush_counter > frequency_) + { + removeHistoryStatesAndMeasurementsOlderThan(filter_.getLastMeasurementTime()-double(double(filterHistoryInterval_)/1000.0)); + } + // The spin will call all the available callbacks and enqueue // their received measurements ros::spinOnce(); @@ -2747,6 +2798,87 @@ namespace RobotLocalization return canTransform; } + + template + void RosFilter::saveFilterState(FilterBase& filter) + { + FilterState state; + state.state_ = filter.getState(); + state.estimate_error_covariance_ = filter.getEstimateErrorCovariance(); + state.time_ = filter.getLastMeasurementTime(); + filterStateHistory_.push_back(state); + ros::Time ts; + ts.fromSec(state.time_); + RF_DEBUG("\n---- RosFilter::saveFilterState for timestamp " << ts << " size = " << filterStateHistory_.size() << "----\n"); + } + + template + bool RosFilter::restoreFilterAndMeasurementsBefore(const double t) + { + RF_DEBUG("\n----- RosFilter::restoreFilterAndMeasurementsBefore (" << t << ") -----\n"); + // Remove all filter states older than t. This yields either the latest + // filter state before t or an empty queue. In that case the smoothing + // window is too small and we cannot reset the filter. + // + + int num_popped_filter_updates = 0; + while (!filterStateHistory_.empty() + && filterStateHistory_.back().time_ > t) + { + filterStateHistory_.pop_back(); + num_popped_filter_updates++; + } + RF_DEBUG("\n---- Popped " << num_popped_filter_updates << " filter updates. Queue is empty/size? " << filterStateHistory_.empty() << "/" << filterStateHistory_.size() << "\n"); + + if (filterStateHistory_.empty()) return false; + + // Reset filter to the latest state from the queue. + const FilterState &old = filterStateHistory_.back(); + filter_.setState(old.state_); + filter_.setEstimateErrorCovariance(old.estimate_error_covariance_); + filter_.setLastMeasurementTime(old.time_); + + // Find all measurements from the history whose timestamp is > t + // and push them to the measurements queue for integration. + // This re-establishes our invariant: measurementHistory_.back().time_ < measurementQueue_.top() + while (!measurementHistory_.empty() + && measurementHistory_.back().time_ > t) + { + measurementQueue_.push(measurementHistory_.back()); + measurementHistory_.pop_back(); + } + + RF_DEBUG("\n----- Restored measurement queue has " << measurementQueue_.size() << " entries now.\n"); + + RF_DEBUG("\n----- /RosFilter::restoreFilterAndMeasurementsBefore\n"); + + return true; + } + + template + void RosFilter::removeHistoryStatesAndMeasurementsOlderThan(const double t) + { + int debug_popped_measurements = 0; + int debug_popped_states = 0; + + while (!measurementHistory_.empty() + && measurementHistory_.front().time_ < t) + { + debug_popped_measurements++; + measurementHistory_.pop_front(); + } + + while (!filterStateHistory_.empty() + && filterStateHistory_.front().time_ < t) + { + debug_popped_states++; + filterStateHistory_.pop_front(); + } + ros::Time ts; ts.fromSec(t); + RF_DEBUG("\n---- /RosFilter::removeHistoryStatesAndMeasurementsOlderThan " + << debug_popped_states << "/" << debug_popped_measurements + << " older than " << ts << "----\n" ); + } } // namespace RobotLocalization // Instantiations of classes is required when template class code