From fde220c19346797c4d55c2f479f7efb2f8c9073a Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 18 Mar 2017 14:32:52 -0500 Subject: Add support for INSCOVS. The position and velocity covariances are rotated into the correct frames: NED and FRD. --- src/body.cpp | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-- src/body.h | 6 +++++- src/main.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++--- src/main.h | 7 ++++++ 4 files changed, 131 insertions(+), 6 deletions(-) diff --git a/src/body.cpp b/src/body.cpp index daf4e69..330c0a8 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -19,7 +19,64 @@ #include #include "body.h" - void +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: H + * Description: Writes the Jacobian of the measurement model into H. + *-------------------------------------------------------------------------------------- + */ +void +Body::H ( Matrix &H ) +{ + H = Matrix::Zero(); + H[0,2] = 1; + return ; +} /* ----- end of method Body::H ----- */ + + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: h + * Description: Writes the predicted measurement vector into h. + *-------------------------------------------------------------------------------------- + */ +void +Body::h ( const Matrix &X, Matrix &h ) +{ + h[0] = X[2]; + return ; +} /* ----- end of method Body::h_hat ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: F + * Description: Computes the Jacobian of the motion model of the body. + *-------------------------------------------------------------------------------------- + */ +void +Body::F ( const Matrix &X, const Vector3d &ang, const Quaternion &q ) +{ + Matrix F = Matrix::Zero(); + Matrix Rbw(q.toRotationMatrix()); + Matrix W; + skewSymmetric(ang,W); + F.block(0,3,3,3) = Rbw; + F.block(3,3,3,3) = -W; + F.block(3,6,3,3) = -Matrix::Identity(); + return ; +} /* ----- end of method Body::F ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: motionModel + * Description: Propagates the motion model of the body into vector X. + *-------------------------------------------------------------------------------------- + */ +void Body::motionModel ( Matrix &X, const Vector3d &acc, const Vector3d &ang, const Quaternion &q, const double dt) { Vector3d bias(X.segment(6,3)); @@ -41,10 +98,18 @@ Body::motionModel ( Matrix &X, const Vector3d &acc, const Vector3d & return ; } /* ----- end of method Body::motionModel ----- */ - void +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: skewSymmetric + * Description: Create the skew symmetric matrix y from the vector x. + *-------------------------------------------------------------------------------------- + */ +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 index d668957..1fc88ff 100644 --- a/src/body.h +++ b/src/body.h @@ -23,8 +23,12 @@ class Body /* ==================== MUTATORS ======================================= */ /* ==================== OPERATORS ======================================= */ - void motionModel ( Matrix &X, const Vector3d &acc, const Vector3d &ang, const Quaternion &q, const double dt); + void motionModel ( Matrix &X, const Vector3d &acc, + const Vector3d &ang, const Quaternion &q, const double dt); + void F(const Matrix &X, const Vector3d &ang, const Quaternion &q); void skewSymmetric(const Vector3d &x, Matrix &y); + void H(Matrix &H); + void h(const Matrix &X, Matrix &h); protected: /* ==================== METHODS ======================================= */ diff --git a/src/main.cpp b/src/main.cpp index 86d87f4..a30ca6c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,15 +22,27 @@ #include "main.h" bool seenutm=false; bool seenpva=false; +Eigen::Matrix P; #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ void +covCallback(const message &msg, Eigen::Matrix &P, const Eigen::Quaternion &q) +{ + Eigen::Matrix3d Rbw(q.toRotationMatrix()); + Eigen::Matrix3d Renuned; + Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; + P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose(); + P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw; +} + +void imgCallback(const message *msg) { return; } + void imuCallback(const message &msg, Eigen::Matrix &X, const Eigen::Quaternion &q, const timestamp dt) { @@ -113,7 +125,39 @@ parseLine(char *line, message *data) rfn = strsep(&line, ","); sscanf(rfn, "%s", &data->image_names+1); } else if (!strcmp(msg_type,"inscovs")) { + using Eigen::Map; + using Eigen::Matrix3d; + double pos[9]; + double att[9]; + double vel[9]; data->msg_type = INSCOVS; + sscanf(line,"%lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf", + pos, pos+1, pos+2, + pos+3, pos+4, pos+5, + pos+6, pos+7, pos+8, + att, att+1, att+2, + att+3, att+4, att+5, + att+6, att+7, att+8, + vel, vel+1, vel+2, + vel+3, vel+4, vel+5, + vel+6, vel+7, vel+8); + data->covariance.position << pos[0], pos[1], pos[2], + pos[3], pos[4], pos[5], + pos[6], pos[7], pos[8]; + data->covariance.attitude << att[0], att[1], att[2], + att[3], att[4], att[5], + att[6], att[7], att[8]; + data->covariance.velocity << vel[0], vel[1], vel[2], + vel[3], vel[4], vel[5], + vel[6], vel[7], vel[8]; } else if (!strcmp(msg_type,"inspvas")) { data->msg_type = INSPVAS; sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", @@ -168,6 +212,7 @@ main(int argc, char **argv) { Eigen::Quaternion qbw; Eigen::Matrix bodyStateVector; + Eigen::Matrix P; // bias in FRD coordinates bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229; timestamp dt{0,0}; @@ -176,9 +221,9 @@ main(int argc, char **argv) #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ - using std::cout; - using std::endl; - using std::cerr; +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]; @@ -209,6 +254,10 @@ while (scanf("%s", line)!=EOF) { imuCallback(msg, bodyStateVector, qbw, dt); break; + case INSCOVS: + covCallback(msg, P, qbw); + cout << P << endl; + default: break; } /* ----- end switch ----- */ diff --git a/src/main.h b/src/main.h index 2139248..aa97995 100644 --- a/src/main.h +++ b/src/main.h @@ -17,6 +17,11 @@ #define MAXLINE 8192 #define MAXFILENAME 1024 +// A struct for storing PVA covariance. +typedef struct { + Eigen::Matrix3d position,velocity,attitude; +} covariance_t; + // A struct for storing x,y,z data. typedef struct { double x,y,z; @@ -69,6 +74,7 @@ typedef struct { gps position; UTM utm; velocity_t velocity; + covariance_t covariance; } message; int parseLine(char *line, message *msg); @@ -77,6 +83,7 @@ timestamp update_dt(const timestamp t, timestamp *t_old); #ifdef USE_ROS void imuCallback(); #else /* ----- not USE_ROS ----- */ +void covCallback(const message &msg, Eigen::Matrix &P, const Eigen::Quaternion &q); void imgCallback(const message *msg); 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); -- cgit v1.1