IMU姿态滤波算法——Mahony算法：原理与代码
1 前言
Mahony算法常见的姿态融合算法，根据加速度计、陀螺仪、以及磁力计，融合计算机体四元数，计算速度快、精度较高。本文介绍六轴融合，即根据加速度计和陀螺仪数据，计算姿态。
我们需要计算的是机体的姿态。计算角度可以通过角速度积分，也可以通过加速度正交分解，但这两种方法都存在缺陷。角速度的误差会随着积分不断增大，而加速度存在高频噪声，因此希望融合两种数据。
本文作者：小L | 来源：微信公众号「3D视觉工坊」
讨论：为什么用叉乘？
陀螺仪由于本身精度问题，测量的角速度存在误差，在积分过程中这个误差会一直累加，我们要做的就是去消除或是补偿这个误差，因为加速度计长期的测量值是准确的，所以可以用加速度计来进行修正。如何找到一个另一个角速度量纲的值来修正陀螺仪的角速度值呢？这里明明只有陀螺仪可以测量角速度！这时候前面提到的向量叉积得到的误差向量就帮上大忙了，这个误差向量不就是反映出了角度变化量吗。算法巧妙的将加速度相关量转化为角度相关量，因而可以用这个角度值乘一个系数来修正陀螺仪的角速度，因为在偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系，也就说明陀螺仪积分误差和向量叉积存在正比关系。[2]
欢迎关注微信公众号「3D视觉工坊」，加群/文章投稿/课程主讲，请加微信：QYong2014，添加时请备注：加群/投稿/主讲申请
方向主要包括：3D视觉领域各细分方向，比如相机标定|三维点云|三维重建|视觉/激光SLAM|感知|控制规划|模型部署|3D目标检测|TOF|多传感器融合|AR|VR|编程基础等。
3 核心代码解析
我们以Matlab代码为例，结合上述内容进行介绍：
function obj = UpdateIMU(obj, Gyroscope, Accelerometer) q = obj.Quaternion; % 当前时刻的四元数 % 归一化加速度计测量数据 if(norm(Accelerometer) == 0), return; end % handle NaN Accelerometer = Accelerometer / norm(Accelerometer); % normalise magnitude % 计算重力在当前四元数位姿下的分量，即上述公式(2) v = [2*(q(2)*q(4) - q(1)*q(3)) 2*(q(1)*q(2) + q(3)*q(4)) q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2]; % 计算重力分量与加速度计的测量误差，上述公式(3) e = cross(Accelerometer, v); if(obj.Ki > 0) obj.eInt = obj.eInt + e * obj.SamplePeriod; % 计算误差的积分，公式(4) else obj.eInt = [0 0 0]; end % 角速度融合，公式(5) Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt; % 公式(6) qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]); % 欧拉积分计算下一时刻四元数，公式(7) q = q + qDot * obj.SamplePeriod; obj.Quaternion = q / norm(q); % 结果归一化end
4 完整代码获取
官方C++/Matlab/C#代码：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
官方python代码：https://github.com/xioTechnologies/Fusion/tree/main/Python
第三方python姿态解算库：https://ahrs.readthedocs.io/en/latest/filters/mahony.html
参考资料：
[1] Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control, 2008, 53(5), pp.1203-1217.
[2] https://www.dgrt.cn/a/1551295.html
[3] https://zhuanlan.zhihu.com/p/395511536
[4] https://zhuanlan.zhihu.com/p/422074759
关注知乎「3D视觉工坊」(https://www.zhihu.com/people/qiyong1993),及时获取每日3D视觉行业最前沿学术研究。
点击加入「SLAM」(https://mp.weixin.qq.com/s/MKi8k8P3PQqPsgCnpryjSQ)技术交流群
