diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/body.cpp b/src/body.cpp index e6afe8a..ff54e89 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -40,7 +40,7 @@ Body::dx ( const Matrix<double,9,1> &del ) void Body::vel ( const Matrix<double,3,1> &v ) { - X.segment(3,3) = v; + X.segment<3>(3) = v; return ; } /* ----- end of method Body::vel ----- */ @@ -127,7 +127,7 @@ Body::H ( ) { Matrix<double,1,9> H; H = Matrix<double,1,9>::Zero(); - H[0,2] = 1; + H(2) = 1.; return H ; } /* ----- end of method Body::H ----- */ @@ -192,9 +192,9 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond Matrix<double,3,3> W; W = skewSymmetric(ang); A.block<3,3>(3,0) = -W; - b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world; + b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; - X.segment(0,6) += (A*X.segment(3,3)+b)*dt; + X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; return ; } /* ----- end of method Body::motionModel ----- */ @@ -226,7 +226,7 @@ Body::unicsv ( ) void Body::accelerometer_bias ( const Vector3d &b ) { - X.segment(6,3) = b; + X.segment<3>(6) = b; return ; } /* ----- end of method State::accelerometer_bias ----- */ |