diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 9 |
1 files changed, 5 insertions, 4 deletions
diff --git a/src/state.cpp b/src/state.cpp index c33c053..aee0caa 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix<double,Dynamic,Dynamic>::Zero(9,9); - P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity(); + P.block<3,3>(6,6) = 1e-6*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -180,7 +180,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, { MatrixXd K; // P^T is implied here since P is symmetric - K = S.partialPivLu().solve(h*P).transpose(); + K = S.fullPivLu().solve(h*P).transpose(); // Compute P_k|k P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; @@ -269,7 +269,7 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni featuresToAdd.push_back(*i); } } - if (featuresToUpdate.size()>0) { + if (featuresToUpdate.size()>1) { #ifdef FULLS MatrixXd h; h = H(q,featuresToUpdate); @@ -289,11 +289,12 @@ void State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) { // Add new features + double z = aboveWater(q); Vector3d pos = body->ned(); for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; // Create feature - Feature *f = new Feature(i->id, i->source, i->reflection, pos, q); + Feature *f = new Feature(i->id, i->source, i->reflection, pos, q, z); if (!f->sane()) { delete f; continue; |