Add new update methods.

But they don't really work.

2 files changed, 101 insertions, 3 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 47364f1..4677595 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -186,7 +186,11 @@ 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;
@@ -308,8 +312,35 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
         MatrixXd S;
         S = h*P*h.transpose() + R(featuresToUpdate);
         kalmanUpdate(h,S,featuresToUpdate,q);
-#else      /* -----  not FULLS  ----- */
-    
+#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);
@@ -317,6 +348,68 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
     return ;
 }		/* -----  end of method State::feature_update  ----- */
 
+MatrixXd
+State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q )
+{
+    int rows = Hrows(z);
+    MatrixXd SI;
+    SI = MatrixXd::Zero(rows,rows);
+    {
+        int row = 0;
+        for (auto i=z.begin(); i!=z.end(); ++i) {
+            if (i->z_type==HEIGHT) {
+                Matrix<double,1,1> s;
+                s = body->S(Pxx());
+                SI.block<1,1>(row,row) = s.inverse();
+                row += 1;
+            } else if (i->z_type==REFLECTION) {
+                Feature *f;
+                Matrix<double,6,6> s;
+                f = featureById(i->id);
+                s = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+                SI.block<6,6>(row,row) = s.inverse();
+                row += 6;
+            } else if (i->z_type==MONO) {
+                /*
+                Feature *f;
+                Matrix<double,6,6> s;
+                f = featureById(i->id);
+                s = f->S(Pxx(), Pxy(i->id), Pyy(i->id), body->ned() ,q);
+                s = s.block<4,4>(0,0);
+                SI.block<4,4>(row,row) = s.inverse();
+                row += 4;
+                */
+            } else {
+                fprintf(stderr, "Unrecognized feature type, quitting\n");
+                exit(1);
+            }
+        }
+    }
+
+    return SI ;
+}		/* -----  end of method State::blockSI  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: Pxy
+ * Description:  Return the covariance block for feature with the given id.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,9,3>
+State::Pxy ( int id )
+{
+    int col = rowById(id);
+    return P.block<9,3>(0,col) ;
+}		/* -----  end of method State::Pxy  ----- */
+
+Matrix<double,3,3>
+State::Pyy ( int id )
+{
+    int row = rowById(id);
+    return P.block<3,3>(row,row) ;
+}		/* -----  end of method State::Pyy  ----- */
+
 void
 State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z)
 {
diff --git a/src/state.h b/src/state.h
index 0e1fe90..11bfa46 100644
--- a/src/state.h
+++ b/src/state.h
@@ -9,9 +9,11 @@
 #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.
+//#define BLOCKSI
+//#define MEAS1
 using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -42,7 +44,10 @@ class State
         int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
         MatrixXd R( const std::vector<measurement_t> &z );
+        MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
         Matrix<double,9,9> Pxx();
+        Matrix<double,9,3> Pxy(int id);
+        Matrix<double,3,3> Pyy(int id);
         Matrix<double,Dynamic,6> L();
 
         /* ====================  MUTATORS      ======================================= */