diff options
| author | Martin Miller | 2017-03-29 22:19:39 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-03-29 22:19:39 -0500 |
| commit | 7dc9a5a7fefbb564ed9efeb77365a8aadc754833 (patch) | |
| tree | 910e78a47f46adc5fd6676f7cca14557d054f751 /src/state.cpp | |
| parent | e5e65d31eb86fc41ddc68e5bb09f3729f8c009ae (diff) | |
| download | refslam-7dc9a5a7fefbb564ed9efeb77365a8aadc754833.zip refslam-7dc9a5a7fefbb564ed9efeb77365a8aadc754833.tar.gz | |
tweak
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; |