六轴EKF四元数姿态解算：从原理到源码实现与调参

简介：面向无人机、机器人及惯性导航等领域的开发者，这套源码以 Python 语言实现了基于扩展卡尔曼滤波（EKF）的四元数姿态解算算法，适用于六轴传感器（加速度计与陀螺仪）数据融合，可帮助理解如何通过滤波递推将角速度与加速度观测转化为稳定的四元数姿态估计。资源包为 zip 压缩格式，体积仅 2KB，包含 2 个文件：EKF.py 为核心算法实现，负责状态预测、观测更新与四元数归一化；.gitignore 为工程辅助配置，便于纳入版本管理。压缩包目录结构简洁，代码量小但算法模块完整，涵盖了六轴姿态解算的预测与更新核心流程，既可用于课程设计与毕业设计的算法验证，也可作为移植到嵌入式平台的参考基线。已有 289 人学习并下载，对于想要快速掌握 EKF 在姿态解算中应用原理的读者，是一份高性价比的入门资料。

1. 为什么六轴 EKF 四元数姿态解算值得自己写一套源码

如果你在 STM32 上做过 MPU6050 的姿态解算，大概率是从互补滤波或 Mahony 起步的：代码短、调试快、四轴悬停够用。但一旦把板子装进穿越机或者机械臂，大机动和振动会让固定系数的互补滤波明显滞后，这时候你想的已经不是“补得稳”，而是“能不能在传感器噪声大时自动降低权重、在真实转动时快速跟上去”。这就是扩展卡尔曼滤波（EKF）和四元数姿态解算的组合价值所在：状态量保持单位四元数，不碰欧拉角的万向锁，量测只依赖加速度计和陀螺仪（六轴），不依赖磁力计。而这个标题里的源码包，本质就是把这套算法拆成可移植的 C 模块，而不是让你去啃一堆论文公式。下面会顺着 EKF 的预测、更新、源码组织、参数设置和精度验证走一遍，让你拿到手能改参数，而不是只会跑例程。

2. 四元数与 EKF 主线：状态方程、量测方程和雅可比矩阵怎么搭

写 EKF 之前，先把“四元数作状态”和“六轴量测”这两层关系钉死。四元数的好处不只是避开欧拉角的万向锁，更重要的是旋转运算是线性矩阵乘形式，对雅可比矩阵构造和代码实现都很友好。这一章把预测和更新用到的数学骨架写清楚，后面源码拆解时你才能看懂每一行在算什么。

2.1 四元数姿态表示：单位范数约束是 EKF 的隐藏边界

四元数q = [q0 q1 q2 q3]表示从参考系（通常取 NED 或 NEU）到机体系的旋转，范数恒为 1。对应旋转矩阵有很多排法，源码里用的通常是世界系到机体系的矩阵：

R_ib(q) = [[1-2(q2²+q3²), 2(q1q2-q0q3), 2(q1q3+q0q2)], [2(q1q2+q0q3), 1-2(q1²+q3²), 2(q2q3-q0q1)], [2(q1q3-q0q2), 2(q2q3+q0q1), 1-2(q1²+q2²)]]

这个矩阵会把世界系下的向量转到机体系，后面加速度计量测模型用的就是它的第三列。EKF 状态更新后一定要重新归一化四元数，否则范数漂移会直接放大加速度计量测残差，导致姿态慢慢滑向错误方向。这是把四元数原理落地到源码时最容易忽略的一步，也是很多“EKF 跑一阵子就偏 10 度”问题的根源。

2.2 预测步：陀螺仪角速度驱动下的离散化与雅可比 F_k

连续时间四元数微分方程写成矩阵形式：

dq/dt = 0.5 * Ω(w) * q

其中w是机体系三轴角速度，已经减去陀螺偏置b_g，Ω(w)是 4×4 反对称矩阵：

Ω(w) = [[0, -wx, -wy, -wz], [wx, 0, wz, -wy], [wy, -wz, 0, wx], [wz, wy, -wx, 0 ]]

如果状态只取四元数，系统就是 4 维；但六轴 EKF 源码里一般会把陀螺零偏也放进状态向量，变成 7 维：x = [q0 q1 q2 q3 bx by bz]。陀螺偏置按随机游走建模，离散化后就是b_g(k+1) = b_g(k)。

离散化方式上，源码里常见两种：前向欧拉和零阶保持（ZOH）。前向欧拉直接把微分写成差分：

q(k+1) = (I + 0.5*Ω(w)*Δt) * q(k)

ZOH 则是把角速度在 Δt 内看成常量，用四元数指数映射精确积分：

Δθ = |w| * Δt q(k+1) = q(k) ⊗ [cos(Δθ/2); sin(Δθ/2) * w/|w|]

