diff --git a/geo_controller/src/controllerImpl.cpp b/geo_controller/src/controllerImpl.cpp
index 7837a50..66c480d 100644
--- a/geo_controller/src/controllerImpl.cpp
+++ b/geo_controller/src/controllerImpl.cpp
@@ -26,8 +26,6 @@ control_out_t ControllerImpl::get_control(state_space_t ss, const desired_state_
 
     Vector3d ex = ss.position - desired_s.x;
     Vector3d ev = ss.velocity - derivatives.d_xd;
-    Vector3d ea = derivatives.dv - derivatives.d2_xd;
-    Vector3d ej = derivatives.d2v - derivatives.d3_xd;
     Vector3d e3(0,0,1);
     Vector3d A = -gains.kx * ex - gains.kv * ev - params.mass * params.gravity * e3 +
                     params.mass * derivatives.d2_xd;
@@ -37,27 +35,27 @@ control_out_t ControllerImpl::get_control(state_space_t ss, const desired_state_
 
     Vector3d b3c = -A/nA;
     Vector3d C = b3c.cross(desired_s.b1);
-    Vector3d b1c = -(1/C.norm())*b3c.cross(C);
     Vector3d b2c = C/C.norm();
+    Vector3d b1c = b2c.cross(b3c);
     Matrix3d Rc;
     Rc << b1c, b2c, b3c;
+    Matrix3d R_dot = ss.R * su::hat(ss.omega);
 
     double nC = C.norm();
-
+    Vector3d ea = params.gravity * e3 + A.dot(ss.R * e3) * ss.R * e3/params.mass - derivatives.d2_xd;
     Vector3d A_1dot = -gains.kx*ev - gains.kv*ea + params.mass*derivatives.d3_xd;
     Vector3d b3c_1dot = -A_1dot / nA + (A.dot(A_1dot)/pow(nA,3))*A;
 
     Vector3d C_1dot = b3c_1dot.cross(desired_s.b1) + b3c.cross(derivatives.db1);
-    Vector3d b2c_1dot = C/nC - C.dot(C_1dot)/(pow(nC,3))*C;
+    Vector3d b2c_1dot = C_1dot/nC - C.dot(C_1dot)/(pow(nC,3))*C;
     Vector3d b1c_1dot = b2c_1dot.cross(b3c) + b2c.cross(b3c_1dot);
-
+    Vector3d ej = A_1dot.dot(ss.R*e3) * ss.R * e3/params.mass + A.dot(R_dot*e3)*ss.R*e3/params.mass + A.dot(ss.R*e3)*R_dot*e3/params.mass-derivatives.d3_xd;
     Vector3d A_2dot = -gains.kx*ea -gains.kv*ej + params.mass*derivatives.d4_xd;
-    Vector3d b3c_2dot = -A_2dot/nA + (2.0/pow(nA,3))*A.dot(A_1dot)*A_1dot +
-            (pow(A_1dot.norm(),2) + A.dot(A_2dot)/(pow(nA,3)))*A - (3.0/pow(nA,5))*(pow(A.dot(A_1dot),2))*A;
+    Vector3d b3c_2dot = -A_2dot/nA + (2.0/pow(nA,3))*A.dot(A_1dot)*A_1dot + (pow(A_1dot.norm(),2) + A.dot(A_2dot)/(pow(nA,3)))*A - (3.0/pow(nA,5))*(pow(A.dot(A_1dot),2))*A;
     
     Vector3d C_2dot   = b3c_2dot.cross(desired_s.b1) + (b3c.cross(derivatives.d2b1)) + 2*b3c_1dot.cross(derivatives.db1);
 
-    Vector3d b2c_2dot = C_2dot/nC - 2.0/(pow(nC,3))*C.dot(C_1dot)*C_1dot - (((pow(C_2dot.norm(),2) + C.dot(C_2dot)))/pow(nC,3))*C
+    Vector3d b2c_2dot = C_2dot/nC - 2.0/(pow(nC,3))*C.dot(C_1dot)*C_1dot - (((pow(C_1dot.norm(),2) + C.dot(C_2dot)))/pow(nC,3))*C
                     + (3.0/pow(nC,5))*(pow(C.dot(C_1dot),2))*C;
     
     Vector3d b1c_2dot = b2c_2dot.cross(b3c) + b2c.cross(b3c_2dot) + 2.0*b2c_1dot.cross(b3c_1dot);
@@ -69,7 +67,7 @@ control_out_t ControllerImpl::get_control(state_space_t ss, const desired_state_
     Vector3d omegac_1dot = su::vee(Rc.transpose()*Rc_2dot -
                                            su::hat(omegac)*su::hat(omegac));
 
-    Vector3d er = 2.0*su::vee(Rc.transpose()*ss.R - ss.R.transpose()*Rc);
+    Vector3d er = su::vee(Rc.transpose()*ss.R - ss.R.transpose()*Rc)/2;
     Vector3d eomega = ss.omega - ss.R.transpose()*Rc*omegac;
 
     Vector3d M = -gains.kr*er - gains.komega*eomega + ss.omega.cross(params.J*ss.omega)