Add State class

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

1 files changed, 37 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  ----- */
+