1 files changed, 50 insertions, 0 deletions
diff --git a/src/body.cpp b/src/body.cpp
new file mode 100644
index 0000000..b9ea683
--- /dev/null
+++ b/src/body.cpp
@@ -0,0 +1,50 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  body.cpp
+ *
+ *    Description:  Method definitions for body class.
+ *
+ *        Version:  1.0
+ *        Created:  03/17/2017 08:07:35 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+
+#include <Eigen/Dense>
+#include "body.h"
+
+    void
+Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
+{
+    Vector3d bias(X.segment(6,3));
+    Vector3d gravity_world(0.,0.,-9.80655);
+
+    Matrix<double,6,3> A;
+    Matrix<double,6,1> b;
+    A = Matrix<double,6,3>::Zero();
+    b = Matrix<double,6,1>::Zero();
+
+    Matrix<double,3,3> Rbw(q.toRotationMatrix());
+    A.block(0,0,3,3) = Rbw;
+    Matrix<double,3,3> W;
+    skewSymmetric(ang,W);
+    A.block(3,0,3,3) = -W;
+    b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
+
+    X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
+    return ;
+}		/* -----  end of method Body::motionModel  ----- */
+
+    void
+Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
+{
+    y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
+    return ;
+}		/* -----  end of method Body::skewSymmetric  ----- */
+