diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/body.cpp | 108 | ||||
| -rw-r--r-- | src/body.h | 23 | ||||
| -rw-r--r-- | src/main.cpp | 83 | ||||
| -rw-r--r-- | src/main.h | 78 | ||||
| -rw-r--r-- | src/state.cpp | 83 | ||||
| -rw-r--r-- | src/state.h | 29 |
6 files changed, 249 insertions, 155 deletions
diff --git a/src/body.cpp b/src/body.cpp index c4d8a82..2832ea4 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -15,10 +15,71 @@ * * ===================================================================================== */ - -#include <Eigen/Dense> #include "body.h" +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: update + * Description: Increments the state by dx after a Kalman update. + *-------------------------------------------------------------------------------------- + */ +void +Body::update ( const Matrix<double,9,1> &dx ) +{ + X += dx; + return ; +} /* ----- end of method Body::update ----- */ + +void +Body::vel ( const Matrix<double,3,1> &v ) +{ + X.segment(3,3) = v; + return ; +} /* ----- end of method Body::vel ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: S + * Description: Returns the matrix S=HPH.T+R. HPH.T is trivial, so Body::H() is + * not called explicitly. + *-------------------------------------------------------------------------------------- + */ +Matrix<double,1,1> +Body::S ( const Matrix<double,9,9> &P ) +{ + Matrix<double,1,1> S; + S << P(2,2); + return S+R(); +} /* ----- end of method Body::S ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: enu + * Description: Stores the position input as ENU. Internally stored as NED. + *-------------------------------------------------------------------------------------- + */ +void +Body::enu ( const UTM &utm ) +{ + utm_c = utm.zone_c; + utm_i = utm.zone_i; + X[0] = utm.northing; + X[1] = utm.easting; + X[2] = -utm.up; + return ; +} /* ----- end of method Body::enu ----- */ + + +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: R + * Description: Returns R matrix of measurement noise for height measurement. + *-------------------------------------------------------------------------------------- + */ Matrix<double,1,1> Body::R() { @@ -27,6 +88,13 @@ Body::R() return R; } +/* + *-------------------------------------------------------------------------------------- + * Class: Body + * Method: Body :: Q + * Description: Returns Q matrix of process noise for the body. + *-------------------------------------------------------------------------------------- + */ Matrix<double,9,9> Body::Q (double dt) { @@ -45,15 +113,16 @@ Body::Q (double dt) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: H - * Description: Writes the Jacobian of the measurement model into H. + * Description: Returns the Jacobian of the measurement model, H. *-------------------------------------------------------------------------------------- */ -void -Body::H ( Matrix<double,1,9> &H ) +Matrix<double,1,9> +Body::H ( ) { + Matrix<double,1,9> H; H = Matrix<double,1,9>::Zero(); H[0,2] = 1; - return ; + return H ; } /* ----- end of method Body::H ----- */ @@ -61,11 +130,11 @@ Body::H ( Matrix<double,1,9> &H ) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: h - * Description: Writes the predicted measurement vector into h. + * Description: Returns the predicted measurement vector, h. *-------------------------------------------------------------------------------------- */ Matrix<double,1,1> -Body::h ( const Matrix<double,9,1> &X) +Body::h ( ) { Matrix<double,1,1> h; h[0] = X[2]; @@ -76,11 +145,11 @@ Body::h ( const Matrix<double,9,1> &X) *-------------------------------------------------------------------------------------- * Class: Body * Method: Body :: F - * Description: Computes the Jacobian of the motion model of the body. + * Description: Returns the Jacobian of the motion model of the body. *-------------------------------------------------------------------------------------- */ Matrix<double,9,9> -Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q ) +Body::F ( const Vector3d &ang, const Quaternion<double> &q ) { Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); @@ -100,7 +169,7 @@ Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<dou *-------------------------------------------------------------------------------------- */ void -Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt) +Body::motionModel ( 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); @@ -136,3 +205,20 @@ Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y ) } /* ----- end of method Body::skewSymmetric ----- */ +void +Body::unicsv ( ) +{ + printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", utm_i, utm_c, + X[1], X[0], -X[2], + X[3], X[4], X[5], + X[6], X[7], X[8]); + return ; +} /* ----- end of method Body::unicsv ----- */ + + void +Body::accelerometer_bias ( const Vector3d &b ) +{ + X.segment(6,3) = b; + return ; +} /* ----- end of method State::accelerometer_bias ----- */ + @@ -2,6 +2,8 @@ #define body_INC #include <Eigen/Dense> #include <iostream> +#include "types.h" + using Eigen::Matrix; using Eigen::Vector3d; using Eigen::Quaternion; @@ -21,16 +23,22 @@ class Body /* ==================== ACCESSORS ======================================= */ /* ==================== MUTATORS ======================================= */ + void accelerometer_bias( const Vector3d &b); + void enu( const UTM &utm); + void update(const Matrix<double,9,1> &dx); + void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ - void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, - const Vector3d &ang, const Quaternion<double> &q, const double dt); - Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q); - void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y); - void H(Matrix<double, 1,9> &H); - Matrix<double,1,1> h(const Matrix<double,9,1> &X); + Matrix<double,9,9> F(const Vector3d &ang, const Quaternion<double> &q); + Matrix<double,1,9> H(); + Matrix<double,1,1> h(); + void motionModel ( const Vector3d &acc, const Vector3d &ang, + const Quaternion<double> &q, const double dt); Matrix<double,9,9> Q(double dt); Matrix<double,1,1> R(); + Matrix<double,1,1> S(const Matrix<double,9,9> &P); + void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y); + void unicsv(); protected: /* ==================== METHODS ======================================= */ @@ -41,6 +49,9 @@ class Body /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ + Matrix<double,9,1> X; + char utm_c; + int utm_i; }; /* ----- end of class Body ----- */ diff --git a/src/main.cpp b/src/main.cpp index c3c6fa9..6cc4317 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,6 +22,7 @@ #include "main.h" bool seenutm=false; bool seenpva=false; +bool seencov=false; using std::cout; using std::endl; using std::cerr; @@ -30,15 +31,15 @@ using std::cerr; #else /* ----- not USE_ROS ----- */ void -covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q) +covCallback(const message &msg, State &mu, const Quaternion<double> &q) { - if (seenutm==false || seenpva==false) { - Matrix3d Rbw(q.toRotationMatrix()); - 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; - } + if (seencov && seenutm && seenpva) return; + seencov=true; + Matrix3d Rbw(q.toRotationMatrix()); + Matrix3d Renuned; + Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; + mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose()); + mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw); } void @@ -48,28 +49,21 @@ imgCallback(const message *msg) } void -imuCallback(const message &msg, Matrix<double,9,1> &X, - Matrix<double,9,9> &P, const Quaternion<double> &q, - const timestamp dt) +imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt) { - if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return; + if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return; 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 body; - State mu; + ang -= ang_bias; double dtf = dt.secs+dt.nsecs*1e-9; - Matrix<double,9,9> F; - F = body.F(X,ang,q); - mu.Pkk1(P,F,body.Q(dtf)); - body.motionModel(X,acc,ang,q,dtf); + mu.motionModel(acc,ang,q,dtf); } void -pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q) +pvaCallback(const message &msg, State &mu, Quaternion<double> &q) { // Update quaternion using Eigen::AngleAxisd; @@ -85,31 +79,24 @@ pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q) q = Quaternion<double>(R); // Set body velocity - if (!seenpva) { + if (!seenpva || !seencov || !seenutm) { Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up); - X.segment(3,3) = R.transpose()*vw; + mu.vel(R.transpose()*vw); seenpva=true; } return; } void -utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P) +utmCallback(const message &msg, State &mu) { - if (!seenutm) { + if (!seenutm || !seencov || !seenpva) { seenutm=true; - X[0] = msg.utm.northing; - X[1] = msg.utm.easting; - X[2] = -msg.utm.up; + mu.enu(msg.utm); } else { // Perform Z measurement - State mu; - Body body; - Matrix<double,1,1> z,h; - Matrix<double,1,9> H; - z(0) = -msg.utm.up; - h = body.h(X); - body.H(H); - mu.update(X,P,H,h,z,body.R()); + Matrix<double,1,1> z; + z << -msg.utm.up; + mu.update(z); } return; } @@ -229,11 +216,12 @@ update_dt(const timestamp t, timestamp *t_old) int main(int argc, char **argv) { + State mu; Quaternion<double> qbw; - Matrix<double,9,1> bodyStateVector; - Matrix<double,9,9> P; // bias in FRD coordinates - bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229; + Vector3d acc_bias; + acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229; + mu.accelerometer_bias(acc_bias); timestamp dt{0,0}; timestamp t_old{0,0}; @@ -249,8 +237,7 @@ while (scanf("%s", line)!=EOF) { switch ( msg.msg_type ) { case BESTUTM: - dt = update_dt(msg.stamp, &t_old); - utmCallback(msg,bodyStateVector,P); + utmCallback(msg, mu); break; case IMG: @@ -258,23 +245,19 @@ while (scanf("%s", line)!=EOF) { break; case INSPVAS: - dt = update_dt(msg.stamp, &t_old); - pvaCallback(msg, bodyStateVector, qbw); - //std::cout << bodyStateVector.transpose() << std::endl; - if (seenutm) - printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c, - bodyStateVector[0],bodyStateVector[1],-bodyStateVector[2], - bodyStateVector[3],bodyStateVector[4],bodyStateVector[5], - bodyStateVector[6],bodyStateVector[7],bodyStateVector[8]); + pvaCallback(msg, mu, qbw); + if (seenutm && seencov && seenpva) + mu.unicsv(); break; case RAWIMUS: dt = update_dt(msg.stamp, &t_old); - imuCallback(msg, bodyStateVector, P, qbw, dt); + imuCallback(msg, mu, qbw, dt); break; case INSCOVS: - covCallback(msg, P, qbw); + covCallback(msg, mu, qbw); + break; default: break; @@ -1,99 +1,29 @@ #ifndef main_INC #define main_INC -//#define USE_ROS /* Uncomment to use with ROS */ - -#ifndef USE_ROS +#include <cmath> #include <cstdio> #include <cstring> -#endif /* ----- USE_ROS ----- */ - -#include <cmath> #include <Eigen/Dense> #include <iostream> #include "body.h" #include "state.h" +#include "types.h" -#define MAXLINE 8192 -#define MAXFILENAME 1024 using Eigen::Matrix; using Eigen::Matrix3d; using Eigen::Quaternion; using Eigen::Vector3d; -// A struct for storing PVA covariance. -typedef struct { - Matrix3d position,velocity,attitude; -} covariance_t; - -// A struct for storing x,y,z data. -typedef struct { - double x,y,z; -} tuple; - -// A struct for storing velocity in world frame -typedef struct { - double east, north, up; -} velocity_t; - -// A struct for storing attitude data. -typedef struct { - double roll,pitch,yaw; -} attitude_t; - -// A struct for storing UTM data. -typedef struct { - double northing,easting,up; - int zone_i; - char zone_c; -} UTM; - -// A struct for storing GPS data. -typedef struct { - double latitude, longitude, altitude; -} gps; - -// Message types -typedef enum {BESTUTM,IMG,INSCOVS,INSPVAS,RAWIMUS} message_type; - -/* - * The message struct is a general container for all message types. Not all - * members are used for all messages, so care must be taken not to use garbage - * values. - */ -typedef struct { - int secs,nsecs; -} timestamp; - -typedef struct { - // These should always be set. - timestamp stamp; - message_type msg_type; - - // Only the members needed by the msg_type are stored. - tuple angular_velocity; - attitude_t attitude; - char image_names[2][MAXFILENAME]; - tuple linear_acceleration; - gps position; - UTM utm; - velocity_t velocity; - covariance_t covariance; -} 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 covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q); void imgCallback(const message *msg); -void imuCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P, - const Quaternion<double> &q, const timestamp dt); +void imuCallback(const message &msg, State &mu, const Quaternion<double> &q, const timestamp dt); void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q); -void utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P); -#endif /* ----- not USE_ROS ----- */ +void utmCallback(const message &msg, State &mu); #endif /* ----- #ifndef main_INC ----- */ diff --git a/src/state.cpp b/src/state.cpp index 294b522..304e5ed 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -17,18 +17,46 @@ */ #include "state.h" +State::State ( ) +{ + body = new Body; + P = Matrix<double,Dynamic,Dynamic>::Zero(9,9); + P.block(6,6,3,3) = 1e-6*Matrix3d::Identity(); + return ; +} /* ----- end of method State::State ----- */ + +void +State::vel ( const Matrix<double,3,1> &v ) +{ + body->vel(v); + return ; +} /* ----- end of method State::vel ----- */ + +void +State::enu ( const UTM &utm ) +{ + utm_c = utm.zone_c; + utm_i = utm.zone_i; + body->enu(utm); + return ; +} /* ----- end of method State::enu ----- */ + void -State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, - const Matrix<double,1,9> H, const Matrix<double,1,1> &h, - const Matrix<double,1,1> &z, const Matrix<double,1,1> &R) +State::update ( const Matrix<double,1,1> &z ) { + Matrix<double,1,9> H; + H=body->H(); Matrix<double,1,1> S; + S=body->S(P); Matrix<double,9,1> K; - S = H*P*H.transpose() + R; K = P*H.transpose()*S.inverse(); P = (Matrix<double,9,9>::Identity()-K*H)*P; P = 0.5*(P+P.transpose()); - X += K*(z-h); + Matrix<double,9,1> dx; + Matrix<double,1,1> h; + h = body->h(); + dx = K*(z-h); + body->update(dx); } /* @@ -38,13 +66,52 @@ State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, * Description: Updates P_k|k-1 *-------------------------------------------------------------------------------------- */ - void -State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F, - const Matrix<double,9,9> &Q ) +void +State::Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt) { + Matrix<double,9,9> F,Q; + Q = body->Q(dt); + F = body->F(ang,q); P = F*P*F.transpose()+Q; // Enforce symmetry P = 0.5*(P+P.transpose()); return ; } /* ----- end of method State::Pkk1 ----- */ +void +State::motionModel ( const Vector3d &acc, const Vector3d &ang, + const Quaternion<double> &q, const double dt) +{ + Pkk1(ang,q,dt); + body->motionModel(acc,ang,q,dt); + return ; +} /* ----- end of method State::motionModel ----- */ + +void +State::position_covariance ( const Matrix<double,3,3> &p ) +{ + P.block(0,0,3,3) = p; + return ; +} /* ----- end of method State::position_covariance ----- */ + +void +State::velocity_covariance ( const Matrix<double,3,3> &p ) +{ + P.block(3,3,3,3) = p; + return ; +} /* ----- end of method State::velocity_covariance ----- */ + +void +State::unicsv ( ) +{ + body->unicsv(); + return ; +} /* ----- end of method State::unicsv ----- */ + +void +State::accelerometer_bias ( const Vector3d &b) +{ + body->accelerometer_bias(b); + return ; +} /* ----- end of method State::accelerometer_bias ----- */ + diff --git a/src/state.h b/src/state.h index 743f5d8..1efb0c2 100644 --- a/src/state.h +++ b/src/state.h @@ -1,7 +1,14 @@ #ifndef state_INC #define state_INC #include <Eigen/Dense> +#include "body.h" +#include "types.h" + +#define MAXFEATURES 30 using Eigen::Matrix; +using Eigen::Dynamic; +using Eigen::Vector3d; +using Eigen::Quaternion; /* * ===================================================================================== * Class: State @@ -12,18 +19,23 @@ class State { public: /* ==================== LIFECYCLE ======================================= */ - State (){}; /* constructor */ + State (); /* constructor */ /* ==================== ACCESSORS ======================================= */ /* ==================== MUTATORS ======================================= */ + void accelerometer_bias(const Vector3d &b); + void enu(const UTM &utm); + void motionModel(const Vector3d &acc, const Vector3d &ang, + const Quaternion<double> &q, const double dt); + void position_covariance(const Matrix<double,3,3> &p); + void velocity_covariance(const Matrix<double,3,3> &p); + void vel(const Matrix<double,3,1> &v); + void Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt); /* ==================== OPERATORS ======================================= */ - void Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F, - const Matrix<double,9,9> &Q ); - void update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, - const Matrix<double,1,9> H, const Matrix<double,1,1> &h, - const Matrix<double,1,1> &z, const Matrix<double,1,1> &R); + void update ( const Matrix<double,1,1> &z); + void unicsv(); protected: /* ==================== METHODS ======================================= */ @@ -34,6 +46,11 @@ class State /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ + Body *body; + Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P; + char utm_c; + int utm_i; + }; /* ----- end of class State ----- */ |