summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/body.cpp50
-rw-r--r--src/body.h42
-rw-r--r--src/main.cpp52
-rw-r--r--src/main.h5
4 files changed, 139 insertions, 10 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 ----- */
+
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 ----- */
diff --git a/src/main.cpp b/src/main.cpp
index 150a141..44e09f3 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -32,12 +32,15 @@ imgCallback(const message *msg)
return;
}
void
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
{
- if (!seenutm || !seenpva) return;
+ if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
using Eigen::Vector3d;
Vector3d acc(msg.linear_acceleration.x,msg.linear_acceleration.y,msg.linear_acceleration.z);
Vector3d ang(msg.angular_velocity.x,msg.angular_velocity.y,msg.angular_velocity.z);
+ Body mu;
+ double dtf = dt.secs+dt.nsecs*1e-9;
+ mu.motionModel(X,acc,ang,q,dtf);
}
void
@@ -66,10 +69,12 @@ pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<
void
utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
{
- seenutm=true;
- X[0] = msg.utm.easting;
- X[1] = msg.utm.northing;
- X[2] = msg.utm.up;
+ if (!seenutm) {
+ seenutm=true;
+ X[0] = msg.utm.easting;
+ X[1] = msg.utm.northing;
+ X[2] = msg.utm.up;
+ }
return;
}
#endif /* ----- not USE_ROS ----- */
@@ -136,10 +141,34 @@ parseLine(char *line, message *data)
}
}
-int main(int argc, char **argv)
+timestamp
+update_dt(const timestamp t, timestamp *t_old)
+{
+ double dtf=0;
+ if (t_old->secs!=0 || t_old->nsecs!=0) {
+ double tf = t.secs+1e-9*t.nsecs;
+ double t_oldf = t_old->secs+1e-9*t_old->nsecs;
+ dtf = tf-t_oldf;
+ }
+ t_old->secs=t.secs;
+ t_old->nsecs=t.nsecs;
+
+ timestamp dt;
+ dt.secs = (int) dtf;
+ dt.nsecs = (int)((dtf-(double)dt.secs)*1e9);
+
+ return dt;
+}
+
+int
+main(int argc, char **argv)
{
Eigen::Quaternion<double> qbw;
Eigen::Matrix<double,9,1> bodyStateVector;
+ bodyStateVector.segment(6,3) << 0.01465135, -0.03713532, -0.00709229;
+ timestamp dt{0,0};
+ timestamp t_old{0,0};
+
#ifdef USE_ROS
#else /* ----- not USE_ROS ----- */
@@ -151,6 +180,7 @@ while (scanf("%s", line)!=EOF) {
switch ( msg.msg_type ) {
case BESTUTM:
+ dt = update_dt(msg.stamp, &t_old);
utmCallback(msg,bodyStateVector);
break;
@@ -159,12 +189,16 @@ while (scanf("%s", line)!=EOF) {
break;
case INSPVAS:
+ dt = update_dt(msg.stamp, &t_old);
pvaCallback(msg, bodyStateVector, qbw);
- std::cout << bodyStateVector << std::endl;
+ //std::cout << bodyStateVector.transpose() << std::endl;
+ if (seenutm)
+ printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[1],bodyStateVector[0]);
break;
case RAWIMUS:
- imuCallback(msg, bodyStateVector, qbw);
+ dt = update_dt(msg.stamp, &t_old);
+ imuCallback(msg, bodyStateVector, qbw, dt);
break;
default:
diff --git a/src/main.h b/src/main.h
index e3dd911..2139248 100644
--- a/src/main.h
+++ b/src/main.h
@@ -12,6 +12,8 @@
#include <Eigen/Dense>
#include <iostream>
+#include "body.h"
+
#define MAXLINE 8192
#define MAXFILENAME 1024
@@ -70,12 +72,13 @@ typedef struct {
} message;
int parseLine(char *line, message *msg);
+timestamp update_dt(const timestamp t, timestamp *t_old);
#ifdef USE_ROS
void imuCallback();
#else /* ----- not USE_ROS ----- */
void imgCallback(const message *msg);
-void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);
+void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt);
void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);
void utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X);
#endif /* ----- not USE_ROS ----- */