diff --git a/include/sot-dynamic/angle-estimator.h b/include/sot-dynamic/angle-estimator.h
index 24d20c71acc3bad33431793fb7a6abe3d5a7d4f0..4c41a430967cebc254dd4f13cecf29f913b0efee 100644
--- a/include/sot-dynamic/angle-estimator.h
+++ b/include/sot-dynamic/angle-estimator.h
@@ -86,6 +86,11 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
   dg::SignalTimeDependent<MatrixHomogeneous,int> waistWorldPositionSOUT; // worldMwaist
   dg::SignalTimeDependent<ml::Vector,int> waistWorldPoseRPYSOUT; // worldMwaist
 
+  dg::SignalPtr<ml::Matrix,int> jacobianSIN;
+  dg::SignalPtr<ml::Vector,int> qdotSIN;
+  dg::SignalTimeDependent<ml::Vector,int> xff_dotSOUT;
+  dg::SignalTimeDependent<ml::Vector,int> qdotSOUT;
+
  public: /* --- FUNCTIONS --- */
   ml::Vector& computeAngles( ml::Vector& res,
 			     const int& time );
@@ -101,6 +106,10 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
 						   const int& time );
   ml::Vector& computeWaistWorldPoseRPY( ml::Vector& res,
 					const int& time );
+  ml::Vector& compute_xff_dotSOUT( ml::Vector& res,
+				   const int& time );
+  ml::Vector& compute_qdotSOUT( ml::Vector& res,
+				const int& time );
 
  public: /* --- PARAMS --- */
   void fromSensor(const bool& fs) { fromSensor_ = fs; }
@@ -111,7 +120,6 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
  private:
   bool fromSensor_;
 
-
 };
 
 
diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
index dfc74ec2f92af858c3be2b1791b1db59a3468dd6..b597064e198ede5f805758db39d0f2858cbcac08 100644
--- a/src/angle-estimator.cpp
+++ b/src/angle-estimator.cpp
@@ -76,6 +76,17 @@ AngleEstimator( const std::string & name )
 			    "sotAngleEstimator("+name
 			    +")::output(vectorRollPitchYaw)::waistWorldPoseRPY" )
 
+  ,jacobianSIN(NULL,"sotAngleEstimator("+name
+			     +")::input()::jacobian")
+  ,qdotSIN(NULL,"sotAngleEstimator("+name
+			     +")::input()::qdot")
+  ,xff_dotSOUT( boost::bind(&AngleEstimator::compute_xff_dotSOUT,this,_1,_2)
+		,jacobianSIN<<qdotSIN
+		,"sotAngleEstimator("+name+")::output(vector)::xff_dot" )
+  ,qdotSOUT( boost::bind(&AngleEstimator::compute_qdotSOUT,this,_1,_2)
+	     ,xff_dotSOUT<<qdotSIN
+	     ,"sotAngleEstimator("+name+")::output(vector)::qdotOUT" )
+
    ,fromSensor_(true)
 {
   sotDEBUGIN(5);
@@ -85,7 +96,8 @@ AngleEstimator( const std::string & name )
 		      << anglesSOUT              << flexibilitySOUT
 		      << driftSOUT               << sensorWorldRotationSOUT
 		      << waistWorldRotationSOUT
-		      << waistWorldPositionSOUT  << waistWorldPoseRPYSOUT  );
+		      << waistWorldPositionSOUT  << waistWorldPoseRPYSOUT
+		      << jacobianSIN << qdotSIN << xff_dotSOUT << qdotSOUT );
 
   /* Commands. */
   {
@@ -100,7 +112,8 @@ AngleEstimator( const std::string & name )
 	       new ::dynamicgraph::command::Setter<AngleEstimator, bool>
 	       (*this, &AngleEstimator::fromSensor, docstring));
 
-    "    Get flag specifying whether angle is measured from sensors or simulated.\n"
+    docstring = "    \n"
+      "    Get flag specifying whether angle is measured from sensors or simulated.\n"
       "    \n"
       "      No input,\n"
       "      return a boolean value.\n"
@@ -323,6 +336,49 @@ computeWaistWorldPoseRPY( ml::Vector& res,
   return res;
 }
 
+/* --- VELOCITY SIGS -------------------------------------------------------- */
+
+ml::Vector& AngleEstimator::
+compute_xff_dotSOUT( ml::Vector& res,
+		     const int& time )
+{
+  const ml::Matrix & J = jacobianSIN( time );
+  const ml::Vector & dq = qdotSIN( time );
+
+  const int nr=J.nbRows(), nc=J.nbCols()-6;
+  assert( nr==6 );
+  ml::Matrix Ja( nr,nc ); ml::Vector dqa(nc);
+  for( int j=0;j<nc;++j )
+    {
+      for( int i=0;i<nr;++i )
+	Ja(i,j) = J(i,j+6);
+      dqa(j) = dq(j+6);
+    }
+  ml::Matrix Jff( 6,6 );
+  for( int j=0;j<6;++j )
+    for( int i=0;i<6;++i )
+      Jff(i,j) = J(i,j);
+
+  res.resize(nr); res = -Jff.inverse()*Ja*dqa;
+  return res;
+}
+
+ml::Vector& AngleEstimator::
+compute_qdotSOUT( ml::Vector& res,
+		  const int& time )
+{
+  const ml::Vector & dq = qdotSIN( time );
+  const ml::Vector & dx = xff_dotSOUT( time );
+
+  assert( dx.size()==6 );
+
+  const int nr=dq.size();
+  res.resize( nr ); res=dq;
+  for( int i=0;i<6;++i ) res(i)=dx(i);
+
+  return res;
+}
+
 /* --- PARAMS --------------------------------------------------------------- */
 /* --- PARAMS --------------------------------------------------------------- */
 /* --- PARAMS --------------------------------------------------------------- */