summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-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 ----- */