diff options
Diffstat (limited to 'src/body.cpp')
-rw-r--r-- | src/body.cpp | 50 |
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 |