当角速度变化剧烈时，前向欧拉会引入不可忽略的积分误差，ZOH 更适合无人机这类动态强的场合。代价是雅可比矩阵 F_k 的表达复杂一些。许多实现为了能写递推式，会取一阶近似：

F_k = I(7x7) + [[0.5*Ω(w)*Δt, -0.5*Γ(q)*Δt], [0(3x4), I(3x3) ]]

其中Γ(q)是四元数动态对偏置的耦合项，源码里通常会数值差分或干脆忽略掉非对角块。忽略后，预测协方差更新会略偏乐观，但实测只要 Q 里给偏置一点点噪声就能掩盖。

// ekf_predict: 7 维状态的一步预测，使用一阶欧拉雅可比 void ekf_predict(ekf_t *ekf, const float gyro[3], float dt) { // 去掉陀螺偏置 float wx = gyro[0] - ekf->bias[0]; float wy = gyro[1] - ekf->bias[1]; float wz = gyro[2] - ekf->bias[2]; // 1) 四元数前向欧拉更新 float q[4]; q[0] = ekf->x[0] + 0.5f * dt * (-wx*ekf->x[1] - wy*ekf->x[2] - wz*ekf->x[3]); q[1] = ekf->x[1] + 0.5f * dt * ( wx*ekf->x[0] + wz*ekf->x[2] - wy*ekf->x[3]); q[2] = ekf->x[2] + 0.5f * dt * ( wy*ekf->x[0] - wz*ekf->x[1] + wx*ekf->x[3]); q[3] = ekf->x[3] + 0.5f * dt * ( wz*ekf->x[0] + wy*ekf->x[1] - wx*ekf->x[2]); // 2) 归一化四元数 float norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); for (int i = 0; i < 4; i++) ekf->x[i] = q[i] / norm; // 3) 陀螺偏置随机游走，状态值不变 for (int i = 0; i < 3; i++) ekf->x[4+i] = ekf->bias[i]; // 4) 协方差预测 P = F P F^T + Q，由 build_F_matrix() 完成 // F 的构造在源码里单独一个函数，避免主循环过长 }

代码里最关键的是前两步：wx/wy/wz必须先减偏置，顺序颠倒会引入固定角速率误差；四元数更新后立刻归一化，这比把归一化放到量测更新之后更安全。如果换成 ZOH，q更新和 F 矩阵都要换成指数映射版本，代码量多 30 行左右，建议保留两种宏定义方便切换数据源对比。注意这里 F 的一阶近似只适合 Δt 在 5ms 左右的场景，如果采样率降到 50Hz，还是要写完整的指数映射雅可比。

2.3 量测模型：加速度计观测的是重力在机体坐标上的投影

六轴 EKF 的量测通常只取加速度计三轴。忽略机体线加速度时，加速度计输出经归一化后等于重力方向在机体系上的投影：

h(q) = [2(q1q3 - q0q2); 2(q2q3 + q0q1); q0² - q1² - q2² + q3²]

这个 h 向量在q=[1,0,0,0]时恰好等于[0,0,1]，与传感器静止平放时 z 轴读数 +1g 对应。如果电路板装配方向不同，在.h里留一个ACC_SIGN配置即可，不要为了对齐符号去改矩阵公式。

量测残差是z - h(x)，其中 z 是归一化后的加速度计读数。由于 h 是 q 的非线性函数，需要雅可比矩阵 H_k，形态是 3×7，偏置列全零：

H(0,:) = [-2q2, 2q3, -2q0, 2q1, 0,0,0] H(1,:) = [ 2q1, 2q0, 2q3, 2q2, 0,0,0] H(2,:) = [ 2q0,-2q1, -2q2, 2q3, 0,0,0]

# 构造加速度计观测雅可比，输入四元数 [q0 q1 q2 q3]，输出 3x7 的 H import numpy as np def build_acc_h_jacob(q): q0, q1, q2, q3 = q H = np.zeros((3, 7)) H[0, :4] = [-2*q2, 2*q3, -2*q0, 2*q1] H[1, :4] = [ 2*q1, 2*q0, 2*q3, 2*q2] H[2, :4] = [ 2*q0, -2*q1, -2*q2, 2*q3] return H

这里 H 的前 4 列是四元数部分的导数，后 3 列对应陀螺偏置，因为加速度计不直接观测偏置所以是 0。更新方程用标准 EKF 公式：S = H P H^T + R，K = P H^T S^-1，x = x + K * residual，P = (I - K H) P。注意四元数更新后仍要归一化。写到这里，公式层面的东西就齐了。真正让 EKF 跑不动的，往往是后面要说的 Q/R 初值和协方差非正定，这些在源码调试时比公式更致命。

