summaryrefslogtreecommitdiff
path: root/src/body.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/body.cpp')
-rw-r--r--src/body.cpp141
1 files changed, 61 insertions, 80 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 ----- */