Skip to content
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

Detect if filter has diverged #25

Open
wants to merge 3 commits into
base: hydro-devel
Choose a base branch
from
Open

Detect if filter has diverged #25

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b417cfa
Detect if filter has diverged
trainman419 Jun 17, 2014
f287c24
Rate-limit error messages
trainman419 Jun 1, 2015
b1882e8
Add divergence check to other filters
trainman419 Jun 1, 2015
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
4 changes: 3 additions & 1 deletion include/graft/GraftUKFAbsolute.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,8 @@ class GraftUKFAbsolute{
double kappa_;

std::vector<boost::shared_ptr<GraftSensor> > topics_;

bool diverged_;
};

#endif
#endif
31 changes: 30 additions & 1 deletion src/GraftUKFAbsolute.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
#include <graft/GraftUKFAbsolute.h>
#include <ros/console.h>

GraftUKFAbsolute::GraftUKFAbsolute(){
GraftUKFAbsolute::GraftUKFAbsolute() : diverged_(false){
graft_state_.setZero();
graft_state_(3) = 1.0; // Normalize quaternion
graft_control_.setZero();
Expand Down Expand Up @@ -371,6 +371,9 @@ double GraftUKFAbsolute::predictAndUpdate(){
if(topics_.size() == 0 || topics_[0] == NULL){
return 0;
}
if( diverged_ ) {
return 0;
}
ros::Time t = ros::Time::now();
double dt = (t - last_update_time_).toSec();
if(last_update_time_.toSec() < 0.0001){ // No previous updates
Expand Down Expand Up @@ -404,6 +407,32 @@ double GraftUKFAbsolute::predictAndUpdate(){

graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose();

for( int i=0; i<SIZE; i++ ) {
for( int j=0; j<SIZE; j++ ) {
if( !std::isfinite(graft_covariance_(i, j)) ) {
diverged_ = true;
}
}
}
if( diverged_ ) {
// print offending messages
std::stringstream errmsg;
errmsg << "Covariance diverged! Offending topics are: ";

// For each topic
for(size_t i = 0; i < topics_.size(); i++){
// Get the measurement msg and covariance
graft::GraftSensorResidual::ConstPtr meas = topics_[i]->z();
if( meas ) {
if( i>0 ) errmsg << ", ";
errmsg << topics_[i]->getName() << "(";
errmsg << *meas << ")";
}
}

ROS_ERROR_STREAM(errmsg.str());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

does it make sense to use a throttled message here? once you have diverged, this will spam the console quite badly I would think

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It does spam the console rather a lot. I'll switch it to a throttled message or a once-only message.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would suggest throttle over once-only so that if you remotely open rqt_console or similar after the filter has diverged, you will still see the message.

}

clearMessages(topics_);
return dt;
}
Expand Down