3. 源码拆解：一个可移植的六轴四元数 EKF 模块怎么组织

3.1 模块划分：把四元数、矩阵和 EKF 分开写

拿到源码包，先看的不是ekf.c，而是文件边界。常见结构是：

这样拆的原因很简单：四元数运算是纯数学，可以单独做单元测试；EKF 更新里的矩阵求逆只涉及 3×3 或 7×7，不需要引入通用线性代数库，自己写一个小矩阵库能减少单片机内存占用。很多开源飞控把一堆运算塞在一个文件里，看着紧凑，换芯片就要整体重写。模块化之后，在 PC 上仿真时只需要替换mpu6050_driver.c，算法代码一行都不用动。

3.2 EKF 数据结构与内存布局

C 语言实现里我最常用的是这种扁平化结构：

typedef struct { float x[7]; // 状态：q0..q3 + gyro_bias[3] float P[7*7]; // 协方差矩阵，行优先存储 float Q[7*7]; // 过程噪声协方差 float R[3*3]; // 加速度计量测噪声协方差 float dt; // 采样周期，由调用方传入 } ekf_t;

P 全部按行优先展开，避免二维数组动态分配带来的碎片化。x 里前 4 个永远是归一化后的四元数，后 3 个是陀螺偏置（单位 rad/s）。初始化时x = [1,0,0,0,0,0,0]，P 取一个较小的对角阵P0 = diag(1e-3,...,1e-4)，偏置部分给 1e-4，表示先验上不信任偏置初值。这里有个常见误用：P0 给太大比如 diag(1)，会导致最开始几百毫秒的估计剧烈抖动，因为卡尔曼增益被初值协方差放大了。

3.3 预测与更新主流程：一个周期内调用的三个函数

每个传感器循环里，代码按下面顺序执行：

void attitude_loop(void) { imu_read(&acc, &gyro); // 1. 读取原始数据 apply_gyro_bias_calibration(gyro); // 2. 上电零偏粗校准 // 3. gyro 转成 rad/s // 4. acc 转成 m/s^2 并归一化 ekf_predict(&ekf, gyro, dt); // 5. 预测 ekf_update_acc(&ekf, acc_norm); // 6. 加速度计量测更新 quat_to_euler(ekf.x, &roll, &pitch, &yaw); }

ekf_update_acc内部实现的是残差和卡尔曼增益计算，注意残差要处理加速度计的方向符号。如果你在静止状态下看到横滚或俯仰角稳定地偏 20 度，不要急着调 Q/R，先去检查h(q)算出来的重力方向是不是和acc同号。这是所有 EKF 姿态源码中最常见的“看起来没 bug 但姿态反着”的问题。另一个容易踩的坑是 dt 不恒定：定时器回调里直接拿1/freq当 dt，但中断偶尔被串口阻塞，实际时间漂移后 Q 矩阵散化失真。建议每次进回调时读一次硬件计数器，把真实间隔传给ekf_predict。

3.4 对接 MPU6050：量程、采样率和符号约定

六轴姿态用的传感器还是以 MPU6050 这类消费级 IMU 为主。加速度计量程建议选 ±4g 或 ±8g，陀螺仪 ±1000 dps，因为 EKF 的观测模型假设加速度计只测重力，量程太大或太小都会压缩有效位。采样率一般取 200 Hz，EKF 的 dt 必须与此一致；如果主循环抖动超过 10%，可考虑读取 DWT 计时器计算实际 dt，否则 Q 矩阵的离散化会失真。

符号约定上，MPU6050 的加速度计寄存器原始值是补码，转成 m/s² 的公式是a = raw / 16384 * 9.80665（±2g 时）。陀螺仪w = raw / 131.0 * pi/180（±250 dps 时）。如果板子的安装方向让acc与h(q)差了一个负号，在ekf_update_acc里对对应轴取反，不要改 H 和 R，方便维护。偏置校准建议上电后静止采集 200 帧陀螺数据取平均，直接减去这个平均值，比让 EKF 的偏置状态慢慢收敛更快。

4. EKF 参数怎么调：Q、R、P0 对收敛和稳态的影响

4.1 先造一条带真值的仿真数据

调参最怕的是在真机上不知道理想姿态是什么。常见做法是先用脚本生成一条带噪声的六轴数据，同时保留真值四元数，然后跑同一套 C 代码的仿真版。下面是用 Python 生成数据的思路：

import numpy as np dt = 0.005 t = np.arange(0, 10, dt) q = np.zeros((len(t), 4)) q[0] = [1, 0, 0, 0] w_true = np.zeros((len(t), 3)) w_true[400:800] = [0, 0.8, 0] # 第2~4秒绕y轴以0.8rad/s转 for k in range(1, len(t)): w = w_true[k] omega = 0.5*np.array([[0, -w[0], -w[1], -w[2]], [w[0], 0, w[2], -w[1]], [w[1], -w[2], 0, w[0]], [w[2], w[1], -w[0], 0]]) q[k] = q[k-1] + dt * omega @ q[k-1] q[k] /= np.linalg.norm(q[k]) # 生成量测：把重力向量旋转到机体系并加噪声 acc_meas = apply_rotation(q, [0, 0, 9.80665]) + np.random.normal(0, 0.05, 3) gyro_meas = w_true + np.random.normal(0, 0.005, 3) np.savetxt("imu_sim.csv", np.hstack([t[:, None], gyro_meas, acc_meas]), delimiter=",")

代码先手动积分真值四元数，保证有基准；加速度计观测方向与 2.3 节h(q)保持一致；陀螺仪给 0.005 rad/s 量级的噪声，接近 MPU6050 的实测水平。生成 CSV 后，把你的 EKF 主循环改成从 CSV 读数据、每次都返回四元数误差，调参效率会高很多。注意这里apply_rotation要自己实现，方向必须和你的h(q)一致，否则仿真结论会反过来。

4.2 四个必调参数：Q_gyro、Q_bias、R_acc、P0

这些量纲看起来很小，是因为四元数误差本身远小于 1 弧度。注意 H 是 3×7，残差单位是归一化后的 g，R_acc 的单位是归一化 g 的平方，所以 R 取 0.01 表示加速度计每个轴有约 0.1 g 的标准差，这在电机振动下是合理值。Q 的单位则与 dt 有关，前向欧拉和 ZOH 对 Q 的敏感度不同：使用 ZOH 时角速度积分更准确，可以给更小的 Q_gyro。

4.3 发散、震荡和慢响应：怎样从曲线反推参数问题

拿到跑完的曲线，按症状对号入座。静态时四元数缓慢漂移，多半是 Q_bias 太小或陀螺零偏没校干净；大机动后姿态要 2 秒才回来，把 Q_gyro 提高 5 倍或把 R_acc 降一半；悬停时姿态高频发抖，优先降低 Q_gyro 而不是降低 R_acc。还有一种隐蔽情况：协方差 P 因为反复归一化 q 而不再非负定，表现为更新几次后增益变成 NaN。源码里可以在ekf_update_acc结束后对 P 做一次对称化P = (P + P^T)/2，同时检查对角线元素是否小于 0，一旦小于 0 就该考虑是不是 dt 设错成负数。

5. 精度验证与两个进阶实用技巧：R 自适应和从四维到七维

5.1 验证姿态精度的最小配置

跑源码时先不要在屏幕上画一堆曲线，直接在串口以 10 Hz 打印 roll/pitch/yaw 和四元数模长。模长与 1.0 的偏差超过 1e-3，说明归一化位置不对；静止 2 分钟后偏置估计应稳定在 ±0.01 rad/s 内。用 Python 的 matplotlib 实时画图，能看到加速度计更新瞬间的协方差抖动，这是判断卡尔曼增益是否合理的直接证据。画图时把互补滤波的结果和 EKF 叠一起看，大机动段落 EKF 的滞后量应明显小于互补滤波。

5.2 加速度计残差触发的动态 R 自适应

六轴 EKF 最大的敌人是线加速度。电机加速或刹车瞬间，加速度计读数不再是重力方向，此时应调大 R_acc。常见做法是用加速度计模长偏离 1g 的程度调节：

float norm = sqrtf(ax*ax + ay*ay + az*az); float lambda = fabsf(norm - 1.0f); // 归一化后 1g=1 float R_adapt = R_base * (1.0f + 10.0f * lambda * lambda); ekf.R[0] = ekf.R[4] = ekf.R[8] = R_adapt;

lambda在振动下约为 0.05~0.2，10 倍系数已经足够压制错误的量测方向；如果把系数调到 100，EKF 会完全退回纯陀螺积分，姿态漂移又会回来。这个自适应逻辑是六轴 EKF 相对互补滤波最大的优势，原理是把“对传感器的信任度”写成了可计算的函数。

5.3 把四维状态升到七维：陀螺偏置估计的实现要点

如果你拿到的源码初始版本是四维状态（只有四元数），升级七维时只需要改三处：状态向量长度改成 7，F 矩阵右上角补上-0.5*Γ(q)*Δt的耦合块，H 矩阵后 3 列补零。耦合块可以先用数值雅可比生成，再用编译期assert校验F*P的对称性。升级后静止 10 分钟，yaw 纹波比四维版明显更小，这就是偏置被观测到的直接收益。之后想再加磁力计变九轴，也只是在量测方程里多拼一列磁场投影，结构和这里完全一致。

本文还有配套的精品资源，点击获取