diff options
author | Martin Miller | 2017-04-03 15:10:47 -0500 |
---|---|---|
committer | Martin Miller | 2017-04-03 15:10:47 -0500 |
commit | 27e98b8b0aedca320e45b5184b3e426e5913e1a3 (patch) | |
tree | 9799878b32f28d206c5a0cd92f4ea31ebe335aec /src | |
parent | 05a04acf46a0a5624e019e4dc947cd3cd587a80b (diff) | |
download | refslam-27e98b8b0aedca320e45b5184b3e426e5913e1a3.zip refslam-27e98b8b0aedca320e45b5184b3e426e5913e1a3.tar.gz |
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.
Diffstat (limited to 'src')
-rw-r--r-- | src/body.cpp | 113 | ||||
-rw-r--r-- | 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<double,13,1> &del ) +#else Body::dx ( const Matrix<double,9,1> &del ) +#endif { X += del; clamp(); @@ -61,7 +65,11 @@ Body::vel ( const Matrix<double,3,1> &v ) *-------------------------------------------------------------------------------------- */ Matrix<double,1,1> +#ifdef FULLSTATE +Body::S ( const Matrix<double,13,13> &Pxx ) +#else Body::S ( const Matrix<double,9,9> &Pxx ) +#endif { Matrix<double,1,1> S; S << Pxx(2,2); @@ -109,17 +117,31 @@ Body::R() * Description: Returns Q matrix of process noise for the body. *-------------------------------------------------------------------------------------- */ +#ifdef FULLSTATE +Matrix<double,13,13> +#else Matrix<double,9,9> +#endif Body::Q (double dt) { +#ifdef FULLSTATE + Matrix<double,13,13> Q; + Q = Matrix<double,13,13>::Zero(); +#else Matrix<double,9,9> Q; Q = Matrix<double,9,9>::Zero(); +#endif Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity(); Q *= IMU_NOISE; +#ifdef FULLSTATE + Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity(); + Q.block<3,3>(10,10) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::Identity(); +#else Q.block<3,3>(6,6) = dt*dt*IMU_RANDOMWALK*Matrix<double,3,3>::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<double,1,13> +#else Matrix<double,1,9> +#endif Body::H ( ) { +#ifdef FULLSTATE + Matrix<double,1,13> H; + H = Matrix<double,1,13>::Zero(); +#else Matrix<double,1,9> H; H = Matrix<double,1,9>::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<double,13,13> +#else Matrix<double,9,9> +#endif Body::F ( const Vector3d &ang, const Quaterniond &q, double dt ) { +#ifdef FULLSTATE + Matrix<double,13,13> F = Matrix<double,13,13>::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<double,3,3> Rbw(qbw.toRotationMatrix()); +#else Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); +#endif Matrix<double,3,3> 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<double,3,3>::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<double,13,13>::Identity(); +#else F *= dt; F += Matrix<double,9,9>::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<double,4,4> +Body::omega ( const Vector3d &x ) +{ + Matrix<double,4,4> 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<double,6,3> A; @@ -195,7 +290,12 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond A = Matrix<double,6,3>::Zero(); b = Matrix<double,6,1>::Zero(); +#ifdef FULLSTATE + Quaterniond qbw(X(9),X(6),X(7),X(8)); + Matrix<double,3,3> Rbw(qbw.toRotationMatrix()); +#else Matrix<double,3,3> Rbw(q.toRotationMatrix()); +#endif A.block<3,3>(0,0) = Rbw; Matrix<double,3,3> 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<double,13,1> +#else Matrix<double,9,1> +#endif Body::asVector ( ) { return X; } /* ----- end of method Body::asVector ----- */ void +#ifdef FULLSTATE +Body::asVector ( const Matrix<double,13,1> &m ) +#else Body::asVector ( const Matrix<double,9,1> &m ) +#endif { X = m; return ; @@ -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<double,13,1> asVector(); + void asVector(const Matrix<double,13,1> &m); + void dx( const Matrix<double,13,1> &del); + Matrix<double,13,13> F(const Vector3d &ang, const Quaterniond &q, double dt); + Matrix<double,1,13> H(); + Matrix<double,13,13> Q(double dt); + Matrix<double,1,1> S(const Matrix<double,13,13> &Pxx); +#else + Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); Matrix<double,9,1> asVector(); + void asVector(const Matrix<double,9,1> &m); + void dx( const Matrix<double,9,1> &del); + Matrix<double,1,9> H(); + Matrix<double,9,9> Q(double dt); + Matrix<double,1,1> S(const Matrix<double,9,9> &Pxx); +#endif Vector3d ned(); Vector3d vel(); /* ==================== MUTATORS ======================================= */ void accelerometer_bias( const Vector3d &b); - void asVector(const Matrix<double,9,1> &m); void clamp(); void pos( const UTM &utm); - void dx( const Matrix<double,9,1> &del); void vel(const Matrix<double,3,1> &v); /* ==================== OPERATORS ======================================= */ - Matrix<double,9,9> F(const Vector3d &ang, const Quaterniond &q, double dt); - Matrix<double,1,9> H(); Matrix<double,1,1> h(); void motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &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> &Pxx); Matrix<double,3,3> skewSymmetric(const Vector3d &x); + Matrix<double,4,4> omega(const Vector3d &x); void unicsv(); protected: @@ -61,9 +73,14 @@ class Body /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ +#ifdef FULLSTATE + Matrix<double,13,1> X; +#else Matrix<double,9,1> X; +#endif char utm_c; int utm_i; + Vector3d gravity_world; }; /* ----- end of class Body ----- */ |