From 27e98b8b0aedca320e45b5184b3e426e5913e1a3 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Mon, 3 Apr 2017 15:10:47 -0500 Subject: Update body to use full state. The changes add quaternion estimation to the body state. The quaternion is still provided as an input, but it is ignored. --- src/body.cpp | 113 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++- src/body.h | 31 ++++++++++++---- 2 files changed, 136 insertions(+), 8 deletions(-) diff --git a/src/body.cpp b/src/body.cpp index 1b07361..99ea1ed 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -38,7 +38,11 @@ Body::clamp() *-------------------------------------------------------------------------------------- */ void +#ifdef FULLSTATE +Body::dx ( const Matrix &del ) +#else Body::dx ( const Matrix &del ) +#endif { X += del; clamp(); @@ -61,7 +65,11 @@ Body::vel ( const Matrix &v ) *-------------------------------------------------------------------------------------- */ Matrix +#ifdef FULLSTATE +Body::S ( const Matrix &Pxx ) +#else Body::S ( const Matrix &Pxx ) +#endif { Matrix S; S << Pxx(2,2); @@ -109,17 +117,31 @@ Body::R() * Description: Returns Q matrix of process noise for the body. *-------------------------------------------------------------------------------------- */ +#ifdef FULLSTATE +Matrix +#else Matrix +#endif Body::Q (double dt) { +#ifdef FULLSTATE + Matrix Q; + Q = Matrix::Zero(); +#else Matrix Q; Q = Matrix::Zero(); +#endif Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix::Identity(); Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix::Identity(); Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix::Identity(); Q *= IMU_NOISE; +#ifdef FULLSTATE + Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix::Identity(); + Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix::Identity(); +#else Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix::Identity(); +#endif return Q; } /* ----- end of method Body::q ----- */ @@ -130,11 +152,20 @@ Body::Q (double dt) * Description: Returns the Jacobian of the measurement model, H. *-------------------------------------------------------------------------------------- */ +#ifdef FULLSTATE +Matrix +#else Matrix +#endif Body::H ( ) { +#ifdef FULLSTATE + Matrix H; + H = Matrix::Zero(); +#else Matrix H; H = Matrix::Zero(); +#endif H(2) = 1.; return H ; } /* ----- end of method Body::H ----- */ @@ -162,24 +193,84 @@ Body::h ( ) * Description: Returns the Jacobian of the motion model of the body. *-------------------------------------------------------------------------------------- */ +#ifdef FULLSTATE +Matrix +#else Matrix +#endif Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) { +#ifdef FULLSTATE + Matrix F = Matrix::Zero(); + double qbw1,qbw2,qbw3,qbw4; + qbw1 = X(6); + qbw2 = X(7); + qbw3 = X(8); + qbw4 = X(9); + Quaterniond qbw(qbw4,qbw1,qbw2,qbw3); + Matrix Rbw(qbw.toRotationMatrix()); +#else Matrix F = Matrix::Zero(); Matrix Rbw(q.toRotationMatrix()); +#endif Matrix W; W = skewSymmetric(ang); F.block<3,3>(0,3) = Rbw; F.block<3,3>(3,3) = -W; F.block<3,3>(3,6) = -Matrix::Identity(); +#ifdef FULLSTATE + /* Jacobian generated using symbolic matlab */ + double v1,v2,v3; + v1 = X(3); + v2 = X(4); + v3 = X(5); + F.block<1,4>(0,6) << 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, + 2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3, + 2*qbw1*v3 - 2*qbw3*v1 - 2*qbw4*v2, + 2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1; + F.block<1,4>(1,6) << 2*qbw2*v1 - 2*qbw1*v2 - 2*qbw4*v3, + 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, + 2*qbw2*v3 - 2*qbw3*v2 + 2*qbw4*v1, + 2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2; + F.block<1,4>(2,6) << 2*qbw3*v1 - 2*qbw1*v3 + 2*qbw4*v2, + 2*qbw3*v2 - 2*qbw2*v3 - 2*qbw4*v1, + 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, + 2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3; + F.block<3,4>(3,6) << -2*gravity_world*qbw3, 2*gravity_world*qbw4, -2*gravity_world*qbw1, 2*gravity_world*qbw2, + -2*gravity_world*qbw4, -2*gravity_world*qbw3, -2*gravity_world*qbw2, -2*gravity_world*qbw1, + 2*gravity_world*qbw1, 2*gravity_world*qbw2, -2*gravity_world*qbw3, -2*gravity_world*qbw4; + F.block<4,4>(6,6) = omega(ang); + F *= dt; + F += Matrix::Identity(); +#else F *= dt; F += Matrix::Identity(); +#endif return F; } /* ----- end of method Body::F ----- */ /* *-------------------------------------------------------------------------------------- * Class: Body + * Method: Body :: omega + * Description: Returns the Omega(w) used for the motion model of the + * quaternion. + *-------------------------------------------------------------------------------------- + */ +Matrix +Body::omega ( const Vector3d &x ) +{ + Matrix Omega; + Omega.block<3,3>(0,0) = -skewSymmetric(x); + Omega.block<3,1>(0,3) = x; + Omega.block<1,3>(3,0) = x.transpose(); + Omega(3,3) = 0; + return Omega; +} /* ----- end of method Body::omega ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Body * Method: Body :: motionModel * Description: Propagates the motion model of the body into vector X. *-------------------------------------------------------------------------------------- @@ -187,7 +278,11 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) void Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt) { - Vector3d bias(X.segment(6,3)); +#ifdef FULLSTATE + Vector3d bias(X.segment<3>(10)); +#else + Vector3d bias(X.segment<3>(6)); +#endif Vector3d gravity_world(0.,0.,9.80655); Matrix A; @@ -195,7 +290,12 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond A = Matrix::Zero(); b = Matrix::Zero(); +#ifdef FULLSTATE + Quaterniond qbw(X(9),X(6),X(7),X(8)); + Matrix Rbw(qbw.toRotationMatrix()); +#else Matrix Rbw(q.toRotationMatrix()); +#endif A.block<3,3>(0,0) = Rbw; Matrix W; W = skewSymmetric(ang); @@ -203,6 +303,9 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; +#ifdef FULLSTATE + X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6); +#endif clamp(); return ; } /* ----- end of method Body::motionModel ----- */ @@ -258,14 +361,22 @@ Body::vel ( ) return X.segment<3>(3); } /* ----- end of method Body::vel ----- */ +#ifdef FULLSTATE +Matrix +#else Matrix +#endif Body::asVector ( ) { return X; } /* ----- end of method Body::asVector ----- */ void +#ifdef FULLSTATE +Body::asVector ( const Matrix &m ) +#else Body::asVector ( const Matrix &m ) +#endif { X = m; return ; diff --git a/src/body.h b/src/body.h index 7bbd50d..b973b00 100644 --- a/src/body.h +++ b/src/body.h @@ -9,6 +9,7 @@ //#define DOCLAMP #define MAXHEIGHT -1.5 /* Notion of max and min is reversed due to z pointing down */ #define MINHEIGHT -0.3 /* */ +#define FULLSTATE /* Uncomment to estimate quaternion */ using Eigen::Matrix; @@ -25,31 +26,42 @@ class Body { public: /* ==================== LIFECYCLE ======================================= */ - Body (){}; /* constructor */ + Body (){ gravity_world << 0.,0.,9.80655; }; /* constructor */ /* ==================== ACCESSORS ======================================= */ +#ifdef FULLSTATE + Matrix asVector(); + void asVector(const Matrix &m); + void dx( const Matrix &del); + Matrix F(const Vector3d &ang, const Quaterniond &q, double dt); + Matrix H(); + Matrix Q(double dt); + Matrix S(const Matrix &Pxx); +#else + Matrix F(const Vector3d &ang, const Quaterniond &q, double dt); Matrix asVector(); + void asVector(const Matrix &m); + void dx( const Matrix &del); + Matrix H(); + Matrix Q(double dt); + Matrix S(const Matrix &Pxx); +#endif Vector3d ned(); Vector3d vel(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); - void asVector(const Matrix &m); void clamp(); void pos( const UTM &utm); - void dx( const Matrix &del); void vel(const Matrix &v); /* ==================== OPERATORS ======================================= */ - Matrix F(const Vector3d &ang, const Quaterniond &q, double dt); - Matrix H(); Matrix h(); void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); - Matrix Q(double dt); Matrix R(); - Matrix S(const Matrix &Pxx); Matrix skewSymmetric(const Vector3d &x); + Matrix omega(const Vector3d &x); void unicsv(); protected: @@ -61,9 +73,14 @@ class Body /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ +#ifdef FULLSTATE + Matrix X; +#else Matrix X; +#endif char utm_c; int utm_i; + Vector3d gravity_world; }; /* ----- end of class Body ----- */ -- cgit v1.1