Use only FULLS kalman update

1 files changed, 2 insertions, 37 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 93a1585..5121e18 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -186,11 +186,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
 {
     MatrixXd K;
     // P^T is implied here since P is symmetric
-#ifdef BLOCKSI // Here S is S^-1
-    K = P*h.transpose()*S;
-#else
     K = S.fullPivLu().solve(h*P).transpose();
-#endif
 
     // Compute P_k|k
     P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
@@ -202,8 +198,8 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
     y = innovation(z,q);
     ya = y;
 
-    if ( (ya < -0.1).any() ) return;
-    if ( (ya > 0.1).any() ) return;
+    if ( (ya < -0.3).any() ) return;
+    if ( (ya > 0.3).any() ) return;
 
     // Get the update
     Matrix<double,Dynamic,1> dx;
@@ -311,42 +307,11 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
     }
     
     if (featuresToUpdate.size()>1) {
-#ifdef  FULLS
         MatrixXd h;
         h = H(q,featuresToUpdate);
         MatrixXd S;
         S = h*P*h.transpose() + R(featuresToUpdate);
         kalmanUpdate(h,S,featuresToUpdate,q);
-#endif
-#ifdef BLOCKSI
-        MatrixXd h;
-        h = H(q,featuresToUpdate);
-        MatrixXd S;
-        S = blockSI(featuresToUpdate,q);
-        cout << S << endl;
-        kalmanUpdate(h,S,featuresToUpdate,q);
-#endif
-#ifdef MEAS1
-        for (auto i=featuresToUpdate.begin();
-                i!=featuresToUpdate.end(); ++i) {
-            Matrix<double,Dynamic,Dynamic,0,6,6> S;
-            MatrixXd h;
-            std::vector<measurement_t> onez;
-            onez.push_back(*i);
-            h = H(q,onez);
-            if (i->z_type==HEIGHT) {
-                S = body->S(Pxx());
-            } else if (i->z_type==REFLECTION) {
-                Feature *f = featureById(i->id);
-                S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
-            } else {
-                Feature *f = featureById(i->id);
-                S = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
-                S = S.block<4,4>(0,0);
-            }
-            kalmanUpdate(h,S,onez,q);
-        }
-#endif     /* -----  not FULLS  ----- */
     }
     addFeatures( featuresToAdd, q, zmeas);