diff options
| -rw-r--r-- | src/body.cpp | 31 | ||||
| -rw-r--r-- | src/body.h | 2 |
2 files changed, 17 insertions, 16 deletions
diff --git a/src/body.cpp b/src/body.cpp index 643d91c..f27a62f 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -100,12 +100,12 @@ Body::Q (double dt) { Matrix<double,9,9> Q; Q = Matrix<double,9,9>::Zero(); - Q.block(0,0,3,3) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity(); - Q.block(3,0,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity(); - Q.block(0,3,3,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.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 *= 800e-6; - Q.block(6,6,3,3) = dt*dt*5e-8*Matrix<double,3,3>::Identity(); + Q.block<3,3>(6,6) = dt*dt*5e-8*Matrix<double,3,3>::Identity(); return Q; } /* ----- end of method Body::q ----- */ @@ -154,10 +154,10 @@ Body::F ( const Vector3d &ang, const Quaterniond &q ) Matrix<double,9,9> F = Matrix<double,9,9>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); Matrix<double,3,3> W; - skewSymmetric(ang,W); - F.block(0,3,3,3) = Rbw; - F.block(3,3,3,3) = -W; - F.block(3,6,3,3) = -Matrix<double,3,3>::Identity(); + 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(); return F; } /* ----- end of method Body::F ----- */ @@ -180,10 +180,10 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b = Matrix<double,6,1>::Zero(); Matrix<double,3,3> Rbw(q.toRotationMatrix()); - A.block(0,0,3,3) = Rbw; + A.block<3,3>(0,0) = Rbw; Matrix<double,3,3> W; - skewSymmetric(ang,W); - A.block(3,0,3,3) = -W; + W = skewSymmetric(ang); + A.block<3,3>(3,0) = -W; b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world; X.segment(0,6) += (A*X.segment(3,3)+b)*dt; @@ -197,11 +197,12 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond * Description: Create the skew symmetric matrix y from the vector x. *-------------------------------------------------------------------------------------- */ -void -Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y ) +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.; - return ; + return y; } /* ----- end of method Body::skewSymmetric ----- */ @@ -37,7 +37,7 @@ class Body Matrix<double,9,9> Q(double dt); Matrix<double,1,1> R(); Matrix<double,1,1> S(const Matrix<double,9,9> &P); - void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y); + Matrix<double,3,3> skewSymmetric(const Vector3d &x); void unicsv(); protected: |