Skip to content

Fixing filter history and measurement queue behavior #279

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
May 6, 2016
Merged

Fixing filter history and measurement queue behavior #279

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 16 additions & 18 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -438,7 +438,7 @@ namespace RobotLocalization
// Invariant still holds: measurementHistoryDeque_.back().time_ < measurementQueue_.top().time_
measurementHistory_.push_back(measurement);

// We should only save the filter state one per unique timstamp
// We should only save the filter state once per unique timstamp
if (measurementQueue_.empty() ||
::fabs(measurementQueue_.top()->time_ - filter_.getLastMeasurementTime()) > 1e-9)
{
Expand Down Expand Up @@ -2580,7 +2580,8 @@ namespace RobotLocalization
state->estimateErrorCovariance_ = Eigen::MatrixXd(filter.getEstimateErrorCovariance());
state->lastMeasurementTime_ = filter.getLastMeasurementTime();
filterStateHistory_.push_back(state);
RF_DEBUG("Saved state with timestamp " << state->lastMeasurementTime_ << " to history. " << filterStateHistory_.size() << " measurements are in the queue.\n");
RF_DEBUG("Saved state with timestamp " << std::setprecision(20) << state->lastMeasurementTime_ <<
" to history. " << filterStateHistory_.size() << " measurements are in the queue.\n");
}

template<typename T>
Expand All @@ -2589,39 +2590,36 @@ namespace RobotLocalization
RF_DEBUG("\n----- RosFilter::revertTo -----\n");
RF_DEBUG("\nRequested time was " << std::setprecision(20) << time << "\n")

// Walk back through the queue until we reach a filter state whose time stamp
// is less than or equal to the requested time
FilterStateHistoryDeque::reverse_iterator state_it = filterStateHistory_.rbegin();

while(state_it != filterStateHistory_.rend() && (*state_it)->lastMeasurementTime_ > time)
// Walk back through the queue until we reach a filter state whose time stamp is less than or equal to the requested time.
// Since every saved state after that time will be overwritten/corrected, we can pop from the queue.
while (!filterStateHistory_.empty() && filterStateHistory_.back()->lastMeasurementTime_ > time)
{
state_it++;
filterStateHistory_.pop_back();
}

// The state and measurement histories are stored at the same time, so if we have insufficient state history, we
// will also have insufficient measurement history.
if (state_it == filterStateHistory_.rend())
if (filterStateHistory_.empty())
{
RF_DEBUG("Insufficient history to revert to time " << time << "\n");

return false;
}

// Reset filter to the latest state from the queue.
filter_.setState((*state_it)->state_);
filter_.setEstimateErrorCovariance((*state_it)->estimateErrorCovariance_);
filter_.setLastMeasurementTime((*state_it)->lastMeasurementTime_);
const FilterStatePtr &state = filterStateHistory_.back();
filter_.setState(state->state_);
filter_.setEstimateErrorCovariance(state->estimateErrorCovariance_);
filter_.setLastMeasurementTime(state->lastMeasurementTime_);

RF_DEBUG("Reverted to state with time " << (*state_it)->lastMeasurementTime_ << "\n");
RF_DEBUG("Reverted to state with time " << state->lastMeasurementTime_ << "\n");

// Repeat for measurements, but push every measurement onto the measurement queue as we go
MeasurementHistoryDeque::reverse_iterator meas_it = measurementHistory_.rbegin();
int restored_measurements = 0;

while (meas_it != measurementHistory_.rend() && (*meas_it)->time_ > time)
while (!measurementHistory_.empty() && measurementHistory_.back()->time_ > time)
{
measurementQueue_.push(*meas_it);
meas_it++;
measurementQueue_.push(measurementHistory_.back());
measurementHistory_.pop_back();
restored_measurements++;
}

Expand Down