◆加速度センサーで拾った重力加速度を使って姿勢のrollとpitchを計算ことができます。移動体の前進方向をX軸とし、前進方向の左方向をY軸とし、真上の方向をZ軸とすれば、X軸Y軸Z軸を回る回転角はroll,pitch,yawになります。本メモは右手系座標系を使っています。黒い色の座標系は地面座標系、青い色の座標系は移動体の座標系。加速度センサーで拾った数値(ax,ay,az)は移動体座標系の各軸方向の加速度成分です。移動体は水平の場合は、重力加速度gがZ軸だけに存在し(az <> 0, ax=0, az=0)、その方向がマイナスです。下図のように、傾いている場合は、重力加速度gが各軸方向から拾われています。

絵を見ている通り、roll、pitchと各軸方向の成分(ax,ay,az)との関係が下記になります。
roll = atan(ay / az)
pitch = -atan(ax / az); // -atanのマイナスは右手系のY軸のオイラー角の正方向に合わせるからです。

◆磁気センサーで拾った地磁気を使って姿勢のyawを算出することができます。X軸を真っ北（地磁気の北）に向ける時に、yaw=0とします。磁気センサーで拾った数値(mx,my,mz)は移動体座標系の各軸方向の磁気強度成分です。下図のように、移動体が真っ北に向いている場合は、地磁気はZ軸とX軸の成分があり、Y軸の成分がないです(ma<>0, my=0, mz<>0)。一方、真っ北に向いていない場合は、磁気強度は各軸方向に分けています(ma<>0, my<>0, mz<>0)。

一方、移動体の傾きでrollとpitchを生じて、各軸の磁気強度(ma,my,mz)を地面座標系に変換する必要があります。まずオイラー角と回転行列間の変換方法を利用して回転行列を作ります。回転行列の中のオイラー角zをゼロに設定し、オイラー角ｙにpitchを代入し、オイラー角xにrollを代入すれば、下図のように、回転行列を作り、回転演算を計算することができます。

行列の掛け算の結果として、地面座標系上の磁気強度(mx',my')は下記の通りです。
mx' = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
my' = my * cosRoll - mz * sinRoll;
座標系の絵を見ている通り、yawと(mx',my')との関係は下記になります。
yaw = -atan(my' / mx'); // -atanのマイナスは右手系のZ軸のオイラー角の正方向に合わせるからです。

◆加速度センサー(ax,ay,az)と磁気センサー(ma,my,mz)から姿勢算出するためのC++ソースは下記に記述します。オイラー角をクォータニオンの変換処理がありますが、詳細については、「オイラー角とクォータニオン間の変換とベクトルの回転」の「１．オイラー角からクォータニオン（四元数）へ変換」を参照してください。

double q0, q1, q2, q3;
double roll, pitch, yaw;

void Sensor2Attitude(double ax, double ay, double az, double mx, double my, double mz)
{
    double cosRoll, sinRoll, cosPitch, sinPitch, cosYaw, sinYaw;
    double mx1, my1;

    roll = atan2(ay, az);
    pitch = -atan2(ax, az);

    cosRoll = cos(roll);
    sinRoll = sin(roll);
    cosPitch = cos(pitch);
    sinPitch = sin(pitch);

    mx1 = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
    my1 = my * cosRoll - mz * sinRoll;
    yaw = -atan2(my1, mx1);

    cosRoll = cos(roll * 0.5);
    sinRoll = sin(roll * 0.5);
    cosPitch = cos(pitch * 0.5);
    sinPitch = sin(pitch * 0.5);
    cosYaw = cos(yaw * 0.5);
    sinYaw = sin(yaw * 0.5);
    q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
    q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}