diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 21 |
1 files changed, 13 insertions, 8 deletions
diff --git a/src/state.cpp b/src/state.cpp index eac80d8..57ebbfa 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -49,8 +49,10 @@ State::enu ( const UTM &utm ) *-------------------------------------------------------------------------------------- */ MatrixXd -State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) +State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) { + Vector3d pos; + pos = body->ned(); MatrixXd h; // Determine the size of H int cols = 9 + 3*features.size(); @@ -161,7 +163,6 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector<measurement_t> &z, const Quaterniond &q) { MatrixXd K; - K = P*h.transpose()*S; // P^T is implied here since P is symmetric K = S.partialPivLu().solve(h*P).transpose(); P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; @@ -229,12 +230,11 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) } void -State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) +State::update ( const Quaterniond &q, const std::vector<measurement_t> &z ) { - #ifdef FULLS MatrixXd h; - h = H(pos,q,z); + h = H(q,z); MatrixXd S; S = h*P*h.transpose() + R(z); kalmanUpdate(h,S,z,q); @@ -255,14 +255,19 @@ void State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q) { std::vector<measurement_t> featuresToAdd; + std::vector<measurement_t> featuresToUpdate; for (auto i=z.begin(); i!=z.end(); ++i) { - if (i->z_type==HEIGHT) continue; - if (exists(i->id)) { - ; + if (i->z_type==HEIGHT) { + featuresToUpdate.push_back(*i); + } else if (exists(i->id)) { + featuresToUpdate.push_back(*i); } else { featuresToAdd.push_back(*i); } } + if (featuresToUpdate.size()>0) { + update(q, featuresToUpdate); + } addFeatures( featuresToAdd, q); return ; |