IMU姿态滤波算法——Mahony算法：原理与代码_mahony 算法

1 前言

Mahony算法常见的姿态融合算法，根据加速度计、陀螺仪、以及磁力计，融合计算机体四元数，计算速度快、精度较高。本文介绍六轴融合，即根据加速度计和陀螺仪数据，计算姿态。
我们需要计算的是机体的姿态。计算角度可以通过角速度积分，也可以通过加速度正交分解，但这两种方法都存在缺陷。角速度的误差会随着积分不断增大，而加速度存在高频噪声，因此希望融合两种数据。

2 算法

2.1 重力对齐误差

首先要指出的是，Mahony算法假设加速度计测量的加速度完全由重力提供，即物体本体运动产生的加速度可忽略不计。在这一假设下，我们假设当前时刻机体的姿态为，则将重力向量的表示转到机体坐标系下，应该为：，这里表示四元数对应的旋转矩阵：

进一步地，带入，得到

我们计加速度计测量得到的加速度，如果此时没有误差，应该有，但实际两个向量并不重合，存在一定的误差 。

为表示出，可以利用向量的叉乘：。因为叉乘的定义为：，当归一化为单位向量时，反应的就是角度。这里更准确的写为，下一时刻{t+1}时的误差为：

其中  为根据当前{t}时刻估计的角度四元数。再记这个误差的积分量为：

误差的积分量也参与了后续计算。

2.2 角速度融合