EKF is working

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  ----- */