summaryrefslogtreecommitdiff
path: root/src/body.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/body.cpp')
-rw-r--r--src/body.cpp50
1 files changed, 42 insertions, 8 deletions
diff --git a/src/body.cpp b/src/body.cpp
index f909a39..0bbe18f 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -42,6 +42,9 @@ Body::dx ( const Matrix<double,STATESIZE,1> &del )
{
X += del;
clamp();
+#if STATESIZE==13
+ normalizeQuaternion();
+#endif
return ;
} /* ----- end of method Body::dx ----- */
@@ -120,7 +123,7 @@ Body::Q (double dt)
Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
Q *= IMU_NOISE;
#if STATESIZE==13
- Q.block<4,4>(6,6) = dt*dt*IMU_NOISE*Matrix<double,4,4>::Identity();
+ Q.block<4,4>(6,6) = 0.25*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();
@@ -195,6 +198,7 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
v1 = X(3);
v2 = X(4);
v3 = X(5);
+ double gw = gravity_world[2];
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,
@@ -207,10 +211,10 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
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.block<3,4>(3,6) << -2*gw*qbw3, 2*gw*qbw4, -2*gw*qbw1, 2*gw*qbw2,
+ -2*gw*qbw4, -2*gw*qbw3, -2*gw*qbw2, -2*gw*qbw1,
+ 2*gw*qbw1, 2*gw*qbw2, -2*gw*qbw3, -2*gw*qbw4;
+ F.block<4,4>(6,6) = 0.5*omega(ang);
F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
#else
F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
@@ -234,7 +238,7 @@ 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.block<1,3>(3,0) = -x.transpose();
Omega(3,3) = 0;
return Omega;
} /* ----- end of method Body::omega ----- */
@@ -269,10 +273,15 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
W = skewSymmetric(ang);
A.block<3,3>(3,0) = -W;
b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world;
+ Matrix<double,6,1> dX;
+ dX = (A*X.segment<3>(3)+b)*dt;
X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt;
#if STATESIZE==13
- X.segment<4>(6) += 0.5*omega(ang)*X.segment<4>(6);
+ Matrix<double,4,1> dq;
+ dq = dt*0.5*omega(ang)*X.segment<4>(6);
+ X.segment<4>(6) += dq;
+ normalizeQuaternion();
#endif
clamp();
return ;
@@ -289,17 +298,27 @@ Matrix<double,3,3>
Body::skewSymmetric ( const Vector3d &x )
{
Matrix<double,3,3> y;
- y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
+ y << 0.,-x[2], x[1],
+ x[2],0.,-x[0],
+ -x[1],x[0],0.;
return y;
} /* ----- end of method Body::skewSymmetric ----- */
void
Body::unicsv ( )
{
+#if STATESIZE==13
+ printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
+ X[0], X[1], -X[2],
+ X[3], X[4], X[5],
+ X[6], X[7], X[8],X[9],
+ X[10],X[11],X[12]);
+#else
printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
X[0], X[1], -X[2],
X[3], X[4], X[5],
X[6], X[7], X[8]);
+#endif
return ;
} /* ----- end of method Body::unicsv ----- */
@@ -384,6 +403,21 @@ Body::qhat ( const Quaterniond &q )
return ;
} /* ----- end of method Body::qhat ----- */
+void
+Body::normalizeQuaternion ( )
+{
+ Quaterniond q = qhat();
+ Matrix<double,4,1> qv;
+ qv[0] = q.x();
+ qv[1] = q.y();
+ qv[2] = q.z();
+ qv[3] = q.w();
+ qv /= qv.norm();
+ X.segment<4>(6) = qv;
+ //qhat(q.normalized());
+ return ;
+} /* ----- end of method Body::normalizequaternion ----- */
+
#endif