Add State::R() method, refactor H()

Method to compute R for given vector of z measurements. Added a method
State::Hrows(z) to return number of rows in H(z).

2 files changed, 64 insertions, 14 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 281fac3..243c34b 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -53,6 +53,46 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
 {
     MatrixXd h;
     // Determine the size of H
+    int cols = 9 + 3*features.size();
+    int rows = Hrows(z);
+    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  ----- */
+
+int
+State::Hrows ( const std::vector<measurement_t> &z) 
+{
     int rows = 0;
     for (auto i=z.begin(); i!=z.end(); ++i) {
         switch ( i->z_type ) {
@@ -72,22 +112,22 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
                 break;
         }				/* -----  end switch  ----- */
     }
-    int cols = 9 + 3*features.size();
-    h = MatrixXd::Zero(rows,cols);
+    return rows ;
+}		/* -----  end of method State::Hrows  ----- */
+
+MatrixXd
+State::R ( const std::vector<measurement_t> &z )
+{
+    int rows = Hrows(z);
+    MatrixXd r;
+    r = MatrixXd::Zero(rows,rows);
+
     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);
+                r.block<6,6>(row,row) = fi->R();
                 row += 6;
                 break;
 
@@ -97,7 +137,7 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
                 break;
 
             case HEIGHT:	
-                h.block<1,9>(row,0) = body->H();
+                r.block<1,1>(row,row) = body->R();
                 row += 1;
                 break;
 
@@ -105,14 +145,22 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
                 break;
         }				/* -----  end switch  ----- */
     }
-    return h;
-}		/* -----  end of method State::H  ----- */
+
+    return r ;
+}		/* -----  end of method State::R  ----- */
 
 void 
 State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z )
 {
     MatrixXd h;
     h = H(pos,q,z);
+    
+#ifdef  FULLS
+    MatrixXd S;
+    S = h*P*h.transpose() + R(z);
+#else      /* -----  not FULLS  ----- */
+        <+ELSE PART+>
+#endif     /* -----  not FULLS  ----- */
     /*
     Matrix<double,1,1> S;
     S=body->S(Pxx());
diff --git a/src/state.h b/src/state.h
index 43342a7..2a18e36 100644
--- a/src/state.h
+++ b/src/state.h
@@ -38,7 +38,9 @@ class State
         Feature *featureById(int id);
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
+        int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
+        MatrixXd R( const std::vector<measurement_t> &z );
         Matrix<double,9,9> Pxx();
         Matrix<double,Dynamic,6> L();