由加速度计解算得到姿态角

先说结论，再解释原理
假设世界坐标系下重力向量为$g=( \begin{matrix} 0 & 0 & 1 \end{matrix} )^T$
机体坐标系下，将加速度计测量得到的数据归一化后得到$a = ( \begin{matrix} ax & ay & az \end{matrix} )^T$
横滚角、俯仰角、偏航角可简单理解为加速度计绕X、Y、Z轴旋转的角度
若使用mpu6050，则将芯片上的X轴对准前方，Y轴向左，Z轴向上(mpu6050芯片上显示的坐标轴为陀螺仪的坐标轴，mpu6050中加速度计和陀螺仪坐标轴正方向相反)即将加速度计X轴对准后方，Y轴向右，Z轴向下(这样表示的好处是如果加速度方向为前，则加速度计所测得的加速度为正)
使用右手定则，大拇指分别指向陀螺仪X、Y、Z轴正方向，四指所指方向角度为正
俯仰角$\theta=asin(-ax)$
横滚角$\phi=atan(ay/az)$
在只有加速度计的情况下，无法测得偏航角$\psi$

以下为原理
假设加速度计绑定在机体上，并且加速度X、Y、Z轴与机体X、Y、Z轴重合，在机体不剧烈运动的情况下，可认为加速度计测出的加速度表示重力加速度，可根据这一特性，解算得出姿态。

在初始未旋转的状态，即机体坐标系与世界坐标系重合时，将加速度计测得的数据归一化后得到$a=g=( \begin{matrix} 0 & 0 & 1 \end{matrix} )^T$
假设经过旋转$R_n$，得到加速度计归一化后的数据$a = ( \begin{matrix} ax & ay & az \end{matrix} )^T$
则$a =R_n*g$
其中
旋转矩阵$R=$

$$\left[ \begin{matrix} cos\theta cos\psi & cos\theta sin\psi & -sin\theta\\ cos\psi sin\theta sin\phi - sin\psi cos\phi & sin\psi sin\theta sin\phi+cos\psi cos\phi & sin\phi cos\theta\\ cos\psi sin\theta cos\phi + sin\psi sin\phi & sin\psi sin\theta cos\phi-cos\psi sin\phi & cos\phi cos\theta \end{matrix} \right]$$
$R_n=R(-\theta, -\psi, -\phi)$
其中$\theta$ 为俯仰角，$\psi$为偏航角，$\phi$为横滚角
由此，得到$ax=sin\theta$，$ay=-sin\phi cos\theta$，$az=cos\phi cos\theta$
因此得到
$\theta=asin(ax)$
$\phi=atan(-ay/az)$
使用mpu6050加速度计和陀螺仪数据时，分别用这两种数据解算得到的姿态角是相反的，一般将加速度计的旋转矩阵取反，即以芯片标明的XYZ轴方向为正方向
因此本文开头部分计算姿态角公式时对旋转矩阵取反
$\theta=asin(-ax)$
$\phi=atan(ay/az)$

向量的旋转与坐标系的旋转

向量旋转
旋转矩阵$R$表示向量$n$按照$Z-Y-X$的顺序，绕这三个轴分别旋转$\psi、\phi、\theta$
得到向量旋转后的坐标为$n'=R*n$
注意上面的旋转矩阵表示的是向量的旋转，而非坐标系的旋转
坐标系旋转
如果向量$n$不旋转，而向量n所在坐标系按照$Z-Y-X$的顺序旋转
则可以得到坐标系旋转后n向量的坐标为$n''=R_n*n=R^T*n$

至于为什么$R_n=R(-\theta, -\psi, -\phi)$，可类比二维向量的旋转
二维旋转矩阵

$$r(\theta)=\left[ \begin{matrix} cos\theta & -sin\theta \\ sin\theta &cos\theta \end{matrix} \right]$$
向量$b=( \begin{matrix} x & y \end{matrix} )^T$绕$Z$轴（沿纸面向外）逆时针旋转$\theta$ 可表示为
$b'=r(\theta)*b$
若向量$b$不旋转，而$Y-X$坐标系绕$Z$轴逆时针旋转$\theta$后，$b$的坐标变为
$b''=r(-\theta)*b$
三维的旋转矩阵与二维的旋转矩阵有相似的规律