diff --git a/src/common_obj.h b/src/common_obj.h
index 65843b3098e886d0db899a349f432a7c48209d83..722021ac5c4fa2c267b659d65f339d8b476659b7 100644
--- a/src/common_obj.h
+++ b/src/common_obj.h
@@ -86,6 +86,7 @@ class CHT{
     Eigen::Matrix4d cam_in_baselink;     // Camera in base_link frames.
     Eigen::Matrix4d tip_in_baselink;     // Arm forward kinematics (from base_link to arm link frame).
     Eigen::Matrix4d armbase_in_baselink; // Camera in base_link frames.
+    Eigen::Matrix4d link1_in_armbase;    // Arm link 1 in arm base frame.
     Eigen::Matrix4d baselink_in_w;       // base_link in World frames.
     Eigen::Matrix4d cam_in_w;            // Camera in World frames.
 
diff --git a/src/tasks/cog.cpp b/src/tasks/cog.cpp
index 03f8bd3995f0a24b7b358afa5a4db77e0432b216..45864de4ed8a776eaaed56802cc25d9773a03065 100644
--- a/src/tasks/cog.cpp
+++ b/src/tasks/cog.cpp
@@ -119,9 +119,7 @@ void CTaskCOG::CoGCommon(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& co
   // std::cout << "cog: " << arm_cog_data.arm_cog << std::endl;
 
   // Rotation between quadrotor body and inertial frames  
-  // TODO: If attitude is estimated, then it could be roll and pitch
-  // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
-  Rib = Eigen::MatrixXd::Identity(3,3);
+  Rib = HT.baselink_in_w.block(0,0,3,3)*HT.armbase_in_baselink.block(0,0,3,3);
 
   // X and Y values of CoG in quadrotor inertial frame
   cog_arm = arm_cog_data.arm_cog.block(0,0,3,1);
@@ -153,18 +151,25 @@ void CTaskCOG::CoGCommon(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& co
 void CTaskCOG::TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
 {
   Eigen::Matrix3d Rib;
-  Eigen::MatrixXd Jj_cog(3,arm.nj);  
+  Eigen::MatrixXd Jj_cog(3,arm.nj);
   CoGCommon(arm, HT, cog_PGxy, cog_arm, Rib, Jj_cog, arm_cog_data);
+  // Task Error
+  sigmaG = -cog_PGxy.array();
 
+  // Task Jacobian
+  JG = Eigen::MatrixXd::Zero(2,4+arm.nj);
+  Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3);
+  Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+  Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3); 
+  Eigen::Matrix3d skew_rot = UAM::CCommon_Fc::Skew(Rb_in_w*Rl1_in_armb*arm_cog_data.arm_cog.block(0,0,3,1));
+  JG.block(0,3,2,1) = skew_rot.block(0,2,2,1);
   Eigen::MatrixXd Jacob_temp(3,arm.nj);
   for (unsigned int ii = 0; ii < arm.nj; ++ii)
-    Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
+    Jacob_temp.col(ii) = Rb_in_w * Rarmb_in_b * Rl1_in_armb * Jj_cog.col(ii);
 
-  JG = Eigen::MatrixXd::Zero(2,4+arm.nj);
-  JG.block(0,4,2,arm.nj) = Jacob_temp.block(0,0,2,arm.nj);
-  JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
+  JG.block(0,4,2,arm.nj)=Jacob_temp.block(0,0,2,arm.nj);
 
-  sigmaG = -cog_PGxy;
+  JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
 }
 
 void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data)
@@ -172,18 +177,23 @@ void CTaskCOG::TaskErrorJac(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd&
   Eigen::Matrix3d Rib;
   Eigen::MatrixXd Jj_cog(3,arm.nj);
   CoGCommon(arm, HT, cog_PGxy, cog_arm, Rib, Jj_cog, arm_cog_data);
+  // Task Error
+  sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
 
+  // Task Jacobian
   JG = Eigen::MatrixXd::Zero(1,4+arm.nj);
-  JG.block(0,0,1,4) = Eigen::MatrixXd::Zero(1,4);
+  Eigen::Matrix3d Rb_in_w = HT.baselink_in_w.block(0,0,3,3);
+  Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+  Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3); 
   Eigen::MatrixXd Jacob_temp(3,arm.nj);
   for (unsigned int ii = 0; ii < arm.nj; ++ii)
-    Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
+    Jacob_temp.col(ii) = Rb_in_w * Rarmb_in_b * Rl1_in_armb * Jj_cog.col(ii);
 
   JG.block(0,4,1,arm.nj)=2*cog_PGxy.transpose()*Jacob_temp.block(0,0,2,arm.nj);
+  
+  JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);  
+}
 
-  JG_pseudo = UAM::CCommon_Fc::CalcPinv(JG);
 
-  sigmaG = -(cog_PGxy.transpose()*cog_PGxy);
-}
 
 } // End of UAM namespace
\ No newline at end of file
diff --git a/src/tasks/cog.h b/src/tasks/cog.h
index 22dcf15a530af638dd53b3d1b29a79fb7ccdbc37..d3c5afe1601b74dece50fd5c0af70bdf0389315f 100644
--- a/src/tasks/cog.h
+++ b/src/tasks/cog.h
@@ -56,7 +56,6 @@ class CTaskCOG
     *
     */  
     static void TaskErrorJacFull(UAM::CArm& arm, const UAM::CHT& HT, Eigen::MatrixXd& cog_PGxy, Eigen::MatrixXd& cog_arm, Eigen::MatrixXd& sigmaG, Eigen::MatrixXd& JG, Eigen::MatrixXd& JG_pseudo, UAM::ArmCogData& arm_cog_data);
-
 };
 
 } // End of UAM namespace
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 9433c9e78087845f3073b7b2a74001ad643f3d79..29c9d90f609ae0670857a381c3780b97e2654d29 100644
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -149,9 +149,10 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   Eigen::MatrixXd ArmJac = UAM::CKinematics::ArmJac(arm);
 
   Eigen::MatrixXd RRb_in_w = Eigen::MatrixXd::Zero(6,6);
-  Eigen::Matrix3d Rarmb_in_b = Eigen::Vector3d(-1, 1,-1).asDiagonal();
-  RRb_in_w.block(0,0,3,3) = Rb_in_w*Rarmb_in_b;
-  RRb_in_w.block(3,3,3,3) = Rb_in_w*Rarmb_in_b;
+  Eigen::Matrix3d Rarmb_in_b = HT.armbase_in_baselink.block(0,0,3,3);
+  Eigen::Matrix3d Rl1_in_armb = HT.link1_in_armbase.block(0,0,3,3);
+  RRb_in_w.block(0,0,3,3) = Rb_in_w * Rarmb_in_b * Rl1_in_armb;
+  RRb_in_w.block(3,3,3,3) = RRb_in_w.block(0,0,3,3);
   JEEPOS_omw.block(0,6,6,arm.nj) = RRb_in_w * ArmJac;
 
   Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);