diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 7 |
1 files changed, 4 insertions, 3 deletions
diff --git a/src/state.cpp b/src/state.cpp index aee0caa..aaf9cb9 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -260,9 +260,11 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni { std::vector<measurement_t> featuresToAdd; std::vector<measurement_t> featuresToUpdate; + double zmeas = body->ned()[2]; for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) { featuresToUpdate.push_back(*i); + zmeas = i->height; } else if (exists(i->id)) { featuresToUpdate.push_back(*i); } else { @@ -280,16 +282,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni #endif /* ----- not FULLS ----- */ } - addFeatures( featuresToAdd, q); + addFeatures( featuresToAdd, q, zmeas); return ; } /* ----- end of method State::feature_update ----- */ void -State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) +State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z) { // 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; |