From 7dc9a5a7fefbb564ed9efeb77365a8aadc754833 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 29 Mar 2017 22:19:39 -0500
Subject: tweak

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

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 94e8a5c..93a1585 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -198,7 +198,12 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
 
     // Compute the innovation or error
     Matrix<double,Dynamic,1> y;
+    Eigen::ArrayXd ya;
     y = innovation(z,q);
+    ya = y;
+
+    if ( (ya < -0.1).any() ) return;
+    if ( (ya > 0.1).any() ) return;
 
     // Get the update
     Matrix<double,Dynamic,1> dx;
-- 
cgit v1.1