diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 5 |
1 files changed, 5 insertions, 0 deletions
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; |