简介基于扩展卡尔曼滤波的姿态估计压缩包面向需要融合陀螺仪与加速度计数据的开发者或学生适用于无人机、机器人及移动设备的姿态解算场景。包内共包含三个脚本文件m文件大小仅2KB结构精炼便于阅读和二次修改。代码按照EKF标准流程组织主脚本完成状态预测与观测更新辅助函数分别负责航向角计算以及姿态航向参考系统的初始化。实现中充分利用加速度计修正俯仰和滚转角度针对陀螺仪长期积分产生的漂移给出了引入磁力计数据并做磁偏角校正来获取稳定航向的有效思路。读者可以通过这个完整示例理解非线性系统状态方程与观测方程的构建方式学习多传感器融合中协方差矩阵设置、初始姿态确定等环节快速建立工程化认识。资源已有1568人学习下载适合具备一定MATLAB基础、希望系统掌握EKF融合算法的实践者。1. EKF 融合陀螺仪和加速度计先看数据再看算法做姿态解算的人基本都经历过这个疑惑陀螺仪积分漂移快加速度计噪声大单独用哪一个都不行。互补滤波是常用解法但它在载体做高速旋转或加减速时会把加速度计的线加速度当成重力输出一个“看起来很平滑但实际已经偏了”的姿态。EKF 融合陀螺仪和加速度计的思路是把陀螺仪的角速度建模成状态预测把加速度计的重力投影建模成观测用卡尔曼滤波的框架去权衡两者谁更可信。这个权衡不是靠经验调 α 和 β而是靠协方差矩阵的数学含义去表达。EKF 强于互补滤波的核心优势在于它能扩展到带磁力计、带 GPS 速度观测的更高维系统一套框架可以持续复用。本文只锁定 6 轴融合这一个场景用 MPU6050 的实际参数做例子从模型推导、代码实现到参数调优把 EKF 融合陀螺仪和加速度计这条路走通。适合看这篇文章的读者有两类一类是刚接触姿态解算、想知道互补滤波之外还有哪些更稳方案的嵌入式工程师另一类是已经在跑里面程序但发现姿态会漂、会抖想知道到底是模型的锅还是参数的锅。这两种情况读完都能找到对应的排查路径。2. EKF 的数学模型四元数状态方程和加速度观测方程在用 EKF 之前先把姿态表示选型讲清楚。欧拉角有万向锁问题旋转矩阵参数冗余四元数是 6 轴姿态解算里最常用的状态量。一个单位四元数q [qw, qx, qy, qz]表示载体从机体坐标系到导航坐标系的旋转它的导数方程为dq/dt 0.5 * q ⊗ ω其中 ω 是陀螺仪测得的机体角速度rad/s⊗表示四元数乘法。这个方程是线性的但系统状态转移矩阵依赖当前时刻的角速度输入所以整个系统呈现非线性特性这正是需要用 EKF 而非标准卡尔曼滤波的原因。2.1 陀螺仪动态模型角速度是输入不是状态把陀螺仪的角速度作为控制输入或者叫外生输入是 6 轴融合里最常见的建模范式。状态向量就是四元数的四个分量那么状态方程可以写成q(k) exp(0.5 * Ω(ω) * Δt) * q(k-1)也就是四元数指数映射形式。如果你的应用对计算资源要求苛刻、又跑的是低算力 MCU也可以用一阶近似q(k) ≈ q(k-1) 0.5 * Δt * q(k-1) ⊗ ω(k-1)这个近似只在 Δt 很小小于 10ms时可靠角速度超过 2 rad/s 时会出现明显的姿态跟踪滞后。这里需要说明一下离散化方式。热词里提到的“zoh 零阶保持”、“ekf前向欧拉和后向欧拉”本质区别在于假设角速度在两个采样时刻之间的变化形式。零阶保持假设角速度在单个采样周期内恒定一阶前向欧拉假设角速度在该时刻的斜率线性外推后向欧拉则用下一时刻的角速度值。MPU6050 这类数字输出传感器采样间隔内角速度本来就相对平滑用零阶保持已经足够前向欧拉的优势是计算量小后向欧拉的优势是数值稳定性好但姿态解算里很少单独用后向欧拉因为它需要未来时刻的角速度工程上不好直接落地。我写代码时默认零阶保持如果你发现姿态在高动态下发散可以换成 RK4 积分不要用前向欧拉硬扛。2.2 加速度计观测模型把重力当作参考向量加速度计测的是比力即惯性力与重力之差。在静止或匀速运动时它测到的就是重力向量在机体坐标系下的投影。由此观测方程可以写成a_meas R(q)^T * g v其中R(q)是从机体到导航坐标系的旋转矩阵g [0, 0, 9.81]是导航系下的重力向量v是测量噪声。将 R(q) 用四元数分量展开可以得到三个观测方程a_x 2 * (qx*qz - qw*qy) * g a_y 2 * (qy*qz qw*qx) * g a_z (qw^2 - qx^2 - qy^2 qz^2) * g这个观测模型有两个隐含假设。一是载体没有线加速度否则观测方程多了未知的干扰项滤波结果会偏向错误方向二是重力向量长度恒定在地球表面附近成立但在高海拔或自由落体场景下不成立。如果你的应用场景里有快速加减速比如无人机急停、机械臂末端高速运动需要额外引入线加速度状态的估计或者使用外部速度观测如 GPS 或光流对重力方向进行约束。2.3 协方差矩阵的初始化和更新语义EKF 的协方差矩阵P是很多新手直接忽略的部分。P的初值表示对初始状态估计的不确定性。在姿态解算场景中通常从静止状态加电启动初始姿态可以用加速度计直接计算横滚角和俯仰角atan2 即可偏航角初始化为 0。此时姿态的不确定性主要来自加速度计的噪声所以P(k0)设为 0.01 * I 是合理的。假如上电时载体正在运动初始姿态是未知的你应该把P设得更大比如 I让滤波器在最初的几十个周期内用加速度计快速修正姿态。有一个常见误区是把过程噪声协方差Q想成“无法测量”的魔法数字。实际上Q的含义可以用陀螺仪的 Allan 方差来标定把陀螺仪静止放置几小时采集角速度数据用 Allan 方差分析得到角度随机游走系数这个系数再乘以 Δt^2 就对应了Q的对角元素。手头没有 Allan 方差工具时可以先取经验值再用后文的方法调优。提示如果你把陀螺仪零偏bias也放进状态向量系统维度就从 4 维变成 7 维。零偏的加入会让观测模型需要额外设计“陀螺仪零偏变化缓慢”的驱动噪声这会显著影响滤波精度也会增加调参难度。6 轴融合场景我建议先不加零偏用在线零偏估计静态时取均值补偿处理。3. 基于 Python 的 EKF 融合实现最小可复现代码模型搭好之后真正动手写 EKF 前先想清楚一个问题你是要把代码烧进 MCU还是在 PC 上用离线数据做分析和调参两条路的代码结构基本一致但 Python 端更容易做数据可视化。本文给出用 Python NumPy 实现的最小版本不依赖任何姿态解算库方便你逐行对照模型。3.1 四元数工具函数EKF 的代码量不大但四元数相关的运算如果能复用写出来的代码会清爽很多。把这些工具函数放在独立模块里后续状态预测和观测更新都能直接调用import numpy as np from scipy.linalg import expm def quat_multiply(q1, q2): 四元数乘法遵循 Hamilton 约定。 q [qw, qx, qy, qz]标量在前。 w1, x1, y1, z1 q1 w2, x2, y2, z2 q2 return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 x1*w2 y1*z2 - z1*y2, w1*y2 - x1*z2 y1*w2 z1*x2, w1*z2 x1*y2 - y1*x2 z1*w2 ]) def quat_to_rotmat(q): 四元数转旋转矩阵机体到导航系 w, x, y, z q return np.array([ [1 - 2*(y*y z*z), 2*(x*y - z*w), 2*(x*z y*w)], [2*(x*y z*w), 1 - 2*(x*x z*z), 2*(y*z - x*w)], [2*(x*z - y*w), 2*(y*z x*w), 1 - 2*(x*x y*y)] ]) def quat_to_euler(q): 转欧拉角用于可视化单位弧度 w, x, y, z q roll np.arctan2(2*(w*x y*z), 1 - 2*(x*x y*y)) pitch np.arcsin(np.clip(2*(w*y - z*x), -1.0, 1.0)) yaw np.arctan2(2*(w*z x*y), 1 - 2*(y*y z*z)) return np.array([roll, pitch, yaw])这段代码里的四元数乘法定义了Hamilton 约定即qw在前。加速度计观测方程推导时用的旋转矩阵和这个约定是配套的如果你在其他资料里看到的四元数约定是[x, y, z, w]观测方程的符号会变需要仔细核对。3.2 预测与更新的主循环EKF 主循环写在下面的ekf_attitude函数里输入是陀螺仪与加速度计的批量数据输出是每一时刻的姿态四元数def ekf_attitude(gyro, acc, dt, q_initNone, Q_scale1e-6, R_scale5e-2): 6 轴 EKF 姿态解算主循环。 参数说明 - gyro: (N, 3) 陀螺仪数据单位 rad/s - acc: (N, 3) 加速度计数据单位 m/s^2 - dt: 采样周期单位 s - Q_scale: 过程噪声协方差缩放系数需要根据实测定 - R_scale: 测量噪声协方差缩放系数需要根据实测定 n len(gyro) q np.array([1.0, 0.0, 0.0, 0.0]) if q_init is None else q_init P np.eye(4) * 0.01 # 初始协方差 # 过程噪声矩阵 Q角速度噪声的方差经四元数雅可比映射 Q np.eye(4) * Q_scale # 测量噪声矩阵 R加速度计三个轴独立同分布 R np.eye(3) * R_scale euler_log np.zeros((n, 3)) q_log np.zeros((n, 4)) for i in range(n): # 预测 w gyro[i] omega 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] ]) # 离散状态转移矩阵零阶保持后的指数映射 F expm(0.5 * omega * dt) q quat_multiply(F q, np.array([1, 0, 0, 0])) # 状态转移的雅可比矩阵数值微分 A np.zeros((4, 4)) eps 1e-6 for j in range(4): q_pert q.copy() q_pert[j] eps # 重新计算 F 对 q 的导数这里用简化形式 # 近似为 F 本身角速度输入下 F 与 q 无关 A F P A P A.T Q # 更新使用加速度计 H compute_H(q) # 观测模型的雅可比见下方 z_meas acc[i] / np.linalg.norm(acc[i]) # 归一化单位方向向量 z_pred quat_to_rotmat(q).T np.array([0, 0, 1.0]) y z_meas - z_pred S H P H.T R K P H.T np.linalg.inv(S) q q K y q q / np.linalg.norm(q) # 更新协方差 I4 np.eye(4) P (I4 - K H) P (I4 - K H).T K R K.T # 记录结果 q_log[i] q euler_log[i] quat_to_euler(q) return q_log, euler_log这段代码有几个参数在跑通后需要专门调整。Q_scale决定滤波器对陀螺仪的信任程度调大相当于告诉滤波器“陀螺仪漂移比较严重”滤波器会更依赖加速度计修正姿态会有更多的高频抖动R_scale决定对加速度计的信任程度调大会让修正变慢姿态会变得更平滑但动态响应变差。两者之间是博弈关系后文会给出系统的调试方法。3.3 观测模型雅可比矩阵的计算上面代码里调用了compute_H这是 EKF 的观测模型雅可比矩阵是实现里最容易出错的地方。可以手推也可以用数值微分来做。数值微分在实际工程中完全可接受且能避免符号推导错误。它的核心就是再对状态加一个小扰动然后通过扰动的观测变化来估计导数def compute_H(q, eps1e-6): 计算观测模型 h(q) R(q)^T * g 对四元数的雅可比矩阵。 h 是 3 维q 是 4 维所以 H 形状为 (3, 4)。 H np.zeros((3, 4)) g np.array([0, 0, 9.81]) for j in range(4): q_plus q.copy() q_minus q.copy() q_plus[j] eps q_minus[j] - eps h_plus quat_to_rotmat(q_plus).T g h_minus quat_to_rotmat(q_minus).T g H[:, j] (h_plus - h_minus) / (2 * eps) return H提示主循环里我用的是归一化加速度计数据观测模型用的是单位重力向量[0,0,1]因此在compute_H里g也必须设成[0,0,1]否则量纲不匹配会导致卡尔曼增益计算错乱。保持代码里g的赋值与观测方程一致即可。数值微分的代价是 8 次旋转矩阵计算和 8 次矩阵乘法对现代 MCU 来说并不大但在 STM32F103 这类低频核上你可以改成手推的解析雅可比以省掉这部分开销。解析形式去参考 Madgwick 或 Mahony 论文里的四元数旋转矩阵展开即可这里不再展开。4. MPU6050 数据采集与 EKF 调参三个必调参数和判断地标融合算法本身只是骨架真正让姿态在实机上跑稳的是数据预处理和参数校准。MPU6050 是实验室最常见的 6 轴 IMU它的原始数据需要经过单位换算、零偏消除和采样间隔确定。三个参数决定了最后输出的姿态质量。4.1 数据采集链路换算、零偏与时间基准读取 MPU6050 时加速度计默认量程可以配置为 ±2g、±4g、±8g、±16g陀螺仪默认量程为 ±250、±500、±1000、±2000 °/s。量程越大分辨率越低所以在实际项目中要按目标应用选择量程。比如无人机运动角速度小用 ±500 °/s 就够手持云台需要更大的角速度范围可选 ±1000 °/s。原始 16 位整数到物理单位的换算关系是加速度 (m/s^2) raw_acc * 量程 / 32768 角速度 (rad/s) raw_gyro * 量程 / 32768 * π / 180零偏处理是 EKF 融合前最关键的一步。静止采集 200 帧取均值作为偏置后续每帧减掉这个均值。注意这个操作必须在每次上电时做因为 MEMS 器件的零偏随温度漂移和上次上电的数值差别可能很大。温度变化大的场景还需要额外做温度补偿曲线不过那通常属于更进阶的优化路线。采样周期dt的精度直接决定积分质量。很多项目直接设dt 1/100但实际代码里定时器抖动和中断延迟会造成偏差。比较好的做法是在每个采样周期的末尾读取当前时间戳然后令dt t_now - t_last哪怕只是用毫秒级的millis()或者HAL_GetTick()也比固定值更可靠。4.2 Q 和 R 的物理含义与初值确定过程噪声协方差Q的物理来源是陀螺仪的测量噪声和积分误差。工程上常用经验值为Q (σ_gyro * dt)^2 * I其中σ_gyro是陀螺仪的噪声标准差从静止数据的方差可以直接估算。比如静止数据看到陀螺仪的标准差是 0.02 rad/sdt0.01s那么Q ≈ (0.02*0.01)^2 4e-8取 Q_scale1e-6 是一个合理起点。测量噪声协方差R的物理来源是加速度计的噪声。静止时加速度计读数的标准熵乘以重力加速度 g 就是 R 的方差。注意这里有一个常见的理解误区加速度计的噪声并不是固定不变的在振动环境下会显著增加。因此如果你的设备装在电机附近R 的实际值应该比静止标定得到的大几十倍否则滤波器会过度信任失效的加速度计信号。调参的总体方向是在静止状态下观察欧拉角的噪声幅度正常应该小于 0.5°在快速转动后立刻静止观察姿态超调和回稳时间正常应该在 1 秒内收敛到静止值。如果静止时噪声偏大增大R_scale如果动态响应慢或回稳滞后减小Q_scale。当然这两者相互影响需要反复试验多次。4.3 振动场景下的加速度计置信度ATV 算法思路很多机器人平台上装了电机后加速度计的高频噪声急剧增大。标准的 EKF 对噪声的建模是高斯白噪声但电机振动带来的噪声是窄带、周期性的不符合高斯假设。一个实用的工程方案是自适应调整加速度计的权重算法实时检测加速度计信号的一阶导数峰值如果检测到剧烈变化就动态调大R让滤波器暂时只信任陀螺仪等振动结束后恢复。这种思路也被称为“加速度计置信度”Accelerometer Trust Value或“自适应卡尔曼滤波”。在 EKF 框架内部实现这一点并不难在观测更新之前先计算加速度计当前值与上一时刻值的差如果幅值超过阈值就对该时刻的 R 乘以一个大权重例如 10100 倍。5. 验证与调优线性回归看残差、Allan 方差查噪声一个 EKF 的姿态输出究竟准不准需要量化验证而不是只看曲线“像是那么回事”。常用的验证手段是用高精度转台提供真值但很多实验室没有这个条件。退而求其次可以用静态漂移测试和动态回中测试来间接评估滤波器的性能。5.1 静态漂移测试怎么判定偏航轴该不该信把设备静止放置 10 分钟记录 EKF 输出的欧拉角。理想情况下横滚角和俯仰角应该保持在 0° 附近的噪声带内不会有明显的长期漂移。偏航角因为没有任何外部观测约束只有加速度计无法提供偏航参考会随陀螺仪零偏缓慢漂移这是正常的、也是 EKF 6 轴方案的物理边界。如果想消除偏航漂移就需要引入磁力计做航向观测而磁力计的 EKF 融合会是另一篇独立文章的内容。这里给你一个判断标准10 分钟内横滚和俯仰漂移量不应该超过 1°偏航漂移速率不应该超过 1°/分钟。如果超过这个量级优先排查零偏补偿是否到位而不是质疑滤波算法本身。5.2 动态回中测试量化超调量和收敛时间让设备以较快速度绕 X 轴转 90°然后迅速静止观察滤波输出从动态状态回到静态零点的过程。这个过程里的两个关键指标是超调量和调节时间。超调量过大会影响云台的稳定效果调节时间过长会让人感觉画面发慢。如果超调量偏大、回中震荡次数多说明Q过大或R过小滤波器对加速度计的信任过多如果调节时间超过 2 秒说明Q过小或R过大滤波器反应迟钝。这个测试可以在真实硬件上重复 20 次把每次的回中时间求平均作为调参的目标函数。这比肉眼观察曲线稳定得多。5.3 从 EKF 到生产级方案的延伸用法EKF 融合陀螺仪和加速度计这套框架实际项目中往往会进一步演化。比如麦克纳姆轮底盘的定位中EKF 的姿态输出会和轮式里程计联合形成 2D 或 3D 的位姿估计再比如把磁力计并入观测方程后航向角也会被约束整个系统变成一个完整的 9 轴姿态航向参考系统AHRS。最后分享一个真正会省你时间的小技巧调试 EKF 时别只打印最终姿态把每时刻的P矩阵对角元素和卡尔曼增益的模长也输出出来。P的数值会告诉你滤波器是否收敛、是否有发散前兆卡尔曼增益可以帮助你判断两个传感器在这一刻谁的权重更大。有了这个视野调参就不再是靠运气乱试了而是一种有方向感的调试。本文还有配套的精品资源点击获取