EKF is working

4 files changed, 57 insertions, 50 deletions

diff --git a/src/body.cpp b/src/body.cpp
index e6afe8a..ff54e89 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -40,7 +40,7 @@ Body::dx ( const Matrix<double,9,1> &del )
 void
 Body::vel ( const Matrix<double,3,1> &v )
 {
-    X.segment(3,3) = v;
+    X.segment<3>(3) = v;
     return ;
 }		/* -----  end of method Body::vel  ----- */
 
@@ -127,7 +127,7 @@ Body::H ( )
 {
     Matrix<double,1,9> H;
     H = Matrix<double,1,9>::Zero();
-    H[0,2] = 1;
+    H(2) = 1.;
     return H ;
 }		/* -----  end of method Body::H  ----- */
 
@@ -192,9 +192,9 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     Matrix<double,3,3> W;
     W = skewSymmetric(ang);
     A.block<3,3>(3,0) = -W;
-    b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
+    b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
 
-    X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
+    X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
     return ;
 }		/* -----  end of method Body::motionModel  ----- */
 
@@ -226,7 +226,7 @@ Body::unicsv ( )
     void
 Body::accelerometer_bias ( const Vector3d &b )
 {
-    X.segment(6,3) = b;
+    X.segment<3>(6) = b;
     return ;
 }		/* -----  end of method State::accelerometer_bias  ----- */
 
diff --git a/src/main.cpp b/src/main.cpp
index bb2a3b3..897a319 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -98,7 +98,7 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
         if (fclose(fin)==EOF) {
             err_sys("fclose");
         }
-        mu.feature_update(z,q);
+        mu.handle_measurements(z,q);
     }
 
     return;
diff --git a/src/state.cpp b/src/state.cpp
index 1df68eb..c33c053 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(6,6,3,3) = 1e-9*Matrix3d::Identity();
+    P.block<3,3>(6,6) = 1e-9*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
@@ -63,13 +63,14 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
             int col;
             case REFLECTION:	
                 col = rowById(z[i].id);
-                Feature *fi;
-                fi = featureById(z[i].id);
                 if (col==-1) {
                     fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
                     exit(1);
                 }
 
+                Feature *fi;
+                fi = featureById(z[i].id);
+
                 h.block<6,9>(row,0) = fi->Hx(pos,q);
                 h.block<6,3>(row,col) = fi->Hy(pos,q);
                 row += 6;
@@ -92,6 +93,14 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
     return h;
 }		/* -----  end of method State::H  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: Hrows
+ * Description:  Returns the number of rows in the measurement vector based on
+ * the number and types of measurements.
+ *--------------------------------------------------------------------------------------
+ */
 int
 State::Hrows ( const std::vector<measurement_t> &z) 
 {
@@ -117,6 +126,13 @@ State::Hrows ( const std::vector<measurement_t> &z)
     return rows ;
 }		/* -----  end of method State::Hrows  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: R
+ * Description:  Returns the measurement noise matrix.
+ *--------------------------------------------------------------------------------------
+ */
 MatrixXd
 State::R ( const std::vector<measurement_t> &z )
 {
@@ -165,11 +181,16 @@ 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();
+
+    // Compute P_k|k
     P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
     P = 0.5*(P+P.transpose());
 
+    // Compute the innovation or error
     Matrix<double,Dynamic,1> y;
     y = innovation(z,q);
+
+    // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
     body->dx(dx.segment<9>(0));
@@ -187,7 +208,7 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
  *--------------------------------------------------------------------------------------
  *       Class:  State
  *      Method:  State :: innovation
- * Description:  Returns the innocation vector z-hhat
+ * Description:  Returns the innovation vector z-hhat
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,Dynamic,1>
@@ -227,31 +248,15 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
     return y;
 }
 
-void 
-State::update ( const Quaterniond &q, const std::vector<measurement_t> &z )
-{
-#ifdef  FULLS
-    MatrixXd h;
-    h = H(q,z);
-    MatrixXd S;
-    S = h*P*h.transpose() + R(z);
-    kalmanUpdate(h,S,z,q);
-    cout << features.size() << endl;
-
-#else      /* -----  not FULLS  ----- */
-    
-#endif     /* -----  not FULLS  ----- */
-    /*
-    Matrix<double,9,1> dx;
-    Matrix<double,1,1> h;
-    h = body->h();
-    dx = K*(z-h);
-    body->update(dx);
-    */
-}
-
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: handle_measurements
+ * Description:  Update EKF, and adds new features to state.
+ *--------------------------------------------------------------------------------------
+ */
 void
-State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
+State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q)
 {
     std::vector<measurement_t> featuresToAdd;
     std::vector<measurement_t> featuresToUpdate;
@@ -265,7 +270,15 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
         }
     }
     if (featuresToUpdate.size()>0) {
-        update(q, featuresToUpdate);
+#ifdef  FULLS
+        MatrixXd h;
+        h = H(q,featuresToUpdate);
+        MatrixXd S;
+        S = h*P*h.transpose() + R(featuresToUpdate);
+        kalmanUpdate(h,S,featuresToUpdate,q);
+#else      /* -----  not FULLS  ----- */
+    
+#endif     /* -----  not FULLS  ----- */
     }
     addFeatures( featuresToAdd, q);
 
@@ -276,11 +289,11 @@ void
 State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 {
     // Add new features
+    Vector3d pos = body->ned();
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
         // Create feature
-        Vector3d pos = body->ned();
-        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]);
+        Feature *f = new Feature(i->id, i->source, i->reflection, pos, q);
         if (!f->sane()) {
             delete f;
             continue;
@@ -288,13 +301,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
         features.push_back(f);
 
         // Expand P
-        expandP();
-        
-        // Initialize P values
-        int j=features.size();
-        Matrix<double,3,3> Pi;
-        Pi = f->P0();
-        initializePi(j,Pi);
+        expandP(f->P0());
     }
     return ;
 }		/* -----  end of method State::addFeatures  ----- */
@@ -564,9 +571,10 @@ State::featureById ( int id )
  *--------------------------------------------------------------------------------------
  */
 void
-State::expandP ( )
+State::expandP ( const Matrix3d &Pi)
 {
-    P.conservativeResize(3+P.rows(),3+P.cols());
+    P.conservativeResizeLike(MatrixXd::Zero(3+P.cols(),3+P.cols()));
+    P.bottomRightCorner<3,3>() = Pi;
     return ;
 }		/* -----  end of method State::expandP  ----- */
 
diff --git a/src/state.h b/src/state.h
index ea8cfef..4001216 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,7 +9,7 @@
 #include "feature.h"
 #include "types.h"
 
-#define MAXFEATURES 30
+#define MAXFEATURES 50
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define FULLS // Comment out to treat each measurement independently.
 using Eigen::Dynamic;
@@ -48,7 +48,7 @@ class State
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
         void pos(const UTM &utm);
-        void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
+        void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
         void initializePi(int i, const Matrix<double,3,3> &Pi);
         void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                 const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -59,7 +59,6 @@ class State
         std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
         void velocity_covariance(const Matrix<double,3,3> &p);
         void vel(const Matrix<double,3,1> &v);
-        void update ( const Quaterniond &q, const std::vector<measurement_t> &z );
 
         /* ====================  OPERATORS     ======================================= */
         Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
@@ -73,7 +72,7 @@ class State
     private:
         /* ====================  METHODS       ======================================= */
         void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
-        void expandP();
+        void expandP ( const Matrix3d &Pi);
         void shrinkP( int i );
 
         /* ====================  DATA MEMBERS  ======================================= */