Add Feature::S() method.

This is a method for computing S for an independent measurement. It can
provide a speed improvement over computing a full, correlated S, but may
reduce quality if correlated measurements are treated as independent.

2 files changed, 65 insertions, 10 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index e44d597..b83f8ee 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -21,6 +21,47 @@
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  Feature
+ *      Method:  Feature :: S
+ * Description:  Returns S, the information matrix for an independent
+ * measurement of the feature. For correlated measurements, combine Hx and Hy
+ * with other measurements outside of the instance and compute S.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,6,6>
+Feature::S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
+        const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q)
+{
+    Matrix<double,6,9> hx;
+    Matrix<double,6,3> hy;
+    hx = Hx( pos, q);
+    hy = Hy( pos, q);
+    Matrix<double,6,6> S;
+    S = hx*Pxx*hx.transpose() + 2*hx*Pxy*hy.transpose() + hy*Pyy*hy.transpose() + R();
+
+    return S;
+}		/* -----  end of method Feature::S  ----- */
+
+Matrix<double,3,3>
+Feature::Q ( const double dt )
+{
+    Matrix<double,3,3> Q;
+    Q = Matrix<double,3,3>::Identity();
+    Q *= dt*dt*1e-5;
+    return Q;
+}		/* -----  end of method Feature::q  ----- */
+
+Matrix<double,6,6>
+Feature::R (  )
+{
+    Matrix<double,6,6> R;
+    R = Matrix<double,6,6>::Identity();
+    R *= 1e-9;
+    return R;
+}		/* -----  end of method Feature::R  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
  *      Method:  Feature :: L
  * Description:  Returns the motion model matrix L. These can be stacked to
  * update the states of many features simultaneously.
@@ -52,14 +93,14 @@ Feature::L ( )
 void
 Feature::motionModel ( const Vector3d  &ang, const Vector3d &vel, const double dt)
 {
-    Matrix<double,3,6> L;
-    L = L();
+    Matrix<double,3,6> f;
+    f = L();
 
     Matrix<double,6,1> V;
     V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0];
 
     Matrix<double,3,1> ydot;
-    ydot = L*V;
+    ydot = f*V;
     X += ydot*dt;
     return ;
 }		/* -----  end of method Feature::motionModel  ----- */
@@ -138,6 +179,14 @@ Feature::x2p ( const Vector3d &x )
 
 
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: Fx
+ * Description:  Returns the Jacobian of the motion model with respect to the
+ * body.
+ *--------------------------------------------------------------------------------------
+ */
 Matrix<double,3,9>
 Feature::Fx ( )
 {
@@ -152,6 +201,14 @@ Feature::Fx ( )
     return F;
 }		/* -----  end of method Feature::Fx  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: Fy
+ * Description:  Returns the Jacobian of the motion model with respect to the
+ * feature.
+ *--------------------------------------------------------------------------------------
+ */
 Matrix<double,3,3>
 Feature::Fy ( const Vector3d &v, const Vector3d &w )
 {
diff --git a/src/feature.h b/src/feature.h
index 971c952..2a492c2 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -5,6 +5,7 @@
 
 using Eigen::Vector3d;
 using Eigen::Matrix;
+using Eigen::MatrixXd;
 using Eigen::Quaterniond;
 
 /*
@@ -32,13 +33,10 @@ class Feature
         Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         Matrix<double,3,6> L();
 
-        /*
-        Matrix<double,9,9> Q(double dt);
-        Matrix<double,1,1> R();
-        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
-        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
-        void unicsv();
-        */
+        Matrix<double,3,3> Q(const double dt);
+        Matrix<double,6,6> R();
+        Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
+            const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
 
     protected:
         /* ====================  METHODS       ======================================= */