1. 概述

首先明确几个概念和名词:

Sensor Fusion - 传感器融合
IMU (Inertial Measurement Unit) - 惯性测量单元
AHRS (Attitude and Heading Reference System) - 航姿参考系统

研究了一下现有的Sensor Fusion技术，参考了几篇文章

Sensor Fusion Algorithms
IMU Data Fusing: Complementary, Kalman, and Mahony Filter
Open source IMU and AHRS algorithms

发现了几个主流算法：

Kalman filter – EKF（extended kalman filter）类似加权平均
Mahony filter – 互补滤波算法（explicit complement filter）
Madgwick filter – 梯度下降算法（gradient descent）

2. 准备知识

2.1 四元数

几个算法中都用到了四元数，这里做简单介绍，在这篇文章中有较详细的描述，另外一篇文章的介绍也有一定参考价值。

当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量。
简单来说，就是在进行坐标系转换（或物体旋转）时，可以用一个四元数来表示旋转的全部信息，一般计做Q=q0+q1 i +q2 j+q3 k。
通过Q，可以通过公式直接计算出pitch，roll，yaw的值，或是旋转矩阵。

这篇文章中对四元数的性质和基本计算进行了总结：

其实欧拉角，四元数，旋转矩阵，都是描述同一动作的不同方法而已，他们之间完全可以通过公式转换。

在使用四元数进行坐标转换时，有如下公式（转自《惯性导航》）：

一个旋转矩阵也可以转换为一个四元数，即给你一个旋转矩阵可以求出（s，x，y，z）这个四元数，方法是：

3. 算法详情

下面一一分析

1. Kalman filter

卡尔曼滤波器，源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载。
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。在知乎上有个帖子介绍的比较通俗，链接在此

个人理解就是如何把2个不确定的观测，最优融合的方法，其中比较牛的是，把两个正态分布拟合成了一个，并基于此进行进一步迭代。其中比较精髓的地方之一是用独立高斯分布的乘积将测量值和估计值进行融合。 用公式计算一下就会发现，融合后的 u u 和 σ\sigma 跟原来2个高斯分布的关系。再进一步发现K（卡尔曼增益）就是估计量方差占总方差的比重。细节可以参考Kalman滤波器从原理到实现

这里尝试用kalman的方法描述加速度传感器的情况：
系统状态模型 xt=Axt−1+Bxt+wt x_t=Ax_{t-1}+Bx_t+w_t ( wt w_t是高斯白噪声)
位移： st=st−1+(vt−1+vt)Δt/2 s_t = s_{t-1}+(v_{t-1}+v_t)\Delta t/2
速度： vt=at−1+(at−1+at)Δt/2 v_t = a_{t-1} +(a_{t-1}+a_t)\Delta t/2

加速度： Zat=Hat+vt Z_{a_t} = Ha_t+v_t （测量而来 vt v_t是高斯白噪声）
这里因为加速度传感器是直接测量a，所以H=I (单位矩阵，如果 at a_t是单个值，H就是1)

这里还没有想清楚怎么用，要融合的测量值和系统估算值需要是一个量纲。

EKF（Extended Kalman Filter）和UKF（Unscented Kalman Filter)

真正用在姿态估算时，EKF用得比较多，因为传统的KF要求系统是线性的，而EKF最主要是用泰勒展开用线性系统逼近非线性系统，其中逼近的程度看展开的阶数，级数越高应该越准确，相应的计算量也越大。而展开后，非线性的问题就变成了线性的，也就可以用KF计算了，可以参考卡尔曼滤波器学习笔记（二）

这篇文章里提到了一个改进的Kalman Filter(Unscented Kalman Filter)

在知乎上看到了一个关于使用kalman滤波的回答，感觉比较有道理，贴在这里：
”如果是四旋翼，那么你的系统实际上假定了几个前提，而这些前提也是你能使用加速度计来测量姿态的原因。
1，你的飞行器在n系下三轴的加速度不大，加减速时间短。
2，你的飞行器大部分时间是保持水平，或说姿态机动幅度小时间短。
在这两个前提下，你可以用加速度计来测量n系下的重力矢量Gn[0 0 9.8]’在b系下的输出Gb，而且他们有关系Gb=Cn_b*Gn，Cn_b代表n系到b系的旋转矩阵。对磁罗盘而言也是同理。如果你使用卡尔曼滤波来融合陀螺仪，加计，磁罗盘，实际上你是将陀螺仪的输出做为系统的增量预测，而姿态的增量方程即为卡尔曼的状态方程（其实就是四元数微分方程），之后再观测重力矢量和磁矢量做为测量方程。这样就得到了融合。卡尔曼的优势在于，对于线性系统中的高斯噪声而言，卡尔曼滤波器是最优而且无偏的。但要达到最优无偏，你必须保证系统线性，而且只有高斯噪声，没有其它有色噪声，意味着你的系统模型必须尽可能准确（与线性冲突），把系统中的各个有色噪声都在状态方程中体现出来。同时，你还要尽可能准确地估计状态与测量噪声的能量（方差）。如果以上条件不满足，卡尔曼并非是最好的选择。这也是目前绝大多数航模级遥控四旋翼不需要使用卡尔曼的原因——它们的传感器各种有色噪声太多且难以估计，而互补滤波足够简单且性能优良。“

