summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorMartin Miller2017-04-03 15:10:47 -0500
committerMartin Miller2017-04-03 15:10:47 -0500
commit27e98b8b0aedca320e45b5184b3e426e5913e1a3 (patch)
tree9799878b32f28d206c5a0cd92f4ea31ebe335aec /src
parent05a04acf46a0a5624e019e4dc947cd3cd587a80b (diff)
downloadrefslam-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.cpp113
-rw-r--r--src/body.h31
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 ;
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<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 ----- */