Body can return velocity

2 files changed, 8 insertions, 1 deletions

diff --git a/src/body.cpp b/src/body.cpp
index caa7cb9..a5c572a 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -234,6 +234,12 @@ Body::accelerometer_bias ( const Vector3d &b )
 Vector3d
 Body::ned (  )
 {
-    return X.segment(0,3) ;
+    return X.segment<3>(0);
 }		/* -----  end of method Body::ned  ----- */
 
+Vector3d
+Body::vel (  )
+{
+    return X.segment<3>(3);
+}		/* -----  end of method Body::vel  ----- */
+
diff --git a/src/body.h b/src/body.h
index f99c9e8..6c1268e 100644
--- a/src/body.h
+++ b/src/body.h
@@ -22,6 +22,7 @@ class Body
 
         /* ====================  ACCESSORS     ======================================= */
         Vector3d ned();
+        Vector3d vel();
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);