2. Mahony filter

可以参考这篇文章

3. Madgwick filter

Madgwick 的算法使用了梯度下降的优化思路，总的资料在这里, 论文原文在此，matlab代码在此。

其中的主要思路是，利用mahoney算法中提到的重力分量（来自加速度传感器）的方向，来修正陀螺仪的四元数，其中还考虑到了加入地磁传感器的信息，思路是一样的。

这里直接分析代码：

public void Update(float gx, float gy, float gz, float ax, float ay, float az)
{float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readabilityfloat norm;float s1, s2, s3, s4;float qDot1, qDot2, qDot3, qDot4;// Auxiliary variables to avoid repeated arithmeticfloat _2q1 = 2f * q1;float _2q2 = 2f * q2;float _2q3 = 2f * q3;float _2q4 = 2f * q4;float _4q1 = 4f * q1;float _4q2 = 4f * q2;float _4q3 = 4f * q3;float _8q2 = 8f * q2;float _8q3 = 8f * q3;float q1q1 = q1 * q1;float q2q2 = q2 * q2;float q3q3 = q3 * q3;float q4q4 = q4 * q4;// Normalise accelerometer measurementnorm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);if (norm == 0f) return; // handle NaNnorm = 1 / norm;        // use reciprocal for divisionax *= norm;ay *= norm;az *= norm;// Gradient decent algorithm corrective steps1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitudes1 *= norm;s2 *= norm;s3 *= norm;s4 *= norm;// Compute rate of change of quaternionqDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;// Integrate to yield quaternionq1 += qDot1 * SamplePeriod;q2 += qDot2 * SamplePeriod;q3 += qDot3 * SamplePeriod;q4 += qDot4 * SamplePeriod;norm = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternionQuaternion[0] = q1 * norm;Quaternion[1] = q2 * norm;Quaternion[2] = q3 * norm;Quaternion[3] = q4 * norm;
}

上面函数是用来更新Quaternion中的四元数的，初值是[1,0,0,0], beta =1
每组新的传感器数值传进来，都要更新一次四元数，也就得到了当前的姿态。

其中引入了一些中间变量，来减少重复计算，并对加速度传感器的数值进行了初始化，由于这里主要考虑的是姿态，初始化只是改变了大小，并没有改变方向，因而对计算没有负面影响，反而能简化运算。

