Set height above water.

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 ;