多源融合——Mahony互补滤波详述
目录
引言
传统的姿态传感器有陀螺仪、磁力计和加速度计，由于陀螺仪是一种相对测量，存在误差累积。磁力计可以绝对测量航向，加速度计则可以绝对测量俯仰角和横滚角。因此，将三种传感器结合可以提供准确的载体姿态，三者往往采用互补滤波方式进行融合。互补滤波作用面非常广，可以用于无人机、智能手机等的定姿，但是目前相关博文介绍多从无人机视角出发，对于传统测绘学生来说具有一定的障碍。因此，本文主要结合智能手机端matlab相关代码介绍博主对于Mahony滤波的理解。
基础知识
在表示由载体坐标系（body frame，b系）到导航坐标系（navigation frame，n系）时常见的物理量有欧拉角、方向余弦矩阵和四元数，这里我们主要介绍四元数方法。
四元数q是由一个实部和三个虚部共四个元素组成的四维数组，表示为（为了简洁，q均采用q代替，具有数字的则为对应元素）
q
=
[
q
1
q
2
q
3
q
4
]
=
=
q
0
+
q
1
i
+
q
2
j
+
q
3
j
+
q
4
k
=
q
0
+
q
v
q=\left[ \begin{matrix} q_1\\ q_2\\ q_3\\ q_4\\ \end{matrix} \right]==q_0+q_1i+q_2j+q_3j+q_4k=q_0+q_v
q=
q1q2q3q4
==q0+q1i+q2j+q3j+q4k=q0+qv
其中，
q
1
q_1
q1为实部，
q
2
到
q
4
q_2到q_4
q2到q4为虚部（
q
v
q_v
qv），并且定义
i
⊗
i
=
−
1
，
j
⊗
j
=
−
1
，
k
⊗
k
=
−
1
i
⊗
j
=
k
，
j
⊗
i
=
−
k
，
k
⊗
j
=
−
i
i
⊗
k
=
−
j
，
j
⊗
k
=
i
，
k
⊗
i
=
j
1
⊗
i
=
i
⊗
1
=
i
，
1
⊗
j
=
j
⊗
1
=
j
，
1
⊗
k
=
k
⊗
1
=
k
i \otimes i=-1，j \otimes j=-1，k \otimes k=-1\\ i \otimes j=k，j \otimes i=-k，k \otimes j=-i\\ i \otimes k=-j，j \otimes k=i，k \otimes i=j\\ 1 \otimes i=i \otimes 1=i，1 \otimes j=j \otimes 1=j，1 \otimes k=k \otimes 1=k\\
i⊗i=−1，j⊗j=−1，k⊗k=−1i⊗j=k，j⊗i=−k，k⊗j=−ii⊗k=−j，j⊗k=i，k⊗i=j1⊗i=i⊗1=i，1⊗j=j⊗1=j，1⊗k=k⊗1=k
并且以矩阵形式定义四元数乘
p
⊗
q
p \otimes q
p⊗q
p
⊗
q
=
[
p
1
−
p
2
−
p
3
−
p
4
p
2
p
1
−
p
4
p
3
p
3
p
4
p
1
−
p
2
p
4
−
p
3
p
2
p
1
]
[
q
1
q
2
q
3
q
4
]
p \otimes q=\left[ \begin{matrix} p_1 & -p_2 & -p_3 & -p_4 \\ p_2 & p_1 & -p_4 & p_3 \\ p_3 & p_4 & p_1 & -p_2 \\ p_4 & -p_3 & p_2 & p_1 \\ \end{matrix} \right] \left[ \begin{matrix} q_1\\ q_2\\ q_3\\ q_4\\ \end{matrix} \right]
p⊗q=
p1p2p3p4−p2p1p4−p3−p3−p4p1p2−p4p3−p2p1
q1q2q3q4
或
p
⊗
q
=
[
q
1
−
q
2
−
q
3
−
q
4
q
2
q
1
q
4
−
q
3
q
3
−
q
4
q
1
q
2
q
4
q
3
−
q
2
q
1
]
[
p
1
p
2
p
3
p
4
]
p \otimes q=\left[ \begin{matrix} q_1 & -q_2 & -q_3 & -q_4 \\ q_2 & q_1 & q_4 & -q_3 \\ q_3 & -q_4 & q_1 & q_2 \\ q_4 & q_3 & -q_2 & q_1 \\ \end{matrix} \right] \left[ \begin{matrix} p_1\\ p_2\\ p_3\\ p_4\\ \end{matrix} \right]
p⊗q=
q1q2q3q4−q2q1−q4q3−q3q4q1−q2−q4−q3q2q1
p1p2p3p4
此外，对于任意单位四元数都有
q
=
q
0
+
q
v
=
cos
(
θ
2
)
+
u
sin
(
θ
2
)
q=q_0+q_v=\cos(\frac{\theta}{2})+u\sin(\frac{\theta}{2})
q=q0+qv=cos(2θ)+usin(2θ)
对于任意三维向量v，算子
L
q
(
v
)
=
q
⊗
v
⊗
q
∗
L_q(v)=q \otimes v\otimes q^*
Lq(v)=q⊗v⊗q∗
的作用等效于使向量v绕向量u的正方向旋转角度
θ
\theta
θ，
q
∗
q^*
q∗为共轭四元数，即实部不变，虚部取负号。
最后，我们定义b系右前上，n系东北天，
q
b
n
q_b^{n}
qbn则代表由b系到n系的四元数，为方便起见，用q代替
q
b
n
q_b^{n}
qbn。
原理与代码实现
我们以http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms网站发布的Mahony代码为例进行原理介绍，下面介绍Mahony代码的流程。
（1）利用GetSensorData读取手机AHRS姿态，转为四元数。
（2）预测当前时刻四元数。
q
˙
=
1
2
q
^
k
−
1
W
k
\dot q=\frac{1}{2}\hat q_{k-1}W_k
q˙=21q^k−1Wk
q
k
−
=
q
^
k
−
1
+
q
˙
∗
δ
t
q_k^-=\hat q_{k-1}+\dot q*\delta t
qk−=q^k−1+q˙∗δt
其中，
q
k
−
q_{k}^-
qk−和
q
^
k
−
1
\hat q_{k-1}
q^k−1代表历元k和k-1时的先验状态和后验状态，
W
k
W_k
Wk则为陀螺仪观测值。
（3）标准化加速度计和磁力计数据。
该步的目的：（1）为了降低噪声的影响，因为都涉及到寻找方向的比例关系，可以近似忽略较小数值的影响；（忘了是在哪篇论文里看过了，欢迎补充）（2）为步骤（5）统一量纲，并得到近似误差。
（4）描述重力加速度和磁场方向。
需要注意的是：（a）楼主在北半球，磁场方向由南指向北，则在n系下对应 b(3) 和 b(4) 有数值，东方向近似为0，并且 b(4) 小于0。（b）加速度计获取的是包含重力加速度的加速度数值，则静止状态下只存在天向的+9.81m/s^2，归一化后为[0, 0, 1]。（c）在第（5）步中要与归一化后的加速度计和磁力计数据进行对比，所以此时的v和w为近似的磁场观测值b和加速度计读数在b系下的观测值（即将观测值b等从载体系转到导航系）。
（5）计算外积。
向量外积公式
a
×
v
=
∣
a
∣
∣
v
∣
sin
θ
a \times v=\lvert a\rvert \lvert v\rvert \sin \theta
a×v=∣a∣∣v∣sinθ，当a和v均为单位向量时，外积结果近似为姿态误差，姿态误差是两种传感器数据外积之和。
（6）补偿，其中Kp和Ki是调节参数，调节这两个参数对最终定位精度影响较大，类似于PDI控制器（比例-积分控制器），对于这方面了解太少，具体含义不太清楚，能用就好。
（7）计算最终四元数结果
q
^
k
\hat q_{k}
q^k
至此，Mahony算法流程介绍完毕，上述步骤中（4）-（6）就是互补滤波实现三种传感器融合的关键，将地球有关向量，如磁场向量、重力向量在磁力计和加速度计中的理论观测值转到b系下，与实际磁力计和加速度计观测值求差再进行补偿，补充了陀螺仪相对测量的不足。
参考文献
1、源码：http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
2、武汉大学牛小骥老师惯导课程：https://space.bilibili.com/521626836?spm_id_from=333.337.search-card.all.click
9793
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