这里比较关键的是s的计算，这里直接用了最终的推导结果，不太容易看懂，看了matlab 的代码就会清晰很多，如下：

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)q = obj.Quaternion; % short name local variable for readability% Normalise accelerometer measurementif(norm(Accelerometer) == 0), return; end   % handle NaNAccelerometer = Accelerometer / norm(Accelerometer);    % normalise magnitude% Gradient decent algorithm corrective stepF = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];J = [-2*q(3),   2*q(4),    -2*q(1), 2*q(2)2*q(2),     2*q(1),     2*q(4), 2*q(3)0,         -4*q(2),    -4*q(3), 0    ];step = (J'*F);step = step / norm(step);   % normalise step magnitude% Compute rate of change of quaternionqDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step';% Integrate to yield quaternionq = q + qDot * obj.SamplePeriod;obj.Quaternion = q / norm(q); % normalise quaternion
end

可以看到，其实计算s时，用到了矩阵F和它的雅克比矩阵，这里的F是根据论文中的推导，用来描述当前姿态计算得到和重力方向同加速度传感器获取到的重力的差异，这里只是在各个分量上做减法，感觉也可以用其它方法。这里其实就是把最小化F当做了梯度下降法中的误差函数。它在论文中的推导如下：

其中公式14就是这里描述的F，只不过是更普适的版本，由于这里考虑的情况基于公式23，24 ，简化后的F就成了公式25. 再将两个矩阵做乘法，就得到了c语言版本中算法的那个s.

这个地方比较难理解，我们换一种说法，已经知道的是四元数可以表示坐标系的转换，而把b系转换为R系的四元数等价旋转矩阵为我们在四元数部分描述的公式9.2.33和34。

这里考虑把地球坐标系的重力加速度，转换到传感器的坐标系，即把在重力坐标系中的重力(归一化后)G[0,0,1]’，这里考虑重力永远是平行地面，垂直地心的。
用四元数Q，把这个G转到传感器坐标，就等价于用G乘以上述公式中的旋转矩阵，结果即可得到F中除去-ax,-ay,-az的部分，也就是重力在传感器坐标系中的值，与此时的传感器读数取差，即得到了此时的误差，这个误差可能是传感器当前Q不准导致的，也可能是加速度传感器不准导致的。

这里需要注意的是公式15描述的并不是真正的目标函数，只在xyz3个分量上取差值得到的是一个向量，无法直接用梯度下降法寻找它的极值，并且用它也不好解释公式21的由来，这里困扰了我一段时间，原论文中也没有详细描述。后面看了【运动传感器】Madgwick算法中详细分析了算法细节，对理解madgwick算法是个不错的参考。感觉这里真正要最小化的是公式15 f 的模，也就是下图描述的

其中所述的高斯牛顿法，我暂时还不完全接受，应为madgwick在论文中专门提到这个算法比高斯牛顿法计算量小。应该只是计算了梯度，但计算的是公式15的模。

参考这篇文章中的一些概念，这里考虑普遍的函数

f(q)=∑i=1ne2i(q)

f(q)=\sum_{i=1}^ne_i^2(q)
求导得(在变量q各个方向求偏导)

f′j(q)=2∑i=1nei(q)∂ei∂qj

f_j'(q)=2\sum_{i=1}^ne_i(q)\frac{\partial e_i}{\partial q_j}
等价于

∇f(q)=2JTe

\nabla f(q)=2J^T\Bbb e

然后对其求导，以得到梯度，注意这里q是自变量，它是四元数，因而分别取4个方向上的偏导。其中如果考虑xyz3个方向的话，n=3. J是 e(q) e(q) 的雅克比矩阵，其实就是各个方向求偏导数。至此，我们明白了c和matlab里求梯度时的计算原理，c语言直接计算了 ∇f(q) \nabla f(q) 的结果，即用 e(q) e(q) 的雅克比矩阵转置乘以 e(q) e(q)，得到了q在4个方向上的梯度，接着就是用梯度下降法对其进行迭代了.

这里还有另外一个重要的推导，过程在此略过，想看详细推导的见这里, 现在直接给出结论：

qt=qt−1+12qt×ωtΔt

q_t = q_{t-1} + \frac{1}{2}q_t \times \omega_t \Delta t
这也就是直接用陀螺仪更新q的公式，后面考虑如何用加速传感器的信息修正它, 即用前面提到的梯度下降法。

qt+1=qt−μ∇f||∇f||

q_{t+1}=q_t - \mu \frac{\nabla f}{||\nabla f||}
同时考虑两个公式得到最终的公式:

qt+1=qt+12qt×ωtΔt−β∇f||∇f||Δt

q_{t+1} = q_{t} + \frac{1}{2}q_t \times \omega_t \Delta t - \beta \frac{\nabla f}{||\nabla f||}\Delta_t

这个公式中需要注意的是， qt×ωt q_t \times \omega_t 是四元数叉乘，即 [q0,q1,q2,q3]×[0,ωx,ωy,ωz] [q_0,q_1,q_2,q_3]\times[0,\omega_x,\omega_y,\omega_z], 根据四元数叉乘：

p×q===(p0+p1i+p2j+p3k)×(q0+q1i+q2j+q3k)(p0q0−p1q1−p2q2−p3q3)+(p0q1+p1q0+p2q3−p3q2)i+(p0q2+p3q1−p1q3)j+(p0q3+p3q0+p1q1−p2q1)kr0+r1i+r2j+r3k

\begin{eqnarray}p\times q&=&(p_0+p_1i+p_2j+p_3k)\times(q_0+q_1i+q_2j+q_3k)\\&=&(p_0q_0-p_1q_1-p_2q_2-p_3q_3)+(p_0q_1+p_1q_0+p_2q_3-p_3q_2)i+(p_0q_2+p_3q_1-p_1q_3)j+(p_0q_3+p_3q_0+p_1q_1-p_2q_1)k\\&=&r_0+r_1i+r_2j+r_3k\end{eqnarray}
上式写成矩阵形式得

⎡⎣⎢⎢⎢r0r1r2r3⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢q0q1q2q3⎤⎦⎥⎥⎥⎥

\left[\begin{array}{c}r_0 \\ r_1 \\r_2 \\ r_3 \end{array}\right]=\left[\begin{array}{c,c,c,c}p_0 & -p_1 & -p_2 & -p_3\\ p_1 & p_0 & -p_3 & p_2\\p_2 & p_3 & p_0 & -p_1\\ p_3 & -p_2 & p_1 & p_0\end{array}\right]\left[\begin{array}{c}q_0 \\ q_1 \\q_2 \\ q_3 \end{array}\right]
所以

qt×ωt===[q1,q2,q3,q4]×[0,ωx,ωy,ωz](−q2ωx−q3ωy−q4ωz)+(q1ωx+q3ωz−q4ωy)i+(q1ωy+q4ωx−q2ωz)j+(q1ωz+q2ωx−q3ωx)k⎡⎣⎢⎢⎢⎢−q2ωx−q3ωy−q4ωzq1ωx+q3ωz−q4ωyq1ωy+q4ωx−q2ωzq1ωz+q2ωx−q3ωx⎤⎦⎥⎥⎥⎥

\begin{eqnarray}q_t \times \omega_t=&[q_1,q_2,q_3,q_4]\times[0,\omega_x,\omega_y,\omega_z]\\=&(-q_2\omega_x-q_3\omega_y-q_4\omega_z)+(q_1\omega_x+q_3\omega_z-q_4\omega_y)i+\\&(q_1\omega_y+q_4\omega_x-q_2\omega_z)j+(q_1\omega_z+q_2\omega_x-q_3\omega_x)k\\=&\left[\begin{array}{c}-q_2\omega_x-q_3\omega_y-q_4\omega_z \\ q_1\omega_x+q_3\omega_z-q_4\omega_y \\q_1\omega_y+q_4\omega_x-q_2\omega_z \\ q_1\omega_z+q_2\omega_x-q_3\omega_x \end{array}\right]\end{eqnarray}

手打弄这种公式好不容易啊~~~~

Sensor Fusion相关推荐

深度学习论文: Multi-modal Sensor Fusion for Auto Driving Perception: A Survey
深度学习论文: Multi-modal Sensor Fusion for Auto Driving Perception: A Survey Multi-modal Sensor Fusion fo ...
赶工读文章（2）— Radar-Camera Sensor Fusion for Joint Object Detection and Distance Estimation in AV
题目是AV是Autonomous Vehicles,太长写不下了.如有错漏,还望指正. Radar-Camera Sensor Fusion for Joint Object Detection an ...
Seeing Through Fog Without Seeing Fog:Deep Multimodal Sensor Fusion in Unseen Adverse Weather （翻）
Title:Seeing Through Fog Without Seeing Fog:Deep Multimodal Sensor Fusion in Unseen Adverse Weather ...
【文献阅读】Sensor Fusion Algorithm Design in Detecting Vehicles Using Laser Scanner and Stereo Vision
利用激光扫描仪和立体视觉检测车辆的传感器融合算法设计摘要众所周知,激光扫描仪在探测障碍物的距离和速度方面比立体视觉具有更好的精度,而立体视觉比激光扫描仪更能分辨物体. 通过传感器融合方法,可以最大 ...
论文阅读综述：自动驾驶感知的多模态传感器融合Multi-modal Sensor Fusion for Auto Driving Perception: A Survey
题目:Multi-modal Sensor Fusion for Auto Driving Perception: A Survey 用于自动驾驶感知的多模态传感器融合:综述链接:https://a ...
[2]Mohamed M. Atia,Steven.L. Waslander. Map-aided Adaptive GNSS/IMU Sensor Fusion Scheme for Robust
[2]Mohamed M. Atia,Steven.L. Waslander. Map-aided Adaptive GNSS/IMU Sensor Fusion Scheme for Robust ...
PointFusion: Deep Sensor Fusion for 3D Bounding Box Estimation
摘要我们提出了PointFusion,一种利用图像和3D点云信息的通用3D对象检测方法.与使用多级管道或保持传感器和数据集特定假设的现有方法不同,PointFusion在概念上简单且与应用程序无关. ...
【自动驾驶】《Multi-modal Sensor Fusion for Auto Driving Perception: A Survey》论文阅读笔记
引用 @article{Wang2020MultiSensorFI,title={Multi-Sensor Fusion in Automated Driving: A Survey},author= ...
论文阅读 2017 —— Sensor Fusion for Fiducial Tags: Highly Robust Pose Estimation from Single Frame RGBD
这是一篇发表在IROS的一篇论文,用RGBD相机提升码标(Aruco)的位姿精度.尽管在增强现实和计算机视觉中可以应用多种码标,但是在机器人应用中,从这些码标中获得准确的位姿目前仍然是个挑战.难点很清 ...

Sensor Fusion