六軸數據(加速規+陀螺儀)實作四元數法轉歐拉角問題!!

vegewell

回復 1# sam183132


   這裡有產品 UM6 Orientation Sensor,可自行秀出 Euler Angles (yaw, pitch, and roll)不用另外程式碼去算,


http://www.chrobotics.com/shop/orientation-sensor-um6
===========================================
>>>歐拉角數值（Roll,Pitch,Yaw）都會亂飄,


If you want to get Euler angles from your quaternion (y』know, heading, pitch & roll) then you』ll need to do something like this
Vector Quaternion::toEuler()
{
    Vector ret;
    double sqw = _w*_w;
    double sqx = _x*_x;
    double sqy = _y*_y;
    double sqz = _z*_z;


    ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
    ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
    ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));


    return ret;
}
This function will pack a vector with heading, pitch and roll information. x = heading, y = pitch, z = roll.
==============================================================================
可以從某些library,裡找到數學運算程式,如 FreeIMU library,