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