Add State class

The State class contains the body and feature classes. It is responsible
for composing matrices, and performing Kalman updates.

2 files changed, 76 insertions, 0 deletions

diff --git a/src/state.cpp b/src/state.cpp
new file mode 100644
index 0000000..8aadb64
--- /dev/null
+++ b/src/state.cpp
@@ -0,0 +1,37 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  state.cpp
+ *
+ *    Description:  A Class for managing body and features.
+ *
+ *        Version:  1.0
+ *        Created:  03/17/2017 07:55:56 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+#include "state.h"
+
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: Pkk1
+ * Description:  Updates P_k|k-1
+ *--------------------------------------------------------------------------------------
+ */
+    void
+State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
+        const Matrix<double,9,9> &Q )
+{
+    P = F*P*F.transpose()+Q;
+    // Enforce symmetry
+    P = 0.5*(P+P.transpose());
+    return ;
+}		/* -----  end of method State::Pkk1  ----- */
+
diff --git a/src/state.h b/src/state.h
new file mode 100644
index 0000000..3a17ea5
--- /dev/null
+++ b/src/state.h
@@ -0,0 +1,39 @@
+#ifndef  state_INC
+#define  state_INC
+#include <Eigen/Dense>
+using Eigen::Matrix;
+/*
+ * =====================================================================================
+ *        Class:  State
+ *  Description:  
+ * =====================================================================================
+ */
+class State
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        State (){};                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+        void Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
+            const Matrix<double,9,9> &Q );
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+}; /* -----  end of class State  ----- */
+
+
+
+#endif   /* ----- #ifndef state_INC  ----- */