diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index 942e1ab55..92f3b9b0b 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -38,13 +38,14 @@ Vector PoseRTV::vector() const {
   Vector rtv(9);
   rtv.head(3) = rotation().xyz();
   rtv.segment(3,3) = translation().vector();
-  rtv.tail(3) = velocity().vector();
+  rtv.tail(3) = velocity();
   return rtv;
 }
 
 /* ************************************************************************* */
 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
-  return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
+  return pose().equals(other.pose(), tol)
+      && equal_with_abs_tol(velocity(), other.velocity(), tol);
 }
 
 /* ************************************************************************* */
@@ -52,7 +53,7 @@ void PoseRTV::print(const string& s) const {
   cout << s << ":" << endl;
   gtsam::print((Vector)R().xyz(), "  R:rpy");
   t().print("  T");
-  velocity().print("  V");
+  gtsam::print((Vector)velocity(), "  V");
 }
 
 /* ************************************************************************* */
@@ -137,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
   Vector6 imu;
 
   // acceleration
-  Vector3 accel = (v2-v1).vector() / dt;
+  Vector3 accel = (v2-v1) / dt;
   imu.head<3>() = r2.transpose() * (accel - kGravity);
 
   // rotation rates
@@ -160,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
   // predict point for constraint
   // NOTE: uses simple Euler approach for prediction
-  Point3 pred_t2 = t() + v2 * dt;
+  Point3 pred_t2 = t().retract(v2 * dt);
   return pred_t2;
 }
 
@@ -187,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
 
   // Note that we rotate the velocity
   Matrix3 D_newvel_R, D_newvel_v;
-  Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
+  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
 
   if (Dglobal) {
     Dglobal->setZero();
diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h
index bef397cb6..e89d570b7 100644
--- a/gtsam_unstable/dynamics/PoseRTV.h
+++ b/gtsam_unstable/dynamics/PoseRTV.h
@@ -13,7 +13,7 @@
 namespace gtsam {
 
 /// Syntactic sugar to clarify components
-typedef Point3 Velocity3;
+typedef Vector3 Velocity3;
 
 /**
  * Robot state for use with IMU measurements
@@ -66,7 +66,7 @@ public:
   // and avoidance of Point3
   Vector vector() const;
   Vector translationVec() const { return pose().translation().vector(); }
-  Vector velocityVec() const { return velocity().vector(); }
+  const Velocity3& velocityVec() const { return velocity(); }
 
   // testable
   bool equals(const PoseRTV& other, double tol=1e-6) const;
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index c9db449f9..c41ea220c 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -106,12 +106,13 @@ private:
   static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
       double dt, const dynamics::IntegrationMode& mode) {
 
-    const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();
-    Velocity3 hx;
+    const Velocity3& v1 = x1.v(), v2 = x2.v();
+    const Point3& p1 = x1.t(), p2 = x2.t();
+    Point3 hx;
     switch(mode) {
-    case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break;
-    case dynamics::EULER_START: hx = p1 + v1 * dt; break;
-    case dynamics::EULER_END  : hx = p1 + v2 * dt; break;
+    case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
+    case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
+    case dynamics::EULER_END  : hx = p1.retract(v2 * dt); break;
     default: assert(false); break;
     }
     return (p2 - hx).vector();
diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
index 51d027b3c..3fff06de1 100644
--- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
+++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp
@@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)),
-          pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0));
+          pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
+          pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
+          pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Unit::Create(6);
@@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
   // create a simple chain of poses to generate IMU measurements
   const double dt = 1.0;
   PoseRTV pose1,
-          pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
-          pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
-          pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0));
+          pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
+          pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
+          pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
 
   // create measurements
   SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp
index a8721a60d..ff3508f45 100644
--- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp
+++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp
@@ -198,7 +198,7 @@ TEST( testPoseRTV, transformed_from_1 ) {
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
-  PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
+  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
   EXPECT(assert_equal(expected, actual, tol));
 
   Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
@@ -217,7 +217,7 @@ TEST( testPoseRTV, transformed_from_2 ) {
 
   Matrix actDTrans, actDGlobal;
   PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
-  PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
+  PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
   EXPECT(assert_equal(expected, actual, tol));
 
   Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
index 56d38714a..f253be371 100644
--- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
+++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
@@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
 const double dt = 1.0;
 
 PoseRTV origin,
-        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)),
+        pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
         pose1a(Point3(0.5, 0.0, 0.0)),
-        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0));
+        pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
 
 /* ************************************************************************* */
 TEST( testVelocityConstraint, trapezoidal ) {
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index bbfcaa418..99b33182f 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -53,9 +53,9 @@ class Dummy {
 class PoseRTV {
   PoseRTV();
   PoseRTV(Vector rtv);
-  PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
-  PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel);
-  PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel);
+  PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel);
+  PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel);
+  PoseRTV(const gtsam::Pose3& pose, const Vector& vel);
   PoseRTV(const gtsam::Pose3& pose);
   PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
 
@@ -66,7 +66,7 @@ class PoseRTV {
   // access
   gtsam::Point3 translation() const;
   gtsam::Rot3 rotation() const;
-  gtsam::Point3 velocity() const;
+  Vector velocity() const;
   gtsam::Pose3 pose() const;
 
   // Vector interfaces