1 files changed, 100 insertions, 4 deletions

diff --git a/src/state.cpp b/src/state.cpp
index dae6a4d..281fac3 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -41,11 +41,79 @@ State::enu ( const UTM &utm )
     return ;
 }		/* -----  end of method State::enu  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: H
+ * Description:  Returns the matrix H for the measurements z
+ *--------------------------------------------------------------------------------------
+ */
+MatrixXd
+State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z )
+{
+    MatrixXd h;
+    // Determine the size of H
+    int rows = 0;
+    for (auto i=z.begin(); i!=z.end(); ++i) {
+        switch ( i->z_type ) {
+            case REFLECTION:	
+                rows += 6;
+                break;
+
+            case MONO:	
+                rows += 4;
+                break;
+
+            case HEIGHT:	
+                rows += 1;
+                break;
+
+            default:	
+                break;
+        }				/* -----  end switch  ----- */
+    }
+    int cols = 9 + 3*features.size();
+    h = MatrixXd::Zero(rows,cols);
+    for (int i=0,row=0; i<z.size(); ++i) {
+        switch ( z[i].z_type ) {
+            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);
+                }
+
+                h.block<6,9>(row,0) = fi->Hx(pos,q);
+                h.block<6,3>(row,col) = fi->Hy(pos,q);
+                row += 6;
+                break;
+
+            case MONO:	
+                fprintf(stderr, "mono points not supported.\n");
+                exit(1);
+                break;
+
+            case HEIGHT:	
+                h.block<1,9>(row,0) = body->H();
+                row += 1;
+                break;
+
+            default:	
+                break;
+        }				/* -----  end switch  ----- */
+    }
+    return h;
+}		/* -----  end of method State::H  ----- */
+
 void 
-State::update ( const Matrix<double,1,1> &z )
+State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z )
 {
-    Matrix<double,1,9> H;
-    H=body->H();
+    MatrixXd h;
+    h = H(pos,q,z);
+    /*
     Matrix<double,1,1> S;
     S=body->S(Pxx());
     Matrix<double,9,1> K;
@@ -57,6 +125,7 @@ State::update ( const Matrix<double,1,1> &z )
     h = body->h();
     dx = K*(z-h);
     body->update(dx);
+    */
 }
 
 void
@@ -196,7 +265,7 @@ void
 State::motionModel ( const Vector3d &acc, const Vector3d &ang,
     const Quaterniond &q, const double dt)
 {
-    //Pkk1(ang,q,dt);
+    Pkk1(ang,q,dt);
     body->motionModel(acc,ang,q,dt);
 
     Vector3d vel;
@@ -334,6 +403,33 @@ State::exists ( int id )
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
+ *      Method:  State :: rowById
+ * Description:  Returns the first row of the feature with the given ID. Returns
+ * -1 if not found.
+ *--------------------------------------------------------------------------------------
+ */
+int
+State::rowById ( int id )
+{
+    int j=9;
+    for (auto i=features.begin(); i!=features.end(); ++i, j+=3) {
+        if ((*i)->id()==id) return j;
+    }
+    return -1;
+}		/* -----  end of method State::rowById  ----- */
+
+Feature *
+State::featureById ( int id )
+{
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        if ((*i)->id()==id) return *i;
+    }
+    return NULL;
+}		/* -----  end of method State::featureById  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
  *      Method:  State :: expandP
  * Description:  Expand the P matrix to fit one more feature.
  *--------------------------------------------------------------------------------------