summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp141
-rw-r--r--src/body.h38
-rw-r--r--src/state.cpp7
3 files changed, 81 insertions, 105 deletions
diff --git a/src/body.cpp b/src/body.cpp
index f7440b5..819bd01 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -38,11 +38,7 @@ Body::clamp()
*--------------------------------------------------------------------------------------
*/
void
-#ifdef FULLSTATE
-Body::dx ( const Matrix<double,13,1> &del )
-#else
-Body::dx ( const Matrix<double,9,1> &del )
-#endif
+Body::dx ( const Matrix<double,STATESIZE,1> &del )
{
X += del;
clamp();
@@ -65,11 +61,7 @@ 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
+Body::S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx )
{
Matrix<double,1,1> S;
S << Pxx(2,2);
@@ -117,26 +109,17 @@ Body::R()
* Description: Returns Q matrix of process noise for the body.
*--------------------------------------------------------------------------------------
*/
-#ifdef FULLSTATE
-Matrix<double,13,13>
-#else
-Matrix<double,9,9>
-#endif
+Matrix<double,STATESIZE,STATESIZE>
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
+ Matrix<double,STATESIZE,STATESIZE> Q;
+ Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
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
+#if STATESIZE==13
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
@@ -152,20 +135,11 @@ 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
+Matrix<double,1,STATESIZE>
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
+ Matrix<double,1,STATESIZE> H;
+ H = Matrix<double,1,STATESIZE>::Zero();
H(2) = 1.;
return H ;
} /* ----- end of method Body::H ----- */
@@ -193,32 +167,29 @@ Body::h ( )
* Description: Returns the Jacobian of the motion model of the body.
*--------------------------------------------------------------------------------------
*/
-#ifdef FULLSTATE
-Matrix<double,13,13>
+Matrix<double,STATESIZE,STATESIZE>
+#if STATESIZE==13
+Body::F ( const Vector3d &ang, double dt )
#else
-Matrix<double,9,9>
-#endif
Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
+#endif
{
-#ifdef FULLSTATE
- Matrix<double,13,13> F = Matrix<double,13,13>::Zero();
- double qbw1,qbw2,qbw3,qbw4;
- Quaterniond qbw = qhat();
- qbw1 = qbw.x();
- qbw2 = qbw.y();
- qbw3 = qbw.z();
- qbw4 = qbw.w();
- 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());
+ Matrix<double,STATESIZE,STATESIZE> F = Matrix<double,STATESIZE,STATESIZE>::Zero();
+#if STATESIZE==13
+ Quaterniond q = qhat();
#endif
+ Matrix<double,3,3> Rbw(q.toRotationMatrix());
+
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
+#if STATESIZE==13
+ double qbw1,qbw2,qbw3,qbw4;
+ qbw1 = q.x();
+ qbw2 = q.y();
+ qbw3 = q.z();
+ qbw4 = q.w();
/* Jacobian generated using symbolic matlab */
double v1,v2,v3;
v1 = X(3);
@@ -240,12 +211,12 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
-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();
+ F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
#else
- F *= dt;
- F += Matrix<double,9,9>::Identity();
+ F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
#endif
+ F *= dt;
+ F += Matrix<double,STATESIZE,STATESIZE>::Identity();
return F;
} /* ----- end of method Body::F ----- */
@@ -276,26 +247,23 @@ Body::omega ( const Vector3d &x )
*--------------------------------------------------------------------------------------
*/
void
-Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
-{
-#ifdef FULLSTATE
- Vector3d bias(X.segment<3>(10));
+#if STATESIZE==13
+Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt)
#else
- Vector3d bias(X.segment<3>(6));
+Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt)
#endif
- Vector3d gravity_world(0.,0.,9.80655);
+{
+ Vector3d bias = accelerometer_bias();
Matrix<double,6,3> A;
Matrix<double,6,1> b;
A = Matrix<double,6,3>::Zero();
b = Matrix<double,6,1>::Zero();
-#ifdef FULLSTATE
- Quaterniond qbw qhat();
- Matrix<double,3,3> Rbw(qbw.toRotationMatrix());
-#else
- Matrix<double,3,3> Rbw(q.toRotationMatrix());
+#if STATESIZE==13
+ Quaterniond q = qhat();
#endif
+ Matrix<double,3,3> Rbw(q.toRotationMatrix());
A.block<3,3>(0,0) = Rbw;
Matrix<double,3,3> W;
W = skewSymmetric(ang);
@@ -303,7 +271,7 @@ 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
+#if STATESIZE==13
X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
#endif
clamp();
@@ -335,13 +303,29 @@ Body::unicsv ( )
return ;
} /* ----- end of method Body::unicsv ----- */
- void
+void
Body::accelerometer_bias ( const Vector3d &b )
{
+#if STATESIZE==13
+ X.segment<3>(10) = b;
+#else
X.segment<3>(6) = b;
+#endif
return ;
} /* ----- end of method State::accelerometer_bias ----- */
+Vector3d
+Body::accelerometer_bias ( )
+{
+ Vector3d bs;
+#if STATESIZE==13
+ bs = X.segment<3>(10);
+#else
+ bs = X.segment<3>(6);
+#endif
+ return bs ;
+} /* ----- end of method State::accelerometer_bias ----- */
+
/*
*--------------------------------------------------------------------------------------
* Class: Body
@@ -361,22 +345,14 @@ Body::vel ( )
return X.segment<3>(3);
} /* ----- end of method Body::vel ----- */
-#ifdef FULLSTATE
-Matrix<double,13,1>
-#else
-Matrix<double,9,1>
-#endif
+Matrix<double,STATESIZE,1>
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
+Body::asVector ( const Matrix<double,STATESIZE,1> &m )
{
X = m;
return ;
@@ -393,7 +369,12 @@ Body::asVector ( const Matrix<double,9,1> &m )
Quaterniond
Body::qhat ( )
{
+#if STATESIZE==13
Quaterniond qbw(X(9),X(6),X(7),X(8));
+#else
+ fprintf(stderr, "Quaternion is not being estimated, quitting.\n");
+ exit(1);
+#endif
return qbw;
} /* ----- end of method Body::qhat ----- */
diff --git a/src/body.h b/src/body.h
index 5544bba..97df1e9 100644
--- a/src/body.h
+++ b/src/body.h
@@ -9,7 +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 */
+#define STATESIZE 13
using Eigen::Matrix;
@@ -29,27 +29,21 @@ class Body
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);
+#if STATESIZE==13
+ Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, double dt);
Quaterniond qhat();
#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);
+ Matrix<double,STATESIZE,STATESIZE> F(const Vector3d &ang, const Quaterniond &q, double dt);
#endif
+ Matrix<double,STATESIZE,1> asVector();
+ void asVector(const Matrix<double,STATESIZE,1> &m);
+ void dx( const Matrix<double,STATESIZE,1> &del);
+ Matrix<double,1,STATESIZE> H();
+ Matrix<double,STATESIZE,STATESIZE> Q(double dt);
+ Matrix<double,1,1> S(const Matrix<double,STATESIZE,STATESIZE> &Pxx);
Vector3d ned();
Vector3d vel();
-
+ Vector3d accelerometer_bias( );
/* ==================== MUTATORS ======================================= */
void accelerometer_bias( const Vector3d &b);
void clamp();
@@ -58,8 +52,12 @@ class Body
/* ==================== OPERATORS ======================================= */
Matrix<double,1,1> h();
+#if STATESIZE==13
+ void motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt);
+#else
void motionModel ( const Vector3d &acc, const Vector3d &ang,
const Quaterniond &q, const double dt);
+#endif
Matrix<double,1,1> R();
Matrix<double,3,3> skewSymmetric(const Vector3d &x);
Matrix<double,4,4> omega(const Vector3d &x);
@@ -74,11 +72,7 @@ class Body
/* ==================== METHODS ======================================= */
/* ==================== DATA MEMBERS ======================================= */
-#ifdef FULLSTATE
- Matrix<double,13,1> X;
-#else
- Matrix<double,9,1> X;
-#endif
+ Matrix<double,STATESIZE,1> X;
char utm_c;
int utm_i;
Vector3d gravity_world;
diff --git a/src/state.cpp b/src/state.cpp
index 5253425..dfe41fa 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -553,9 +553,6 @@ State::F ( const Vector3d &w, double dt )
State::F ( const Quaterniond &q, const Vector3d &w, double dt )
#endif
{
-#if STATESIZE==13
- Quaterniond q = body->qhat();
-#endif
Vector3d v;
v = body->vel();
// Allocate matrix F
@@ -564,7 +561,11 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt )
f = MatrixXd::Zero(rows,rows);
// Set body F
+#if STATESIZE==13
+ f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,dt);
+#else
f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,q,dt);
+#endif
// Set Fxi Fyi
{ // limit i's scope
auto i = features.begin();