From 6e0de4f0d4fac653dde826712e81cee95b470195 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Fri, 17 Mar 2017 21:31:12 -0500 Subject: 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. --- src/body.cpp | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/body.h | 42 ++++++++++++++++++++++++++++++++++++++++++ src/main.cpp | 52 +++++++++++++++++++++++++++++++++++++++++++--------- src/main.h | 5 ++++- 4 files changed, 139 insertions(+), 10 deletions(-) create mode 100644 src/body.cpp create mode 100644 src/body.h (limited to 'src') 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 +#include "body.h" + + void +Body::motionModel ( Matrix &X, const Vector3d &acc, const Vector3d &ang, const Quaternion &q, const double dt) +{ + Vector3d bias(X.segment(6,3)); + Vector3d gravity_world(0.,0.,-9.80655); + + Matrix A; + Matrix b; + A = Matrix::Zero(); + b = Matrix::Zero(); + + Matrix Rbw(q.toRotationMatrix()); + A.block(0,0,3,3) = Rbw; + Matrix 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 &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 +#include +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 &X, const Vector3d &acc, const Vector3d &ang, const Quaternion &q, const double dt); + void skewSymmetric(const Vector3d &x, Matrix &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 &X, Eigen::Quaternion &q) +imuCallback(const message &msg, Eigen::Matrix &X, const Eigen::Quaternion &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 &X, Eigen::Quaternion< void utmCallback(const message &msg, Eigen::Matrix &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 qbw; Eigen::Matrix 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 #include +#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 &X, Eigen::Quaternion &q); +void imuCallback(const message &msg, Eigen::Matrix &X, const Eigen::Quaternion &q, const timestamp dt); void pvaCallback(const message &msg, Eigen::Matrix &X, Eigen::Quaternion &q); void utmCallback(const message &msg, Eigen::Matrix &X); #endif /* ----- not USE_ROS ----- */ -- cgit v1.1