diff --git a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
index 3ba5771..719958a 100644
--- a/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
+++ b/gimbal_ctrl/driver/src/GX40/GX40_gimbal_funtion.cpp
@@ -20,9 +20,9 @@
 uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
 {
     carrierStateMutex.lock();
-    targetPos[0] = (int16_t)(-pos.roll / 0.01f);
+    targetPos[0] = (int16_t)(pos.roll / 0.01f);
     targetPos[1] = (int16_t)(-pos.pitch / 0.01f);
-    targetPos[2] = (int16_t)(-pos.yaw / 0.01f);
+    targetPos[2] = (int16_t)(pos.yaw / 0.01f);
     carrierStateMutex.unlock();
     return pack(GX40::GIMBAL_CMD_MODE_EULER, nullptr, 0);
 }
@@ -38,9 +38,9 @@ uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
 uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
 {
     carrierStateMutex.lock();
-    targetPos[0] = (int16_t)(-speed.roll / 0.1f);
+    targetPos[0] = (int16_t)(speed.roll / 0.1f);
     targetPos[1] = (int16_t)(-speed.pitch / 0.1f);
-    targetPos[2] = (int16_t)(-speed.yaw / 0.1f);
+    targetPos[2] = (int16_t)(speed.yaw / 0.1f);
     carrierStateMutex.unlock();
 
     return pack(GX40::GIMBAL_CMD_MODE_FOLLOW, nullptr, 0);
diff --git a/gimbal_ctrl/sv_gimbal.cpp b/gimbal_ctrl/sv_gimbal.cpp
index 11f0123..4e97e86 100644
--- a/gimbal_ctrl/sv_gimbal.cpp
+++ b/gimbal_ctrl/sv_gimbal.cpp
@@ -509,6 +509,50 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
     temp.yaw = yaw_rate;
     pdevTemp->setGimabalSpeed(temp);
 }
+void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
+                                    const GimbalVelocityT &speed,
+                                    const GimbalVelocityT &acc, void *extenData)
+{
+    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
+    AMOV_GIMBAL_QUATERNION_T temp_q;
+    temp_q.q0 = quaterion.q0;
+    temp_q.q1 = quaterion.q1;
+    temp_q.q2 = quaterion.q2;
+    temp_q.q3 = quaterion.q3;
+
+    AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
+    temp_v1.x = speed.x;
+    temp_v1.y = speed.y;
+    temp_v1.z = speed.z;
+
+    temp_v2.x = acc.x;
+    temp_v2.y = acc.y;
+    temp_v2.z = acc.z;
+
+    pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, extenData);
+}
+
+void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
+                                    const GimbalVelocityT &speed,
+                                    const GimbalVelocityT &acc, void *extenData)
+{
+    amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
+    AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
+    temp_v1.x = speed.x;
+    temp_v1.y = speed.y;
+    temp_v1.z = speed.z;
+
+    temp_v2.x = acc.x;
+    temp_v2.y = acc.y;
+    temp_v2.z = acc.z;
+
+    AMOV_GIMBAL_POS_T temp_p;
+    temp_p.pitch = pos.pitch;
+    temp_p.yaw = pos.yaw;
+    temp_p.roll = pos.roll;
+
+    pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, extenData);
+}
 
 sv::Gimbal::~Gimbal()
 {
diff --git a/include/sv_gimbal.h b/include/sv_gimbal.h
index 0df5dcd..a3ab82e 100644
--- a/include/sv_gimbal.h
+++ b/include/sv_gimbal.h
@@ -78,6 +78,28 @@ namespace sv
     FLOWCONTROL_HARDWARE = 2,
   };
 
+  typedef struct
+  {
+    double q0;
+    double q1;
+    double q2;
+    double q3;
+  } GimbalQuaternionT;
+
+  typedef struct
+  {
+    double x; // or N
+    double y; // or E
+    double z; // or UP
+  } GimbalVelocityT;
+
+  typedef struct
+  {
+    double yaw;
+    double roll;
+    double pitch;
+  } GimbalPosT;
+
   static inline void emptyCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
                                    double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
                                    double &fovX, double &fovY)
@@ -165,6 +187,12 @@ namespace sv
     bool takePhoto();
     bool takeVideo(int state);
     int getVideoState();
+    void attitudeCorrection(const GimbalQuaternionT &quaterion,
+                                    const GimbalVelocityT &speed,
+                                    const GimbalVelocityT &acc, void *extenData);
+    void attitudeCorrection(const GimbalPosT &pos,
+                                    const GimbalVelocityT &speed,
+                                    const GimbalVelocityT &acc, void *extenData);
 
     //! Set gimbal angles
     /*!