From b417cfaf333026950544423df0e5ba527ab956e8 Mon Sep 17 00:00:00 2001 From: trainman419 Date: Tue, 17 Jun 2014 14:31:25 -0700 Subject: [PATCH 1/3] Detect if filter has diverged --- include/graft/GraftUKFAbsolute.h | 4 +++- src/GraftUKFAbsolute.cpp | 31 ++++++++++++++++++++++++++++++- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/include/graft/GraftUKFAbsolute.h b/include/graft/GraftUKFAbsolute.h index 6f6957d..61e32b1 100644 --- a/include/graft/GraftUKFAbsolute.h +++ b/include/graft/GraftUKFAbsolute.h @@ -91,6 +91,8 @@ class GraftUKFAbsolute{ double kappa_; std::vector > topics_; + + bool diverged_; }; -#endif \ No newline at end of file +#endif diff --git a/src/GraftUKFAbsolute.cpp b/src/GraftUKFAbsolute.cpp index 6e336b8..c14c7ef 100644 --- a/src/GraftUKFAbsolute.cpp +++ b/src/GraftUKFAbsolute.cpp @@ -34,7 +34,7 @@ #include #include - GraftUKFAbsolute::GraftUKFAbsolute(){ + GraftUKFAbsolute::GraftUKFAbsolute() : diverged_(false){ graft_state_.setZero(); graft_state_(3) = 1.0; // Normalize quaternion graft_control_.setZero(); @@ -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 @@ -404,6 +407,32 @@ double GraftUKFAbsolute::predictAndUpdate(){ graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose(); + for( int i=0; iz(); + if( meas ) { + if( i>0 ) errmsg << ", "; + errmsg << topics_[i]->getName() << "("; + errmsg << *meas << ")"; + } + } + + ROS_ERROR_STREAM(errmsg.str()); + } + clearMessages(topics_); return dt; } From f287c2441e4fa1c06e3feeaffed986120dcbdb6e Mon Sep 17 00:00:00 2001 From: trainman419 Date: Sun, 31 May 2015 21:59:16 -0700 Subject: [PATCH 2/3] Rate-limit error messages --- src/GraftUKFAbsolute.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/GraftUKFAbsolute.cpp b/src/GraftUKFAbsolute.cpp index c14c7ef..1301b69 100644 --- a/src/GraftUKFAbsolute.cpp +++ b/src/GraftUKFAbsolute.cpp @@ -430,7 +430,7 @@ double GraftUKFAbsolute::predictAndUpdate(){ } } - ROS_ERROR_STREAM(errmsg.str()); + ROS_ERROR_STREAM_THROTTLE(5.0, errmsg.str()); } clearMessages(topics_); From b1882e8f0f0934e57271dccb5727ab281b4dd3d9 Mon Sep 17 00:00:00 2001 From: trainman419 Date: Mon, 1 Jun 2015 03:35:46 -0700 Subject: [PATCH 3/3] Add divergence check to other filters --- include/graft/GraftUKFAttitude.h | 4 +++- include/graft/GraftUKFVelocity.h | 4 +++- src/GraftUKFAttitude.cpp | 31 ++++++++++++++++++++++++++++++- src/GraftUKFVelocity.cpp | 31 ++++++++++++++++++++++++++++++- 4 files changed, 66 insertions(+), 4 deletions(-) diff --git a/include/graft/GraftUKFAttitude.h b/include/graft/GraftUKFAttitude.h index 27259a3..478560e 100644 --- a/include/graft/GraftUKFAttitude.h +++ b/include/graft/GraftUKFAttitude.h @@ -91,6 +91,8 @@ class GraftUKFAttitude{ double kappa_; std::vector > topics_; + + bool diverged_; }; -#endif \ No newline at end of file +#endif diff --git a/include/graft/GraftUKFVelocity.h b/include/graft/GraftUKFVelocity.h index 3697a5e..7d34242 100644 --- a/include/graft/GraftUKFVelocity.h +++ b/include/graft/GraftUKFVelocity.h @@ -91,6 +91,8 @@ class GraftUKFVelocity{ double kappa_; std::vector > topics_; + + bool diverged_; }; -#endif \ No newline at end of file +#endif diff --git a/src/GraftUKFAttitude.cpp b/src/GraftUKFAttitude.cpp index 6905954..dabcbc9 100644 --- a/src/GraftUKFAttitude.cpp +++ b/src/GraftUKFAttitude.cpp @@ -34,7 +34,7 @@ #include #include - GraftUKFAttitude::GraftUKFAttitude(){ + GraftUKFAttitude::GraftUKFAttitude() : diverged_(false){ graft_state_.setZero(); graft_state_(0,0) = 1.0; // Normalize quaternion graft_control_.setZero(); @@ -330,6 +330,9 @@ double GraftUKFAttitude::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 @@ -362,6 +365,32 @@ double GraftUKFAttitude::predictAndUpdate(){ graft_state_.block(0, 0, 4, 1) = unitQuaternion(graft_state_.block(0, 0, 4, 1)); graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose(); + for( int i=0; iz(); + if( meas ) { + if( i>0 ) errmsg << ", "; + errmsg << topics_[i]->getName() << "("; + errmsg << *meas << ")"; + } + } + + ROS_ERROR_STREAM_THROTTLE(5.0, errmsg.str()); + } + clearMessages(topics_); return dt; } diff --git a/src/GraftUKFVelocity.cpp b/src/GraftUKFVelocity.cpp index 591b6d9..b22c2de 100644 --- a/src/GraftUKFVelocity.cpp +++ b/src/GraftUKFVelocity.cpp @@ -34,7 +34,7 @@ #include #include - GraftUKFVelocity::GraftUKFVelocity(){ + GraftUKFVelocity::GraftUKFVelocity() : diverged_(false){ graft_state_.setZero(); graft_control_.setZero(); graft_covariance_.setIdentity(); @@ -237,6 +237,9 @@ double GraftUKFVelocity::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 @@ -268,6 +271,32 @@ double GraftUKFVelocity::predictAndUpdate(){ graft_state_ = predicted_mean + K*(z - predicted_measurement); graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose(); + for( int i=0; iz(); + if( meas ) { + if( i>0 ) errmsg << ", "; + errmsg << topics_[i]->getName() << "("; + errmsg << *meas << ")"; + } + } + + ROS_ERROR_STREAM_THROTTLE(5.0, errmsg.str()); + } + clearMessages(topics_); return dt; }