先说结论，再解释原理
假设世界坐标系下重力向量为g=(001)T
g
=
(
0
0
1
)
T
机体坐标系下，将加速度计测量得到的数据归一化后得到a=(axayaz)T
a
=
(
a
x
a
y
a
z
)
T
横滚角、俯仰角、偏航角可简单理解为加速度计绕X、Y、Z轴旋转的角度
若使用mpu6050，则将芯片上的X轴对准前方，Y轴向左，Z轴向上(mpu6050芯片上显示的坐标轴为陀螺仪的坐标轴，mpu6050中加速度计和陀螺仪坐标轴正方向相反)即将加速度计X轴对准后方，Y轴向右，Z轴向下(这样表示的好处是如果加速度方向为前，则加速度计所测得的加速度为正)
使用右手定则，大拇指分别指向陀螺仪X、Y、Z轴正方向，四指所指方向角度为正
俯仰角θ=asin(−ax)
θ
=
a
s
i
n
(
−
a
x
)
横滚角ϕ=atan(ay/az)
ϕ
=
a
t
a
n
(
a
y
/
a
z
)
在只有加速度计的情况下，无法测得偏航角ψ
ψ
以下为原理
假设加速度计绑定在机体上，并且加速度X、Y、Z轴与机体X、Y、Z轴重合，在机体不剧烈运动的情况下，可认为加速度计测出的加速度表示重力加速度，可根据这一特性，解算得出姿态。
在初始未旋转的状态，即机体坐标系与世界坐标系重合时，将加速度计测得的数据归一化后得到a=g=(001)T
a
=
g
=
(
0
0
1
)
T
假设经过旋转Rn
R
n
，得到加速度计归一化后的数据a=(axayaz)T
a
=
(
a
x
a
y
a
z
)
T
则a=Rn∗g
a
=
R
n
∗
g
其中
旋转矩阵R=
R
=
⎡⎣⎢⎢cosθcosψcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ+sinψsinϕcosθsinψsinψsinθsinϕ+cosψcosϕsinψsinθcosϕ−cosψsinϕ−sinθsinϕcosθcosϕcosθ⎤⎦⎥⎥ [ c o s θ c o s ψ c o s θ s i n ψ − s i n θ c o s ψ s i n θ s i n ϕ − s i n ψ c o s ϕ s i n ψ s i n θ s i n ϕ + c o s ψ c o s ϕ s i n ϕ c o s θ c o s ψ s i n θ c o s ϕ + s i n ψ s i n ϕ s i n ψ s i n θ c o s ϕ − c o s ψ s i n ϕ c o s ϕ c o s θ ]
Rn=R(−θ,−ψ,−ϕ)
R
n
=
R
(
−
θ
,
−
ψ
,
−
ϕ
)
其中
θ
θ
为俯仰角，
ψ
ψ
为偏航角，
ϕ
ϕ
为横滚角
由此，得到
ax=sinθ
a
x
=
s
i
n
θ
，
ay=−sinϕcosθ
a
y
=
−
s
i
n
ϕ
c
o
s
θ
，
az=cosϕcosθ
a
z
=
c
o
s
ϕ
c
o
s
θ
因此得到
θ=asin(ax)
θ
=
a
s
i
n
(
a
x
)
ϕ=atan(−ay/az)
ϕ
=
a
t
a
n
(
−
a
y
/
a
z
)
使用mpu6050加速度计和陀螺仪数据时，分别用这两种数据解算得到的姿态角是相反的，一般将加速度计的旋转矩阵取反，即以芯片标明的XYZ轴方向为正方向
因此本文开头部分计算姿态角公式时对旋转矩阵取反
θ=asin(−ax)
θ
=
a
s
i
n
(
−
a
x
)
ϕ=atan(ay/az)
ϕ
=
a
t
a
n
(
a
y
/
a
z
)
向量的旋转与坐标系的旋转
向量旋转
旋转矩阵R
R
表示向量nn按照Z−Y−X
Z
−
Y
−
X
的顺序，绕这三个轴分别旋转ψ、ϕ、θ
ψ
、
ϕ
、
θ
得到向量旋转后的坐标为n′=R∗n
n
′
=
R
∗
n
注意上面的旋转矩阵表示的是向量的旋转，而非坐标系的旋转
坐标系旋转
如果向量n
n
不旋转，而向量n所在坐标系按照Z−Y−XZ−Y−X的顺序旋转
则可以得到坐标系旋转后n向量的坐标为n″=Rn∗n=RT∗n
n
″
=
R
n
∗
n
=
R
T
∗
n
至于为什么Rn=R(−θ,−ψ,−ϕ)
R
n
=
R
(
−
θ
,
−
ψ
,
−
ϕ
)
，可类比二维向量的旋转
二维旋转矩阵
r(θ)=[cosθsinθ−sinθcosθ] r ( θ ) = [ c o s θ − s i n θ s i n θ c o s θ ]
向量
b=(xy)T
b
=
(
x
y
)
T
绕
Z
Z
轴（沿纸面向外）逆时针旋转θθ 可表示为
b′=r(θ)∗b
b
′
=
r
(
θ
)
∗
b
若向量
b
b
不旋转，而Y−XY−X坐标系绕
Z
Z
轴逆时针旋转θθ后，
b
b
的坐标变为
b″=r(−θ)∗bb″=r(−θ)∗b
三维的旋转矩阵与二维的旋转矩阵有相似的规律
