Compute Pk|k-1 using full state.

Previously State::Pkk1() was only being computed for the body states.
Methods State::F() and State::Q() were written to compose the full
versions of their respective matrices and the results are used to update
Pk|k-1.

1 files changed, 33 insertions, 5 deletions

diff --git a/src/state.cpp b/src/state.cpp
index f2344f0..959cf22 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -119,6 +119,36 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
     return ;
 }		/* -----  end of method State::initializePi  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: Q
+ * Description:  Compose full Q from body and features.
+ *--------------------------------------------------------------------------------------
+ */
+MatrixXd
+State::Q ( double dt )
+{
+    MatrixXd q;
+    int rows = 9 + 3*features.size();
+    q = MatrixXd::Zero(rows,rows);
+    q.topLeftCorner<9,9>() = body->Q(dt);
+    { // limit i's scope
+        auto i = features.begin();
+        for (int row=9; row<rows; row+=3, ++i) {
+            q.block<3,3>(row,row) = (*i)->Q(dt);
+        }
+    }
+    return q;
+}		/* -----  end of method State::q  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: F
+ * Description:  Compose full F from body and features
+ *--------------------------------------------------------------------------------------
+ */
 MatrixXd
 State::F ( const Quaterniond &q, const Vector3d &w, double dt )
 {
@@ -153,12 +183,10 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt )
 void
 State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 {
-    Matrix<double,9,9> Q;
-    Q = body->Q(dt);
-    MatrixXd f;
+    MatrixXd f,fullQ;
+    fullQ = Q(dt);
     f = F ( q, ang, dt );
-    //cout << f << endl;
-    P = f*P*f.transpose()+Q;
+    P = f*P*f.transpose()+fullQ;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
     return ;