Integration is implemented. But there are problems.

The coordinates are not working correctly. When I run integration on a
known dataset the body seems to turn in the opposite direction of
reality. It's probably best to change it to NED and FRD. Currenty it is
ENU and RFU.

1 files changed, 42 insertions, 0 deletions

diff --git a/src/body.h b/src/body.h
new file mode 100644
index 0000000..d668957
--- /dev/null
+++ b/src/body.h
@@ -0,0 +1,42 @@
+#ifndef  body_INC
+#define  body_INC
+#include <Eigen/Dense>
+#include <iostream>
+using Eigen::Matrix;
+using Eigen::Vector3d;
+using Eigen::Quaternion;
+
+/*
+ * =====================================================================================
+ *        Class:  Body
+ *  Description:  State representation of body.
+ * =====================================================================================
+ */
+class Body
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        Body (){};                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+        void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt);
+        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+}; /* -----  end of class Body  ----- */
+
+
+#endif   /* ----- #ifndef body_INC  ----- */