diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 39 |
1 files changed, 2 insertions, 37 deletions
diff --git a/src/state.cpp b/src/state.cpp index 93a1585..5121e18 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -186,11 +186,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, { MatrixXd K; // P^T is implied here since P is symmetric -#ifdef BLOCKSI // Here S is S^-1 - K = P*h.transpose()*S; -#else K = S.fullPivLu().solve(h*P).transpose(); -#endif // Compute P_k|k P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; @@ -202,8 +198,8 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, y = innovation(z,q); ya = y; - if ( (ya < -0.1).any() ) return; - if ( (ya > 0.1).any() ) return; + if ( (ya < -0.3).any() ) return; + if ( (ya > 0.3).any() ) return; // Get the update Matrix<double,Dynamic,1> dx; @@ -311,42 +307,11 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni } if (featuresToUpdate.size()>1) { -#ifdef FULLS MatrixXd h; h = H(q,featuresToUpdate); MatrixXd S; S = h*P*h.transpose() + R(featuresToUpdate); kalmanUpdate(h,S,featuresToUpdate,q); -#endif -#ifdef BLOCKSI - MatrixXd h; - h = H(q,featuresToUpdate); - MatrixXd S; - S = blockSI(featuresToUpdate,q); - cout << S << endl; - kalmanUpdate(h,S,featuresToUpdate,q); -#endif -#ifdef MEAS1 - for (auto i=featuresToUpdate.begin(); - i!=featuresToUpdate.end(); ++i) { - Matrix<double,Dynamic,Dynamic,0,6,6> S; - MatrixXd h; - std::vector<measurement_t> onez; - onez.push_back(*i); - h = H(q,onez); - if (i->z_type==HEIGHT) { - S = body->S(Pxx()); - } else if (i->z_type==REFLECTION) { - Feature *f = featureById(i->id); - S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q); - } else { - Feature *f = featureById(i->id); - S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q); - S = S.block<4,4>(0,0); - } - kalmanUpdate(h,S,onez,q); - } -#endif /* ----- not FULLS ----- */ } addFeatures( featuresToAdd, q, zmeas); |