From c1ca399fcb2516da1ebe8e6a92f3eb4fff44f224 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 7 Apr 2017 11:21:22 -0500
Subject: changes

---
 src/state.cpp | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 0bae8e7..937a653 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -204,6 +204,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
+    if (dx.segment<3>(3).norm()>0.04) {
+        cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl;
+        return K;
+    }
     assert (dx.rows()==STATESIZE+3*features.size());
     body->dx(dx.segment<STATESIZE>(0));
     {
-- 
cgit v1.1