1.计算roll和pitch 当多旋翼无人机在地理坐标系中静止时， 加速度计的量测输出为：
a
n
=
[
0
0
g
]
T
a_n = [0 \quad 0 \quad g]^T
an=[00g]T 其中g 为重力加速度。
当多旋翼无人机处于任意姿态时，加速度计在机体坐标系中的测量输出为：
a
b
=
[
a
x
a
y
a
z
]
T
a_b=[a_x \quad a_y \quad a_z]^T
ab=[axayaz]T
我们知道地理坐标系的速度可以通过旋转矩阵得到机体坐标系下的数据：
[
a
x
a
y
a
z
]
=
C
b
n
[
0
0
g
]
=
[
c
o
s
ψ
c
o
s
θ
s
i
n
ψ
c
o
s
θ
−
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
γ
c
o
s
θ
s
i
n
γ
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
γ
]
[
0
0
g
]
=
[
−
g
s
i
n
θ
g
c
o
s
θ
s
i
n
γ
g
c
o
s
θ
c
o
s
γ
]
\begin{bmatrix}a_x \\ a_y \\ a_z\end{bmatrix} = \textbf{C}_b^n \begin{bmatrix}0 \\ 0 \\ g\end{bmatrix} \\ =\begin{bmatrix} cosψcosθ & sinψcosθ & −sinθ \\ cosψsinθsinγ−sinψcosγ & sinψsinθsinγ+cosψcosγ & cosθsinγ \\ cosψsinθcosγ+sinψsinγ & sinψsinθcosγ−cosψsinγ & cosθcosγ \end{bmatrix} \begin{bmatrix}0 \\ 0 \\ g\end{bmatrix} \\ =\begin{bmatrix}−gsinθ \\ gcosθsinγ \\ gcosθcosγ\end{bmatrix}
⎣⎡axayaz⎦⎤=Cbn⎣⎡00g⎦⎤=⎣⎡cosψcosθcosψsinθsinγ−sinψcosγcosψsinθcosγ+sinψsinγsinψcosθsinψsinθsinγ+cosψcosγsinψsinθcosγ−cosψsinγ−sinθcosθsinγcosθcosγ⎦⎤⎣⎡00g⎦⎤=⎣⎡−gsinθgcosθsinγgcosθcosγ⎦⎤
所以：
θ
=
a
r
c
s
i
n
(
−
a
x
g
)
γ
=
a
r
c
t
a
n
(
a
y
a
z
)
θ=arcsin(-\frac{a_x}{g}) \\ γ=arctan(\frac{a_y}{a_z})
θ=arcsin(−gax)γ=arctan(azay) 2.计算yaw 多旋翼无人机在地理坐标系（ 导航坐标系） 下磁感应强度表示为：
m
n
=
[
m
x
n
m
y
n
m
z
n
]
T
\textbf{m}_n=[m^n_x \quad m^n_y \quad m^n_z]^T
mn=[mxnmynmzn]T 地磁场是一个矢量，对于一个固定的地点来说，这个矢量可以被分解为两个与当地水平面（即地理坐标系的xy平面）平行的分量和一个与当地水平面（即地理坐标系的xy平面）垂直的分量。
对水平方向的两个分量来说，他们的矢量和总是指向磁北的，所以我们只需要水平的磁力分量即可算出传感器x轴与磁北的夹角：
ψ
m
=
a
r
c
t
a
n
(
m
y
n
m
x
n
)
ψ_m=arctan(\frac{m_y^n}{ m^n_x})
ψm=arctan(mxnmyn) 如果我们认为磁北是0度，那我们就可以得到当前的偏航角。
所以令旋转矩阵，yaw为0，即yaw不旋转。根据旋转顺序ZYX可得：
C
b
n
′
=
[
c
o
s
θ
s
i
n
θ
s
i
n
γ
s
i
n
θ
c
o
s
γ
0
c
o
s
γ
−
s
i
n
γ
−
s
i
n
θ
c
o
s
θ
s
i
n
γ
c
o
s
θ
c
o
s
γ
]
\textbf{C}^{n'}_{b}=\begin{bmatrix}cosθ & sinθsinγ & sinθcosγ \\ 0 & cosγ & −sinγ \\ −sinθ & cosθsinγ & cosθcosγ\end{bmatrix}
Cbn′=⎣⎡cosθ0−sinθsinθsinγcosγcosθsinγsinθcosγ−sinγcosθcosγ⎦⎤ 磁力计的输出是在机体坐标系下的磁感应强度，表示为:
m
b
=
[
m
x
b
m
y
b
m
z
b
]
T
\textbf{m}_b=[m^b_x \quad m^b_y \quad m^b_z]^T
mb=[mxbmybmzb]T 可知
m
n
m_n
mn与
m
b
m_b
mb关系式为：
m
n
=
C
b
n
′
m
b
\textbf{m}_n=\textbf{C}^{n′}_b \textbf{m}_b
mn=Cbn′mb 带入已知条件可以得:
[
m
x
n
m
y
n
m
z
n
]
=
C
b
n
′
[
m
x
b
m
y
b
m
z
b
]
=
[
c
o
s
θ
s
i
n
θ
s
i
n
γ
s
i
n
θ
c
o
s
γ
0
c
o
s
γ
−
s
i
n
γ
−
s
i
n
θ
c
o
s
θ
s
i
n
γ
c
o
s
θ
c
o
s
γ
]
[
m
x
b
m
y
b
m
z
b
]
=
[
m
x
b
∗
c
o
s
θ
+
m
y
b
∗
s
i
n
θ
∗
s
i
n
γ
+
m
z
b
∗
s
i
n
θ
∗
c
o
s
γ
m
y
b
∗
c
o
s
γ
−
m
z
b
∗
s
i
n
γ
−
m
x
b
∗
s
i
n
θ
−
m
y
b
∗
s
i
n
γ
∗
c
o
s
θ
+
m
z
b
∗
c
o
s
θ
∗
c
o
s
γ
]
=
[
m
x
b
∗
c
o
s
(
p
i
t
c
h
)
+
m
y
b
∗
s
i
n
(
p
i
t
c
h
)
∗
s
i
n
(
r
o
l
l
)
+
m
z
b
∗
s
i
n
(
p
i
t
c
h
)
∗
c
o
s
(
r
o
l
l
)
m
y
b
∗
c
o
s
(
r
o
l
l
)
−
m
z
b
∗
s
i
n
(
r
o
l
l
)
m
x
b
∗
s
i
n
(
p
i
t
c
h
)
−
m
y
b
∗
s
i
n
(
r
o
l
l
)
∗
c
o
s
(
p
i
t
c
h
)
+
m
z
b
∗
c
o
s
(
p
i
t
c
h
)
∗
c
o
s
(
r
o
l
l
)
]
\begin{bmatrix}m^n_x \\ m^n_y \\ m^n_z\end{bmatrix}=\textbf{C}^{n′}_b \begin{bmatrix}m^b_x \\ m^b_y \\ m^b_z\end{bmatrix}\\ =\begin{bmatrix}cosθ & sinθsinγ & sinθcosγ \\ 0 & cosγ & −sinγ \\−sinθ & cosθsinγ & cosθcosγ\end{bmatrix} \begin{bmatrix} m^b_x \\ m^b_y \\ m^b_z \end{bmatrix} \\ =\begin{bmatrix}m^b_x∗cosθ+m^b_y∗sinθ∗sinγ+m^b_z∗sinθ∗cosγ \\ m^b_y∗cosγ−m^b_z∗sinγ \\ −m^b_x∗sinθ−m^b_y∗sinγ∗cosθ+m^b_z∗cosθ∗cosγ \end{bmatrix} \\ =\begin{bmatrix}m^b_x∗cos(pitch)+m^b_y∗sin(pitch)∗sin(roll)+m^b_z∗sin(pitch)∗cos(roll) \\ m^b_y∗cos(roll)−m^b_z∗sin(roll) \\ m^b_x∗sin(pitch)−m^b_y∗sin(roll)∗cos(pitch)+m^b_z∗cos(pitch)∗cos(roll) \end{bmatrix}
⎣⎡mxnmynmzn⎦⎤=Cbn′⎣⎡mxbmybmzb⎦⎤=⎣⎡cosθ0−sinθsinθsinγcosγcosθsinγsinθcosγ−sinγcosθcosγ⎦⎤⎣⎡mxbmybmzb⎦⎤=⎣⎡mxb∗cosθ+myb∗sinθ∗sinγ+mzb∗sinθ∗cosγmyb∗cosγ−mzb∗sinγ−mxb∗sinθ−myb∗sinγ∗cosθ+mzb∗cosθ∗cosγ⎦⎤=⎣⎡mxb∗cos(pitch)+myb∗sin(pitch)∗sin(roll)+mzb∗sin(pitch)∗cos(roll)myb∗cos(roll)−mzb∗sin(roll)mxb∗sin(pitch)−myb∗sin(roll)∗cos(pitch)+mzb∗cos(pitch)∗cos(roll)⎦⎤
注意z轴的正负（原始数据是z轴朝上为正）
m
x
n
=
m
x
b
c
o
s
θ
+
m
y
b
s
i
n
θ
s
i
n
γ
+
m
z
b
s
i
n
θ
c
o
s
γ
m^n_x=m^b_xcosθ+m^b_ysinθsinγ+m^b_zsinθcosγ
mxn=mxbcosθ+mybsinθsinγ+mzbsinθcosγ
m
y
n
=
m
y
b
c
o
s
γ
−
m
z
b
s
i
n
γ
m^n_y=m^b_ycosγ−m^b_zsinγ
myn=mybcosγ−mzbsinγ
用同样的方法求得x轴与磁北的夹角：
ψ
m
=
a
r
c
t
a
n
(
m
y
n
m
x
n
)
ψ_m=arctan(\frac{m^n_y}{m^n_x})
ψm=arctan(mxnmyn) 磁力计所解算的航向角是机体坐标x轴相对于磁北而言的，而真北与磁北之间存在一个磁偏角
Δ
ψ
Δψ
Δψ。
所以机体坐标纵轴相对于真北的航向角为：
ψ
=
ψ
m
+
Δ
ψ
ψ=ψ_m+Δψ
ψ=ψm+Δψ
