前言
在无人机、机器人、VR/AR等智能设备蓬勃发展的今天，低成本姿态感知技术已成为实现智能交互的关键核心。面对专业级惯性导航系统的高昂成本，基于MEMS加速度计与陀螺仪的数据融合方案以其优异的性价比，为消费级产品提供了稳定可靠的姿态解算能力。
本文将系统阐述从传感器原理到算法实现的完整技术路径，重点解析互补滤波、Mahony滤波、Madgwick滤波和扩展卡尔曼滤波四种主流融合方法，并提供经过实践验证的C语言代码实现，助力开发者掌握低成本高精度的姿态解算技术。
姿态解算原理
在姿态解算时我们需要用到加速度计与陀螺仪两者的数据，它们有着各自的特点且在姿态解算中它们都有着不同的意义与使用方法。
加速度计
加速度计主要测量的是物体加速度，他是一个‘比力’由物体加速度与重力加速度组合而成。
- 比力 = 物体加速度 - 重力加速度（在惯性导航中的标准定义）
- 重力加速度：这是地球对你产生的永恒不变的吸引力。它永远把你朝着地心的方向拉，大小基本不变。
- 物体加速度：你自己运动状态改变时产生的加速度。比如汽车、飞机、你自己启动、刹车、转弯时感受到的推力。 在这里我们可以将重力加速度理解为一个指向地心永恒不变的力，将物体加速度理解为物体的惯性，而加速度计则是测量它们的‘比力’在物体坐标系下是如何分布在x,y,z轴上的。
- 物体坐标系：一个“长在物体身上的坐标系”，它会跟着物体一起移动、一起转动。一个以物体为中心构建的坐标系。
综上所述，加速度计测量的是物体坐标系下X、Y、Z轴上的三个力（记为ax, ay, az）。在静止状态下（即物体加速度为零），物体仅受永恒不变的重力影响，故加速度计的输出值即可被视为物体坐标系下的重力向量（记为 ）。据此，由于该重力向量与世界坐标系的Z轴平行，是一个绝对的参考基准，我们便可通过它在物体坐标系中的方向，反推出物体在世界坐标系下的姿态（重力向量在物体坐标系下的状态就是物体在世界坐标系下的状态）。
陀螺仪
陀螺仪主要测量的是物体坐标系下物体围绕着x,y,z三轴的角速度(记为wx, wy, wz)，是一个描述x,y,z三轴角度变化快慢的量，是角度的导数。它的单位通常是 度/秒 (°/s) 或 弧度/秒 (rad/s)。
- 角速度与线速度在数学上都是变化率，遵循相似的积分关系：
（速度与距离）距离=速度x时间
（角速度与角度）角度=角速度x时间。
在计算机的离散系统中，我们不能直接使用连续的物理公式，而需将其离散化：
（速度与位置）当前位置 = 上一时刻位置 + 速度 × 时间
（角速度与角度）当前角度 = 上一时刻角度 + 角速度 × 时间 - 因为我们总是默认在初始时刻，物体坐标系是与世界坐标系对齐的，所以，这个通过积分自身角速度得到的、相对于初始姿态的旋转量，就直接等价于当前物体坐标系相对于世界坐标系的姿态
通过对三个轴角速度的连续积分我们可以直接求出物体姿态，但在实际工程中，陀螺仪存在多种误差，其中最为关键的是零偏误差（即无旋转时的非零输出）。由于姿态解算需要对角速度进行积分，任何微小的零偏都会在积分过程中被不断累积放大，导致计算出的姿态角随时间发生严重漂移。
姿态描述的方法
在描述物体的姿态时，常用的方法有三种：欧拉角、四元数、旋转矩阵。它们之间的关系就像是距离中的公里、英里、海里一样，且它们之间可以互相转换。 欧拉角：
- 优点：非常直观
- 缺点：万向节死锁、计算复杂 旋转矩阵:
- 优点：无奇异性、旋转向量直接、组合简单
- 缺点：计算冗余(用9个变量表示姿态)。 四元数:
- 优点：紧凑、无奇异性、计算高效、数值稳定
- 缺点：不直观(人类很难直接理解一个四元数的几何意义)
欧拉角、四元数、旋转矩阵的转换与计算涉及到三角函数与坐标系转换，因此它们是非线性的。
姿态解算理论
在之前的描述中我们可以发现，加速度计与陀螺仪它们都可以用来完成状态解算，但它们都有各自的局限性。 加速度计：
- 解算公式：俯仰角 (Pitch, θ)，滚转角 (Roll, φ)：
- 特点：测得的重力向量姿态就是物体在世界坐标系下的姿态，且重力的大小与方向是不变的。
- 局限性：受到物体加速度的影响，只有在静止状态下准确。重力向量于世界坐标系的z轴平行无法检测偏航角Yaw（ψ） 陀螺仪：
- 解算公式：(欧拉角)当前角度 = 上一时刻角度 + 角速度 × 时间
- 特点：测量的值是固定时间内角度的变化，积分后是固定时间内上一时刻于当前时刻变化的角，相应速度块。
- 局限性：角度在积分累积时零偏的误差也在累积，时间长了需要矫正误差。 注：无法检测偏航角Yaw是由于加速度计的特性导致的，想要检测就必须使用磁力计。但磁力计会受到周围磁场的影响，尤其是在电机与电源电感的附近。
由于它们各自的特性导致我们单独采取任何一方的数据都达不到我们想要的效果，因此我们想要将它们的数据进行融合处理。 融合的过程一般是以角速度为主，加速度为辅。使用加速度计测量到的重力向量与使用角速度预测的重力向量作比较来纠正角速度。
常用方法
常用的姿态融合方法有四种：互补滤波、Mahony滤波、Madgwick滤波、扩展卡尔曼滤波。 在使用姿态融合算法时我们通常使用两种描述姿态：四元数、欧拉角。 由于四元数没有万向节死锁的缺点，所以通常我们都是使用四元数进行姿态的描述与融合，但由于四元数是双线性的，所以使用四元数为扩展卡尔曼滤波的算数单元是不适合的，在实践中会带来较大误差(结果会非常不理想)。
姿态融合
前期处理
在姿态融合前需要做好数据的前期处理，其中最重要的是保证角速度的单位是弧度每秒，以及加速度的重力向量归一化。
变量设计
#define PI 3.1415926535f
//欧拉角
typedef struct {
float yaw; // 偏航角 ψ (Z轴)
float pitch; // 俯仰角 θ (Y轴)
float roll; // 横滚角 φ (X轴)
} EulerAngles;
//四元数
typedef struct {
float w, x, y, z;
} Quaternion;
// 定义角速度结构体
typedef struct {
float wx, wy, wz;
} AngularVelocity;
// 加速度数据结构体
typedef struct {
float ax, ay, az;
} AccelerationData;
// 三维向量数据结构体
typedef struct {
float x, y, z; // 三维向量
} VectorQuantity3D;
//预测的重力向量（在世界坐标系中预测机体坐标系）公式的雅可比矩阵
typedef struct {
float J[3][4];
} QuaternionToPredictedGravity_JacobiMatrix;
//IMU的数据
typedef struct {
AccelerationData Acc;
AngularVelocity Gyr;
} IUN_OriginalData;
常用数学函数(四元数基础运算函数)
/*************************************四元数基础运算函数*************************************/
// 归一化四元数
Quaternion normalize_quaternion(Quaternion q)
{
float norm_sq = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;
if (norm_sq > 1e-12f) {
// 使用快速逆平方根或直接除法
float inv_norm = 1.0f / sqrtf(norm_sq);
q.w *= inv_norm;
q.x *= inv_norm;
q.y *= inv_norm;
q.z *= inv_norm;
} else {
// 重置为单位四元数
q.w = 1.0f; q.x = 0.0f; q.y = 0.0f; q.z = 0.0f;
}
return q;
}
//四元数乘以常数
Quaternion quaternionMultiplyScalar(Quaternion q, float scalar)
{
Quaternion result;
result.w = q.w * scalar;
result.x = q.x * scalar;
result.y = q.y * scalar;
result.z = q.z * scalar;
return result;
}
//四元数加法
Quaternion quaternionAdd(Quaternion q1, Quaternion q2) {
Quaternion result;
result.w = q1.w + q2.w;
result.x = q1.x + q2.x;
result.y = q1.y + q2.y;
result.z = q1.z + q2.z;
return result;
}
//四元数减法
Quaternion quaternionSubtract(Quaternion q1, Quaternion q2)
{
Quaternion result;
result.w = q1.w - q2.w;
result.x = q1.x - q2.x;
result.y = q1.y - q2.y;
result.z = q1.z - q2.z;
return result;
}
// 四元数乘法
Quaternion quaternion_multiply(Quaternion q1, Quaternion q2)
{
Quaternion result;
result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return result;
}
// 重置姿态为单位四元数
void reset_attitude(Quaternion* q)
{
q->w = 1.0f;
q->x = 0.0f;
q->y = 0.0f;
q->z = 0.0f;
}
// 获取当前四元数的共轭（用于坐标变换等）
Quaternion quaternion_conjugate(Quaternion q)
{
Quaternion conj;
conj.w = q.w;
conj.x = -q.x;
conj.y = -q.y;
conj.z = -q.z;
return conj;
}
// 四元数点积
float quaternion_dot(Quaternion q1, Quaternion q2)
{
return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
}
// 四元数球面线性插值
Quaternion quaternion_slerp(Quaternion q1, Quaternion q2, float t)
{
// 确保t在[0,1]范围内
if (t < 0.0f) t = 0.0f;
if (t > 1.0f) t = 1.0f;
float dot = quaternion_dot(q1, q2);
// 如果点积为负，取反其中一个四元数以保证最短路径
if (dot < 0.0f) {
q2.w = -q2.w;
q2.x = -q2.x;
q2.y = -q2.y;
q2.z = -q2.z;
dot = -dot;
}
const float DOT_THRESHOLD = 0.9995f;
if (dot > DOT_THRESHOLD) {
// 如果四元数非常接近，使用线性插值避免数值问题
Quaternion result;
result.w = q1.w + t * (q2.w - q1.w);
result.x = q1.x + t * (q2.x - q1.x);
result.y = q1.y + t * (q2.y - q1.y);
result.z = q1.z + t * (q2.z - q1.z);
return normalize_quaternion(result);
}
float theta_0 = acosf(dot); // 角度
float theta = theta_0 * t; // 插值角度
float sin_theta = sinf(theta);
float sin_theta_0 = sinf(theta_0);
float s1 = cosf(theta) - dot * sin_theta / sin_theta_0;
float s2 = sin_theta / sin_theta_0;
Quaternion result;
result.w = s1 * q1.w + s2 * q2.w;
result.x = s1 * q1.x + s2 * q2.x;
result.y = s1 * q1.y + s2 * q2.y;
result.z = s1 * q1.z + s2 * q2.z;
return normalize_quaternion(result);
}
常用数学函数 (四元数与欧拉角转换)
// 安全数学函数
float safe_asin(float x)
{
if (x >= 1.0f) return PI/2.0f;
if (x <= -1.0f) return -PI/2.0f;
return asinf(x);
}
// 欧拉角 → 四元数 (ZYX顺序)
Quaternion euler_to_quaternion(EulerAngles euler)
{
float half_psi = euler.yaw * 0.5f;
float half_theta = euler.pitch * 0.5f;
float half_phi = euler.roll * 0.5f;
float cos_psi = cosf(half_psi);
float sin_psi = sinf(half_psi);
float cos_theta = cosf(half_theta);
float sin_theta = sinf(half_theta);
float cos_phi = cosf(half_phi);
float sin_phi = sinf(half_phi);
Quaternion q;
// ZYX顺序：q = q_z(ψ) * q_y(θ) * q_x(φ)
q.w = cos_psi * cos_theta * cos_phi + sin_psi * sin_theta * sin_phi;
q.x = cos_psi * cos_theta * sin_phi - sin_psi * sin_theta * cos_phi;
q.y = cos_psi * sin_theta * cos_phi + sin_psi * cos_theta * sin_phi;
q.z = sin_psi * cos_theta * cos_phi - cos_psi * sin_theta * sin_phi;
return normalize_quaternion(q);
}
// 四元数 → 欧拉角 (ZYX顺序)
EulerAngles quaternion_to_euler(Quaternion q)
{
q = normalize_quaternion(q);
EulerAngles euler;
// 俯仰角 θ = asin(2*(w*y - x*z))
float sin_pitch = 2.0f * (q.w * q.y - q.x * q.z);
euler.pitch = safe_asin(sin_pitch);
// 检查万向锁 (俯仰角接近 ±90°)
float cos_theta = cosf(euler.pitch);
if (fabsf(cos_theta) > 1e-6f)
{
// 非万向锁情况
// 偏航角 ψ = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
euler.yaw = atan2f(2.0f * (q.w * q.z + q.x * q.y),
1.0f - 2.0f * (q.y*q.y + q.z*q.z));
// 横滚角 φ = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
euler.roll = atan2f(2.0f * (q.w * q.x + q.y * q.z),
1.0f - 2.0f * (q.x*q.x + q.y*q.y));
} else
{
// 万向锁情况：俯仰角接近 ±90°
// 此时只能确定 ψ - φ 或 ψ + φ
if (euler.pitch > 0)
{ // θ = +90°
euler.roll = 0.0f;
euler.yaw = 2.0f * atan2f(q.x, q.w);
}
else
{ // θ = -90°
euler.roll = 0.0f;
euler.yaw = -2.0f * atan2f(q.x, q.w);
}
}
return euler;
}
常用数学函数(四元数微分方程与增量旋转方法)
用于使用角速度跟新四元数，角速度跟新四元数时单位必须是弧度/秒
// 改进的欧拉法（二阶龙格-库塔法）更新四元数
Quaternion integrate_angular_velocity_improved_euler(Quaternion q, AngularVelocity omega, float dt)
{
// // 将角速度从度/秒转换为弧度/秒
// float wx_rad = omega.wx * PI / 180.0f;
// float wy_rad = omega.wy * PI / 180.0f;
// float wz_rad = omega.wz * PI / 180.0f;
float wx_rad = omega.wx ;
float wy_rad = omega.wy ;
float wz_rad = omega.wz ;
// 第一步：计算初始斜率 k1
Quaternion omega_q1 = {0.0f, wx_rad, wy_rad, wz_rad};
Quaternion k1 = quaternion_multiply(q, omega_q1);
k1.w *= 0.5f; k1.x *= 0.5f; k1.y *= 0.5f; k1.z *= 0.5f;
// 第二步：使用k1预测中间点的四元数
Quaternion q_mid;
q_mid.w = q.w + k1.w * dt * 0.5f;
q_mid.x = q.x + k1.x * dt * 0.5f;
q_mid.y = q.y + k1.y * dt * 0.5f;
q_mid.z = q.z + k1.z * dt * 0.5f;
q_mid = normalize_quaternion(q_mid);
// 第三步：计算中间点的斜率 k2
Quaternion k2 = quaternion_multiply(q_mid, omega_q1);
k2.w *= 0.5f; k2.x *= 0.5f; k2.y *= 0.5f; k2.z *= 0.5f;
// 第四步：使用平均斜率更新四元数
q.w += k2.w * dt;
q.x += k2.x * dt;
q.y += k2.y * dt;
q.z += k2.z * dt;
return normalize_quaternion(q);
}
// 四阶龙格-库塔法更新四元数（更高精度）
Quaternion integrate_angular_velocity_rk4(Quaternion q, AngularVelocity omega, float dt)
{
// // 将角速度从度/秒转换为弧度/秒
// float wx_rad = omega.wx * PI / 180.0f;
// float wy_rad = omega.wy * PI / 180.0f;
// float wz_rad = omega.wz * PI / 180.0f;
float wx_rad = omega.wx ;
float wy_rad = omega.wy ;
float wz_rad = omega.wz ;
Quaternion omega_q = {0.0f, wx_rad, wy_rad, wz_rad};
// k1 = f(t, q)
Quaternion k1 = quaternion_multiply(q, omega_q);
k1.w *= 0.5f; k1.x *= 0.5f; k1.y *= 0.5f; k1.z *= 0.5f;
// k2 = f(t + dt/2, q + k1*dt/2)
Quaternion q2;
q2.w = q.w + k1.w * dt * 0.5f;
q2.x = q.x + k1.x * dt * 0.5f;
q2.y = q.y + k1.y * dt * 0.5f;
q2.z = q.z + k1.z * dt * 0.5f;
q2 = normalize_quaternion(q2);
Quaternion k2 = quaternion_multiply(q2, omega_q);
k2.w *= 0.5f; k2.x *= 0.5f; k2.y *= 0.5f; k2.z *= 0.5f;
// k3 = f(t + dt/2, q + k2*dt/2)
Quaternion q3;
q3.w = q.w + k2.w * dt * 0.5f;
q3.x = q.x + k2.x * dt * 0.5f;
q3.y = q.y + k2.y * dt * 0.5f;
q3.z = q.z + k2.z * dt * 0.5f;
q3 = normalize_quaternion(q3);
Quaternion k3 = quaternion_multiply(q3, omega_q);
k3.w *= 0.5f; k3.x *= 0.5f; k3.y *= 0.5f; k3.z *= 0.5f;
// k4 = f(t + dt, q + k3*dt)
Quaternion q4;
q4.w = q.w + k3.w * dt;
q4.x = q.x + k3.x * dt;
q4.y = q.y + k3.y * dt;
q4.z = q.z + k3.z * dt;
q4 = normalize_quaternion(q4);
Quaternion k4 = quaternion_multiply(q4, omega_q);
k4.w *= 0.5f; k4.x *= 0.5f; k4.y *= 0.5f; k4.z *= 0.5f;
// 最终更新: q = q + (k1 + 2*k2 + 2*k3 + k4) * dt / 6
q.w += (k1.w + 2.0f*k2.w + 2.0f*k3.w + k4.w) * dt / 6.0f;
q.x += (k1.x + 2.0f*k2.x + 2.0f*k3.x + k4.x) * dt / 6.0f;
q.y += (k1.y + 2.0f*k2.y + 2.0f*k3.y + k4.y) * dt / 6.0f;
q.z += (k1.z + 2.0f*k2.z + 2.0f*k3.z + k4.z) * dt / 6.0f;
return normalize_quaternion(q);
}
// 角速度积分更新四元数（一阶龙格-库塔法/欧拉法）
Quaternion integrate_angular_velocity_euler(Quaternion q, AngularVelocity omega, float dt)
{
// 将角速度从度/秒转换为弧度/秒
// float wx_rad = omega.wx * PI / 180.0f;
// float wy_rad = omega.wy * PI / 180.0f;
// float wz_rad = omega.wz * PI / 180.0f;
float wx_rad = omega.wx ;
float wy_rad = omega.wy ;
float wz_rad = omega.wz ;
// 将角速度转换为四元数形式的角速度
Quaternion omega_q;
omega_q.w = 0.0f;
omega_q.x = wx_rad;
omega_q.y = wy_rad;
omega_q.z = wz_rad;
// 计算四元数导数: dq/dt = 0.5 * q ? ω_q
Quaternion dq = quaternion_multiply(q, omega_q);
dq.w *= 0.5f;
dq.x *= 0.5f;
dq.y *= 0.5f;
dq.z *= 0.5f;
// 一阶积分: q(t+dt) = q(t) + dq * dt
q.w += dq.w * dt;
q.x += dq.x * dt;
q.y += dq.y * dt;
q.z += dq.z * dt;
// 归一化四元数
return normalize_quaternion(q);
}
// 增量旋转方法：q(t+dt) = q(t) * Δq（精确方法）
Quaternion integrate_angular_velocity_incremental(Quaternion q, AngularVelocity omega, float dt)
{
// 将角速度从度/秒转换为弧度/秒
// float wx_rad = omega.wx * PI / 180.0f;
// float wy_rad = omega.wy * PI / 180.0f;
// float wz_rad = omega.wz * PI / 180.0f;
float wx_rad = omega.wx ;
float wy_rad = omega.wy ;
float wz_rad = omega.wz ;
float omega_magnitude = sqrtf(wx_rad * wx_rad + wy_rad * wy_rad + wz_rad * wz_rad);
// 如果角速度很小，使用一阶欧拉法避免数值问题
if (omega_magnitude < 1e-6f) {
return integrate_angular_velocity_euler(q, omega, dt);
}
// 计算旋转角度和旋转轴
float rotation_angle = omega_magnitude * dt;
float axis_x = wx_rad / omega_magnitude;
float axis_y = wy_rad / omega_magnitude;
float axis_z = wz_rad / omega_magnitude;
// 构造增量旋转四元数
float half_angle = rotation_angle * 0.5f;
float sin_half = sinf(half_angle);
float cos_half = cosf(half_angle);
Quaternion delta_q;
delta_q.w = cos_half;
delta_q.x = axis_x * sin_half;
delta_q.y = axis_y * sin_half;
delta_q.z = axis_z * sin_half;
// 应用增量旋转: q(t+dt) = q(t) ? Δq
return quaternion_multiply(q, delta_q);
}
常用数学函数(向量的基本运算)
/********************************************向量的基本运算*******************************************/
/**
* 计算向量的模长（欧几里得范数）
* @param v 输入向量
* @return 向量的模长
*/
float Vector_Magnitude(VectorQuantity3D v)
{
return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
}
/**
* 向量归一化：将向量转换为单位向量
* @param v 输入向量（将被归一化）
* @return 归一化后的单位向量
* @note 若输入为零向量，则返回零向量（避免除以零错误）
*/
VectorQuantity3D Vector_Normalize(VectorQuantity3D v)
{
float mag = Vector_Magnitude(v);
VectorQuantity3D result = {0};
// 检查模长是否接近零（避免浮点数精度问题）
if (mag > 1e-6f) {
result.x = v.x / mag;
result.y = v.y / mag;
result.z = v.z / mag;
}
// 若模长为零，直接返回零向量（保持原始向量会导致除以零）
return result;
}
/**
* 向量加法：逐分量相加
* @param a 向量a
* @param b 向量b
* @return a + b 的结果向量
*/
VectorQuantity3D Vector_Add(VectorQuantity3D a, VectorQuantity3D b)
{
return (VectorQuantity3D){a.x + b.x, a.y + b.y, a.z + b.z};
}
/**
* 向量减法：逐分量相减
* @param a 向量a
* @param b 向量b
* @return a - b 的结果向量
*/
VectorQuantity3D Vector_Subtract(VectorQuantity3D a, VectorQuantity3D b)
{
return (VectorQuantity3D){a.x - b.x, a.y - b.y, a.z - b.z};
}
/**
* 向量乘以常数（标量乘法）
* @param vector 输入向量
* @param scalar 标量常数
* @return 缩放后的向量
*/
VectorQuantity3D Vector_ScalarMultiply(VectorQuantity3D vector, float scalar)
{
VectorQuantity3D result;
result.x = vector.x * scalar;
result.y = vector.y * scalar;
result.z = vector.z * scalar;
return result;
}
/**
* 向量除以常数（标量除法）
* @param vector 输入向量
* @param scalar 标量常数
* @return 缩放后的向量
* @note 如果标量为0，则返回原始向量（避免除以零错误）
*/
VectorQuantity3D Vector_ScalarDivide(VectorQuantity3D vector, float scalar)
{
VectorQuantity3D result;
// 检查除零情况
if (fabsf(scalar) < 1e-6f) {
// 返回原始向量而不是除以零
return vector;
}
float inv_scalar = 1.0f / scalar;
result.x = vector.x * inv_scalar;
result.y = vector.y * inv_scalar;
result.z = vector.z * inv_scalar;
return result;
}
/**
* 向量点积（内积）
* @param a 向量a
* @param b 向量b
* @return 点积结果（标量）
*/
float Vector_DotProduct(VectorQuantity3D a, VectorQuantity3D b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/**
* 向量叉积（外积）
* @param a 向量a
* @param b 向量b
* @return 叉积结果向量（垂直于a和b构成的平面）
*/
VectorQuantity3D Vector_CrossProduct(VectorQuantity3D a, VectorQuantity3D b)
{
return (VectorQuantity3D){
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
};
}
/**
* 计算两点间距离（视为从原点出发的向量）
* @param a 点A（向量）
* @param b 点B（向量）
* @return 两点间的欧几里得距离
*/
float Vector_Distance(VectorQuantity3D a, VectorQuantity3D b)
{
VectorQuantity3D diff = Vector_Subtract(a, b);
return Vector_Magnitude(diff);
}
/**
* 使用三维向量直接构建四元数（所有分量）
* @param w 实部
* @param vec 虚部向量
* @return 构建的四元数
*/
Quaternion VectorToQuaternion_Full(float w, VectorQuantity3D vec)
{
return (Quaternion){w, vec.x, vec.y, vec.z};
}
/**
* 提取四元数的虚部构建三维向量
* @param q 输入四元数
* @return 虚部组成的三维向量
*/
VectorQuantity3D QuaternionToVector_Imaginary(Quaternion q)
{
return (VectorQuantity3D){q.x, q.y, q.z};
}
/********************************************向量的基本运算*******************************************/
预测重力向量
设世界坐标系下的归一化重力向量为[0,0,1]
根据当前的姿态四元数旋转设定的重力向量得到角速度的预测重力向量
旋转公式：
其中：就世界坐标系下的重力向量，是旋转后在角速度的物体坐标系下的预测重力向量
将重力向量:[0,0,1]带入旋转公式：
得到预测公式：
g_pred = [ 2*(x*z - w*y), # x轴分量 2*(w*x + y*z), # y轴分量 1 - 2*(x*x + y*y) # z轴分量 w*w - x*x - y*y + z*z或1 - 2*(xx + yy) ]
注：重力向量:[0,0,1]的前提是你的加速度测量的重力为正数(加速度计的物体坐标系的z轴是朝上的)
/**
* 根据四元数计算预测的重力向量
* 设重力向量[0,0,1]
* @param q 输入四元数（通常表示设备朝向）
* @return 预测的重力向量（在世界坐标系中）
*/
VectorQuantity3D QuaternionToPredictedGravity(Quaternion q)
{
VectorQuantity3D gravity;
float w = q.w, x = q.x, y = q.y, z = q.z;
// 标准公式：将四元数旋转应用于[0,0,1]向量
gravity.x = 2.0f * (x*z - w*y);
gravity.y = 2.0f * (y*z + w*x);
// gravity.z = w*w - x*x - y*y + z*z;或1 - 2*(x² + y²)
gravity.z = 1.0f - 2.0f * (x*x + y*y); // z分量正确
return gravity;
}
加速度预处理
- 将加速度转换为米每秒
- 确定加速度计物体坐标系z轴的朝向
- 归一化加速度计的重力向量
/*******************************加速度处理函数****************************/
#define M_PI_2 1.57079632679489661923f
// 在归一化前检查原始数据
int is_reasonable_acceleration(AccelerationData accel)
{
float norm = sqrtf(accel.ax*accel.ax + accel.ay*accel.ay + accel.az*accel.az);
// 地球重力范围：约7.8-11.8 m/s2（考虑海拔和纬度）
return (norm > 7.0f && norm < 12.0f);
}
// 加速度数据归一化函数
AccelerationData normalize_acceleration(AccelerationData accel)
{
AccelerationData normalized;
float norm = sqrtf(accel.ax * accel.ax + accel.ay * accel.ay + accel.az * accel.az);
if (norm > 1e-6f)
{
normalized.ax = accel.ax / norm;
normalized.ay = accel.ay / norm;
normalized.az = accel.az / norm;
}
else
{
// 返回零向量表示归一化失败
normalized.ax = 0.0f;
normalized.ay = 0.0f;
normalized.az = 0.0f;
}
return normalized;
}
// 加速度数据归一化函数
AccelerationData normalize_acceleration_2(AccelerationData accel)
{
AccelerationData normalized = {0};
// 使用双精度计算避免精度损失
double norm_sq = (double)accel.ax*accel.ax +
(double)accel.ay*accel.ay +
(double)accel.az*accel.az;
float norm = sqrtf((float)norm_sq);
// 更合理的阈值，考虑实际传感器范围
if (norm > 0.5f && norm < 20.0f)
{ // 放宽范围但仍排除异常值
normalized.ax = accel.ax / norm;
normalized.ay = accel.ay / norm;
normalized.az = accel.az / norm;
}
return normalized;
}
// 检查是否为有效归一化数据
int is_valid_normalized_data(AccelerationData norm_accel)
{
float check_norm = sqrtf(norm_accel.ax * norm_accel.ax +
norm_accel.ay * norm_accel.ay +
norm_accel.az * norm_accel.az);
// 放宽检查范围，考虑数值误差
return (check_norm > 0.8f && check_norm < 1.2f);
}
// 使用归一化加速度数据计算欧拉角
EulerAngles compute_euler_angles_from_normalized_accel(AccelerationData normalized_accel)
{
EulerAngles euler_angles;
// 检查输入数据是否有效
if (!is_valid_normalized_data(normalized_accel))
{
euler_angles.roll = 0.0f;
euler_angles.pitch = 0.0f;
euler_angles.yaw = 0.0f;
return euler_angles;
}
// 计算横滚角 (绕X轴)
if (fabsf(normalized_accel.az) > 1e-6f || fabsf(normalized_accel.ay) > 1e-6f)
{
euler_angles.roll = atan2f(normalized_accel.ay, normalized_accel.az);
}
else
{
euler_angles.roll = 0.0f;
}
// 计算俯仰角 (绕Y轴)
float denominator = sqrtf(normalized_accel.ay * normalized_accel.ay + normalized_accel.az * normalized_accel.az);
if (denominator > 1e-6f)
{
euler_angles.pitch = atan2f(-normalized_accel.ax, denominator);
}
else
{
euler_angles.pitch = (normalized_accel.ax > 0) ? -M_PI_2 : M_PI_2;
}
// 加速度计无法测量偏航角
euler_angles.yaw = 0.0f;
return euler_angles;
}
/*******************************加速度处理函数****************************/
角速度预处理
- 将角速度转换弧度每秒
- 确定检测间隔时间(秒)
/*******************************辅助功能函数*****************************/
// 弧度与度数转换函数
float BSP_rad_to_deg(float radians)
{
return radians * 180.0f / PI;
}
float BSP_deg_to_rad(float degrees)
{
return degrees * PI / 180.0f;
}
// 欧拉角度数转换为弧度
EulerAngles BSP_euler_deg_to_rad(EulerAngles euler_deg)
{
EulerAngles euler_rad;
euler_rad.yaw = BSP_deg_to_rad(euler_deg.yaw);
euler_rad.pitch = BSP_deg_to_rad(euler_deg.pitch);
euler_rad.roll = BSP_deg_to_rad(euler_deg.roll);
return euler_rad;
}
// 欧拉角弧度转换为度数
EulerAngles BSP_euler_rad_to_deg(EulerAngles euler_rad)
{
EulerAngles euler_deg;
euler_deg.yaw = BSP_rad_to_deg(euler_rad.yaw);
euler_deg.pitch = BSP_rad_to_deg(euler_rad.pitch);
euler_deg.roll = BSP_rad_to_deg(euler_rad.roll);
return euler_deg;
}
/*******************************辅助功能函数*****************************/
互补滤波
- 读取传感器初始值：三轴角速度(wx,wy,wz)、三轴加速度(ax,ay,az)
- 提取加速度姿态的四元数
- 加速度低通滤波
- 归一化加速度:->重力向量
- 三轴加速度计算俯仰角(pitch)和横滚角(roll)： pitch_acc = atan2(ay, sqrt(ax² + az²)) roll_acc = atan2(-ax, az)
- 将偏航角设为0，与俯仰角、横滚角转换为四元数
- 或者使用直接构建法:
- 陀螺仪更新姿态四元数
- 使用四元数微分方程：dq/dt = 0.5 * q ⊗ ω
- 或使用旋转增量四元数：q_new = q ⊗ Δq
- 四元数域互补滤波 四元数融合方法：球面线性插值(SLERP) q_fused = slerp(q_gyro, q_accel, 1-α)
- 归一化最终四元数
/**********************************互补滤波融合法***********************************/
/**
* @description: 第二代互补滤波
* @param {Quaternion*} gyro_q 提供初始的四元数的指针
* @param {IUN_OriginalData} ium 角速度 弧度/秒 三轴加速度 m/s2
* @param {float} alpha 滤波系数
* @param {float} dt 间隔时间
* @return {Quaternion} 当前状态四元数
*/
Quaternion IUM_6_Complementary_filtering_2(Quaternion *q, IUN_OriginalData ium, float alpha, float dt)
{
//归一化加速度->重力向量
AccelerationData Acc= normalize_acceleration_2(ium.Acc);
// 使用归一化加速度数据计算欧拉角
EulerAngles Acc_ol= compute_euler_angles_from_normalized_accel(Acc);
// 欧拉角 → 四元数 (ZYX顺序)
Quaternion q_Acc = euler_to_quaternion(Acc_ol) ;
Quaternion q_Gyr = *q;
// 增量旋转方法：q(t+dt) = q(t) * Δq（精确方法）
q_Gyr = integrate_angular_velocity_incremental(q_Gyr,ium.Gyr,dt);
// 归一化四元数
q_Gyr = normalize_quaternion(q_Gyr) ;
Quaternion q_;
// 四元数球面线性插值
q_ = quaternion_slerp(q_Gyr, q_Acc, alpha);
*q=q_;
return q_;
}
EulerAngles IUM_6_Complementary_filtering_2_to_euler(Quaternion *q, IUN_OriginalData ium, float alpha, float dt)
{
*q=IUM_6_Complementary_filtering_2(q,ium,alpha,dt);
return quaternion_to_euler(*q);
}
/**********************************互补滤波融合法***********************************/
Mahony滤波
- 读取三轴角速度（m/s）、三轴加速度（度/秒）
- 提取重力向量
- 加速度滤波
- 归一化三轴加速度->实际重力向量a_measured_normalized
- 获取当前姿态四元数
- 从上一次迭代继承(步骤8)或初始值
- 提取前姿态四元数的预测重力向量
- （v' = q ⊗ v ⊗ q*）世界坐标系中旋转到物体坐标系
// 使用四元数 q = [q0, q1, q2, q3]
vx = 2.0f * (q1*q3 - q0*q2);
vy = 2.0f * (q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
- 计算预测重力向量与实际重力向量的误差
- error = cross(g_predicted, a_measured_normalized)//cross为求叉积函数
cross(v1, v2):
x = v1[1]*v2[2] - v1[2]*v2[1]
y = v1[2]*v2[0] - v1[0]*v2[2]
z = v1[0]*v2[1] - v1[1]*v2[0]
return [x, y, z]
- 校正角速度
- 比例控制gyro_correction_p = Kp × error
- 积分控制gyro_correction_i += Ki × error × dt //dt:时间间隔
- gyro_correction=gyro_correction_p+gyro_correction_i
- 校正corrected_gyro = gyro_measured（陀螺仪测量值） + gyro_correction
- 用校正后的角速度更新四元数
- 使用四元数微分方程：
- 使用旋转增量四元数：q_new = q ⊗ Δq
- 归一化最终四元数
注：叉积包含了一个向量旋转到另一个向量的旋转轴、旋转方向和旋转角度可以简单理解需要绕哪个轴、用多大力气旋转身体来纠正姿态偏差。 注：v' = q ⊗ v ⊗ q* 是[[航天惯例中使用四元数经行坐标系转换]]的公式
从流程中我们可以看到，Mahony滤波其实就是一个将计算单位改为向量的PID控制器。
Quaternion IUM_6_Mahony_filtering_1(Quaternion *q, IUN_OriginalData ium, float Kp, float Ki, float dt)
{
static VectorQuantity3D integral_error = {0, 0, 0}; // 积分误差
// 1. 归一化加速度
AccelerationData Acc = normalize_acceleration_2(ium.Acc);
VectorQuantity3D VQ_Acc = {Acc.ax, Acc.ay, Acc.az};
// 2. 获取预测的重力向量（使用修正的公式）
VectorQuantity3D VQ_Gyr = QuaternionToPredictedGravity(*q);
// 3. 计算误差（修正叉积顺序）
VectorQuantity3D VQ_Err = Vector_CrossProduct(VQ_Acc,VQ_Gyr);
// 4. 校正角速度（比例+积分控制）
VectorQuantity3D VQ_angular_velocity = (*((VectorQuantity3D*)&ium.Gyr));
// 比例项
VectorQuantity3D VQ_correction = Vector_ScalarMultiply(VQ_Err, Kp);
// 积分项
integral_error = Vector_Add(integral_error, Vector_ScalarMultiply(VQ_Err, dt));
VectorQuantity3D VQ_integral_correction = Vector_ScalarMultiply(integral_error, Ki);
// 综合校正
VQ_correction = Vector_Add(VQ_correction, VQ_integral_correction);
VQ_angular_velocity = Vector_Add(VQ_angular_velocity, VQ_correction);
// 5. 更新四元数
AngularVelocity Angular = (*((AngularVelocity*)&VQ_angular_velocity));
*q = integrate_angular_velocity_incremental(*q, Angular, dt);
// 6. 确保归一化
*q = normalize_quaternion(*q);
return *q;
}
Quaternion IUM_6_Mahony_filtering_2(Quaternion *q, IUN_OriginalData ium, float Kp, float Ki, float dt)
{
static VectorQuantity3D integral_error = {0, 0, 0}; // 积分误差
const float integral_limit = 0.9f; // 积分限幅
// 1. 归一化加速度
AccelerationData Acc = normalize_acceleration_2(ium.Acc);
VectorQuantity3D VQ_Acc = {Acc.ax, Acc.ay, Acc.az};
// 2. 获取预测的重力向量（使用修正的公式）
VectorQuantity3D VQ_Gyr = QuaternionToPredictedGravity(*q);
// 3. 计算误差（修正叉积顺序）
VectorQuantity3D VQ_Err = Vector_CrossProduct(VQ_Acc,VQ_Gyr);
// 4. 校正角速度（比例+积分控制）
VectorQuantity3D VQ_angular_velocity = (*((VectorQuantity3D*)&ium.Gyr));
// 比例项
VectorQuantity3D VQ_correction = Vector_ScalarMultiply(VQ_Err, Kp);
// 积分项（带限幅）
integral_error = Vector_Add(integral_error, Vector_ScalarMultiply(VQ_Err, dt));
if (Vector_Magnitude(integral_error) > integral_limit) {
integral_error = Vector_ScalarMultiply(
Vector_Normalize(integral_error),
integral_limit
);
}
VectorQuantity3D VQ_integral_correction = Vector_ScalarMultiply(integral_error, Ki);
// 综合校正
VQ_correction = Vector_Add(VQ_correction, VQ_integral_correction);
VQ_angular_velocity = Vector_Add(VQ_angular_velocity, VQ_correction);
// 5. 更新四元数
AngularVelocity Angular = (*((AngularVelocity*)&VQ_angular_velocity));
*q = integrate_angular_velocity_incremental(*q, Angular, dt);
// 6. 确保归一化
*q = normalize_quaternion(*q);
return *q;
}
Madgwick滤波
- 读取三轴角速度（m/s）、三轴加速度（度/秒）
- 提取重力向量：滤波->归一化->重力向量(a_normalized )
- 获取当前姿态四元数
- 从上一次迭代继承当前的四元数
- 或使用初始值
- q_current = [w, x, y, z] # 初始值通常为 [1, 0, 0, 0]
- 获取预测重力向量与测量重力向量只差
- 获取预测重力向量：v' = q ⊗ v ⊗ q*(v=[0,0,1])
g_predicted = [2*(x*z - w*y), 2*(w*x + y*z),1 - 2*(x*x + y*y)]f(q) = g_predicted - a_normalized
- 计算预测重力向量的雅可比矩阵
J = [
[-2*y, 2*z, -2*w, 2*x],
[ 2*x, 2*w, 2*z, 2*y],
[ 0, -4*x, -4*y, 0]
]
这是关于g_predicted的雅可比矩阵
- 计算梯度下降步长
- 梯度 ∇f = J^T × f(q) 梯度是一个向量，指向函数值增长最快的方向，在Madgwick滤波器中的梯度就是指向误差增长最快的方向。
- 步长 = β × (∇f / ||∇f||) 步长决定沿着梯度方向走多远。
- 陀螺仪陀螺仪跟新(q_gyro)
- 使用四元数微分方程：
- 使用旋转增量四元数：q_gyro = q ⊗ Δq
- 应用梯度下降校正
- q_corrected = q_gyro - 步长 × dt 注：这里的步长是减去，意味着指向误差增长最快的反方向
- 归一化四元数
Quaternion IUM_6_Madgwick_filtering_1(Quaternion *q, IUN_OriginalData ium, float beta, float dt)
{
//加速度归一化
AccelerationData Acc = normalize_acceleration_2(ium.Acc);
//得到重力向量
VectorQuantity3D VQ_Acc = {Acc.ax, Acc.ay, Acc.az};
//从当前四元数预测机体坐标系中的重力向量
VectorQuantity3D VQ_Gyr = QuaternionToPredictedGravity(*q);
//预测重力与实际测量重力之间的误差
VectorQuantity3D VQ_Err = Vector_Subtract(VQ_Gyr,VQ_Acc);
//目标函数关于四元数的雅可比矩阵
QuaternionToPredictedGravity_JacobiMatrix J_m3X4 = calculateJacobianMatrix(*q);
//计算梯度
Quaternion gradient=calculateGradient(J_m3X4,VQ_Err) ;
//归一化梯度
Quaternion gradient_norm=normalize_quaternion(gradient);
//计算梯度下降步长
Quaternion step=quaternionMultiplyScalar(gradient_norm,beta) ;
//陀螺仪积分四元数
// 增量旋转方法：q(t+dt) = q(t) * Δq（精确方法）
Quaternion q_gyro= integrate_angular_velocity_incremental(*q,ium.Gyr,dt);
//归一化四元数
q_gyro=normalize_quaternion(q_gyro);
//应用梯度下降校正
Quaternion step_ = quaternionMultiplyScalar(step,dt);
Quaternion q_corrected = quaternionSubtract(q_gyro,step_) ;
//归一化四元数
q_corrected=normalize_quaternion(q_corrected);
*q=q_corrected;
return q_corrected;
}
/**
* @brief 计算从四元数到预测重力向量的雅可比矩阵
* @param q 输入四元数
* @return 雅可比矩阵（3x4）
*/
QuaternionToPredictedGravity_JacobiMatrix calculateJacobianMatrix(Quaternion q)
{
QuaternionToPredictedGravity_JacobiMatrix jacobian;
// 根据公式设置雅可比矩阵
// 第一行：对应重力向量的x分量对四元数的偏导
jacobian.J[0][0] = -2 * q.y; // g_x/w
jacobian.J[0][1] = 2 * q.z; // g_x/x
jacobian.J[0][2] = -2 * q.w; // g_x/y
jacobian.J[0][3] = 2 * q.x; // g_x/z
// 第二行：对应重力向量的y分量对四元数的偏导
jacobian.J[1][0] = 2 * q.x; // g_y/w
jacobian.J[1][1] = 2 * q.w; // g_y/x
jacobian.J[1][2] = 2 * q.z; // g_y/y
jacobian.J[1][3] = 2 * q.y; // g_y/z
// 第三行：对应重力向量的z分量对四元数的偏导
jacobian.J[2][0] = 0; // g_z/w
jacobian.J[2][1] = -4 * q.x; // g_z/x
jacobian.J[2][2] = -4 * q.y; // g_z/y
jacobian.J[2][3] = 0; // g_z/z
return jacobian;
}
Quaternion calculateGradient(QuaternionToPredictedGravity_JacobiMatrix jacobian,VectorQuantity3D error_vector)
{
Quaternion gradient;
// 梯度 ?f = J^T × error_vector
// 由于 J 是 3x4，J^T 是 4x3，error_vector 是 3x1
// 结果是 4x1 向量
// 计算梯度向量的每个分量
// 第一个分量 (对应四元数的w分量)
gradient.w = jacobian.J[0][0] * error_vector.x + // J[0][0] * VQ_q.x
jacobian.J[1][0] * error_vector.y + // J[1][0] * VQ_q.y
jacobian.J[2][0] * error_vector.z; // J[2][0] * VQ_q.z
// 第二个分量 (对应四元数的x分量)
gradient.x = jacobian.J[0][1] * error_vector.x + // J[0][1] * VQ_q.x
jacobian.J[1][1] * error_vector.y + // J[1][1] * VQ_q.y
jacobian.J[2][1] * error_vector.z; // J[2][1] * VQ_q.z
// 第三个分量 (对应四元数的y分量)
gradient.y = jacobian.J[0][2] * error_vector.x + // J[0][2] * VQ_q.x
jacobian.J[1][2] * error_vector.y + // J[1][2] * VQ_q.y
jacobian.J[2][2] * error_vector.z; // J[2][2] * VQ_q.z
// 第四个分量 (对应四元数的z分量)
gradient.z = jacobian.J[0][3] * error_vector.x + // J[0][3] * VQ_q.x
jacobian.J[1][3] * error_vector.y + // J[1][3] * VQ_q.y
jacobian.J[2][3] * error_vector.z; // J[2][3] * VQ_q.z
return gradient;
}
扩展卡尔曼滤波
由于扩展卡尔曼滤波比较复杂，且四元数的乘法为双线性与卡尔曼的条件不符。因此这部分只简单列举一下它的公式与重点。
模型建立：
状态方程：X_t = f(X_t-1, u_t, W_t)
观测方程：Y_t = h(X_t) + V_t
-------------------------------------------
在最优估计点 X_t-1^ & W_t=0处对状态方程进行一阶泰勒展开：
X_t ≈ f(X_t-1^, u_t, 0) + F_t * (X_t-1 - X_t-1^) + L_t * W_t
其中：
F_t = ∂f/∂X |_{X=X_t-1} （状态转移雅可比矩阵）
L_t = ∂f/∂W |_{W=0} （过程噪声雅可比矩阵）
在预测状态 X_t~ & V_t=0处对观测方程进行一阶泰勒展开：
Y_t ≈ h(X_t~, 0) + H_t * (X_t - X_t~) + M_t * V_t
其中：
H_t = ∂h/∂X |_{X=X_t~} （观测雅可比矩阵）
M_t = ∂h/∂V |_{V=0} （观测噪声雅可比矩阵）
--------------------------------------------
W_t → N(0:Q_t)
V_t → N(0:R_t)
其中：Y_t = z_t
扩展卡尔曼滤波：
预测：
X_t~ = f(X_t-1, u_t, 0)
P_t~ = F_t * P_t-1 * F_t^T + L_t * Q_t * L_t^T
更新：
K_t = P_t~ * H_tᵀ * (H_t * P_t~ * H_t^T + R_t)^(-1)
X_t = X_t~ + K_t * (z_t - h(X_t~))
P_t = (I - K_t * H_t) * P_t~
通过观测我们可以发现，整个公式可以分为两部分：
- 模型建立
- 状态方程
- 观测方程
- 扩展卡尔曼滤波
- 预测
- 更新 其中最为困难的部分便是模型建立，以及将模型转换为状态方程与观测方程。其中的雅可比矩阵中的偏导一个也不能错。
接下来我们来看看扩展卡尔曼的观测方程与观测残差部分：
- 观测方程
- h(X_t)=v’ = q ⊗ v ⊗ q*：世界坐标 → 机体坐标(v=[0,0,1],q=[q0,q1,q1,q3])
- vx=2(q1q3-q0q2)
- vy=2(q2q3+q0q1)
- vz=1-2(q1^2+q2^2)
- 观测残差
- 状态向量X_t：通常是7维(四元数+3维陀螺仪零偏) [q0,q1,q2,q3,bx,by,bz]
- 观测向量z_t：3维加速度计测量 [ax,ay,az]
- 残差：(z_t - h(X_t~))：测量重力向量与预测重力向量之差
通过观察残差部分我们我们可以发现扩展卡尔曼滤波同样是使用两个重力向量之差来调整状态向量，只不过调整的力度是由卡尔曼增益来决定的，本质与Madgwick滤波、Mahony滤波是相同的。
总结
通过对扩展卡尔曼滤波、Madgwick滤波、Mahony滤波方法的分析我们可以总结出姿态融合的大致流程：
- 处理元素数据
- 角速度：统一单位
- 加速度：归一化重力
- 观测
- 获取上一次或初始四元数
- 通过四元数预测重力向量
- 获取预测重力向量与实际向量误差
- 校正
- 通过误差校正角速度
- EKF：通过卡尔曼增益直接更新状态向量(其实是通过状态转移矩阵跟新状态向量中的四元数)
- 姿态输出
- 归一化四元数
- 可转换为欧拉角等格式
互补滤波方法姿态融合的大致流程：
- 处理元素数据
- 角速度：统一单位
- 加速度：归一化重力向量
- 姿态融合
- 获取上一次或初始四元数
- 通过角速度跟新四元数G
- 获取归一化重力向量
- 使用重力向量构建四元数A
- 使用球面线性插值(SLER)融合四元数G、四元数A得到新四元数
