Add State methods for kalman update and innovation.

State::kalmanUpdate should be general enough to work with both the full
and independent update methods.

The State::innovation method calculates the innovation vector directly
rather than building z and hhat individually.

2 files changed, 91 insertions, 11 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 243c34b..eac80d8 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -149,25 +149,100 @@ State::R ( const std::vector<measurement_t> &z )
     return r ;
 }		/* -----  end of method State::R  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: kalmanUpdate
+ * Description:  Performs the kalman update. 
+ *--------------------------------------------------------------------------------------
+ */
+void
+State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
+        const std::vector<measurement_t> &z, const Quaterniond &q)
+{
+    MatrixXd K;
+    K = P*h.transpose()*S;
+    // P^T is implied here since P is symmetric
+    K = S.partialPivLu().solve(h*P).transpose();
+    P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
+    P = 0.5*(P+P.transpose());
+
+    Matrix<double,Dynamic,1> y;
+    y = innovation(z,q);
+    Matrix<double,Dynamic,1> dx;
+    dx = K*y;
+    body->update(dx.segment<9>(0));
+    {
+        int row=9;
+        for (auto i=features.begin(); i!=features.end(); ++i, row+=3) {
+            (*i)->dx(dx.segment<3>(row));
+        }
+    }
+    
+    return ;
+}		/* -----  end of method State::kalmanUpdate  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: innovation
+ * Description:  Returns the innocation vector z-hhat
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,Dynamic,1>
+State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
+{
+    Matrix<double,Dynamic,1> y;
+    int rows = Hrows(z);
+    y = Matrix<double,Dynamic,1>::Zero(rows,1);
+
+    for (int i=0,row=0; i<z.size(); ++i) {
+        measurement_type mt=z[i].z_type;
+        if (mt==REFLECTION) {
+            Feature *fi;
+            fi = featureById(z[i].id);
+            Matrix<double,6,1> hi, zi;
+            hi = fi->h(body->ned(), q);
+            zi.segment<2>(0) = z[i].source.segment<2>(0);
+            zi.segment<2>(2) = fi->initialView();
+            zi.segment<2>(4) = z[i].reflection.segment<2>(0);
+            y.segment<6>(row) = zi-hi;
+            row += 6;
+        } else if (mt==MONO) {
+            fprintf(stderr, "mono points not supported.\n");
+            exit(1);
+        } else if (mt==HEIGHT) {
+            Matrix<double,1,1> zi, hi;
+            zi[0] = z[i].height;
+            Vector3d xbw;
+            xbw = body->ned();
+            hi[0] = xbw[2];
+
+            y.segment<1>(row) = zi-hi;
+            row += 1;
+        } else {
+            fprintf(stderr, "Unknown measurement type\n");
+        }
+    }
+
+    return y;
+}
+
 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 h;
+    h = H(pos,q,z);
     MatrixXd S;
     S = h*P*h.transpose() + R(z);
+    kalmanUpdate(h,S,z,q);
+
 #else      /* -----  not FULLS  ----- */
-        <+ELSE PART+>
+    
 #endif     /* -----  not FULLS  ----- */
     /*
-    Matrix<double,1,1> S;
-    S=body->S(Pxx());
-    Matrix<double,9,1> K;
-    K = P*H.transpose()*S.inverse();
-    P = (Matrix<double,9,9>::Identity()-K*H)*P;
-    P = 0.5*(P+P.transpose());
     Matrix<double,9,1> dx;
     Matrix<double,1,1> h;
     h = body->h();
@@ -181,6 +256,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
 {
     std::vector<measurement_t> featuresToAdd;
     for (auto i=z.begin(); i!=z.end(); ++i) {
+        if (i->z_type==HEIGHT) continue;
         if (exists(i->id)) {
             ;
         } else {
diff --git a/src/state.h b/src/state.h
index 2a18e36..3a90217 100644
--- a/src/state.h
+++ b/src/state.h
@@ -12,11 +12,12 @@
 #define MAXFEATURES 30
 //#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;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
-using Eigen::Dynamic;
-using Eigen::Vector3d;
 using Eigen::Quaterniond;
+using Eigen::Vector2d;
+using Eigen::Vector3d;
 using std::cout;
 using std::cerr;
 using std::endl;
@@ -49,6 +50,8 @@ class State
         void enu(const UTM &utm);
         void feature_update( 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);
         void motionModel(const Vector3d &acc, const Vector3d &ang,
                 const Quaterniond &q, const double dt);
         void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
@@ -59,6 +62,7 @@ class State
         void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
 
         /* ====================  OPERATORS     ======================================= */
+        Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
         void unicsv();
 
     protected: