Adjusted coordinate frame to NED and FRD.

The new coordinates frame for the world and the body correspond to
Junho's code and work well in other versions of this code.

2 files changed, 20 insertions, 12 deletions

diff --git a/src/body.cpp b/src/body.cpp
index b9ea683..daf4e69 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -23,7 +23,7 @@
 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);
+    Vector3d gravity_world(0.,0.,9.80655);
 
     Matrix<double,6,3> A;
     Matrix<double,6,1> b;
diff --git a/src/main.cpp b/src/main.cpp
index 44e09f3..86d87f4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -36,8 +36,11 @@ imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quate
 {
     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);
+    Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
+    Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
+    // WARNING: This is the bias for 1112-1
+    Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, 1.418145565750002614e-03);
+    ang-=ang_bias;
     Body mu;
     double dtf = dt.secs+dt.nsecs*1e-9;
     mu.motionModel(X,acc,ang,q,dtf);
@@ -52,14 +55,14 @@ pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<
     attitude_t A=msg.attitude;
     roll = M_PI*msg.attitude.roll/180.;
     pitch = M_PI*msg.attitude.pitch/180.;
-    yaw = M_PI*(90.-msg.attitude.yaw)/180.;
+    yaw = M_PI*(msg.attitude.yaw)/180.;
     Eigen::Matrix3d R;
     R = AngleAxisd(yaw, Vector3d::UnitZ())
-      * AngleAxisd(roll, Vector3d::UnitY())
-      *  AngleAxisd(pitch, Vector3d::UnitX());
+      * AngleAxisd(roll, Vector3d::UnitX())
+      *  AngleAxisd(pitch, Vector3d::UnitY());
     q = Eigen::Quaternion<double>(R);
     if (!seenpva) {
-        Vector3d vw(msg.velocity.east,msg.velocity.north,msg.velocity.up);
+        Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
         X.segment(3,3) = R.transpose()*vw;
         seenpva=true;
     }
@@ -71,9 +74,9 @@ utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
 {
     if (!seenutm) {
         seenutm=true;
-        X[0] = msg.utm.easting;
-        X[1] = msg.utm.northing;
-        X[2] = msg.utm.up;
+        X[0] = msg.utm.northing;
+        X[1] = msg.utm.easting;
+        X[2] = -msg.utm.up;
     }
     return;
 }
@@ -165,13 +168,18 @@ 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;
+    // bias in FRD coordinates
+    bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229;
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
+    using std::cout;
+    using std::endl;
+    using std::cerr;
+printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
 // Read sensors from file
 char line[MAXLINE];
 while (scanf("%s", line)!=EOF) {
@@ -193,7 +201,7 @@ while (scanf("%s", line)!=EOF) {
             pvaCallback(msg, bodyStateVector, qbw);
             //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]);
+                printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[0],bodyStateVector[1]);
             break;
 
         case RAWIMUS: