diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/body.cpp | 2 | ||||
| -rw-r--r-- | src/main.cpp | 30 |
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: |