update

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;