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<boost::shared_ptr<GraftSensor> > topics_;
+
+    bool diverged_;
 };
 
-#endif
\ No newline at end of file
+#endif
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<boost::shared_ptr<GraftSensor> > 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<boost::shared_ptr<GraftSensor> > topics_;
+
+    bool diverged_;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/GraftUKFAbsolute.cpp b/src/GraftUKFAbsolute.cpp
index 6e336b8..1301b69 100644
--- a/src/GraftUKFAbsolute.cpp
+++ b/src/GraftUKFAbsolute.cpp
@@ -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();
@@ -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; 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_THROTTLE(5.0, errmsg.str());
+	}
+
 	clearMessages(topics_);
 	return dt;
 }
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 <graft/GraftUKFAttitude.h>
  #include <ros/console.h>
 
- 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; 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_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 <graft/GraftUKFVelocity.h>
  #include <ros/console.h>
 
- 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; 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_THROTTLE(5.0, errmsg.str());
+	}
+
 	clearMessages(topics_);
 	return dt;
 }