Add motion model.

The motion model is broken into two parts, the creation of the L matrix
and the state update. This means that ydot can be computed from the
state object for many features at once, saving computation time.

2 files changed, 49 insertions, 2 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 45e24e7..e44d597 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -18,6 +18,52 @@
  */
 #include "feature.h"
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: L
+ * Description:  Returns the motion model matrix L. These can be stacked to
+ * update the states of many features simultaneously.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,3,6>
+Feature::L ( )
+{
+    double y0,y1,y2;
+    Matrix<double,3,6> L;
+    y0 = X[0];
+    y1 = X[1];
+    y2 = X[2];
+
+    L << -y2, 0, y2*y0, y0*y1, -(1+y0*y0), y1,
+         0, -y2, y1*y2, 1+y1*y1, -y0*y1, -y0,
+         0, 0, y2*y2, y2*y1, -y2*y0, 0;
+
+    return L;
+}		/* -----  end of method Feature::L  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: motionModel
+ * Description:  Updates the feature state according to the motion model.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Feature::motionModel ( const Vector3d  &ang, const Vector3d &vel, const double dt)
+{
+    Matrix<double,3,6> L;
+    L = 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;
+    X += ydot*dt;
+    return ;
+}		/* -----  end of method Feature::motionModel  ----- */
+
 Matrix<double,6,1>
 Feature::h ( const Vector3d &x, const Quaterniond &q )
 {
diff --git a/src/feature.h b/src/feature.h
index 2982276..971c952 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -22,6 +22,7 @@ class Feature
         /* ====================  ACCESSORS     ======================================= */
 
         /* ====================  MUTATORS      ======================================= */
+        void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
 
         /* ====================  OPERATORS     ======================================= */
         Matrix<double,3,9> Fx();
@@ -29,9 +30,9 @@ class Feature
         Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
+        Matrix<double,3,6> L();
+
         /*
-        void motionModel ( const Vector3d &acc, const Vector3d &ang, 
-                const Quaterniond &q, const double dt);
         Matrix<double,9,9> Q(double dt);
         Matrix<double,1,1> R();
         Matrix<double,1,1> S(const Matrix<double,9,9> &P);