简介：基于扩展卡尔曼滤波（EKF）的四旋翼无人机姿态估计Matlab实现，适合无人机控制、导航及智能机器领域的学习者与开发者，用于解决非线性系统下姿态角（横滚、俯仰、偏航）的准确估计问题。资源包共10个文件、约390KB，其中3个m文件为核心算法源码，5个jpg与1个png为各姿态角及角速度的滤波结果图，另含README说明文档，整体结构紧凑、便于直接阅读和仿真。当前已有156人学习/下载，属于入门级到中级进阶的实用参考。内容涵盖状态向量定义、动力学模型线性化、预测更新与观测更新等关键技术步骤，通过Matlab代码可直观理解EKF在无人机姿态估计中的完整应用流程，也可为其他智能机器的状态估计提供思路。

1. 四旋翼姿态估计为什么绕不开EKF：先从“一开机就飘”说起

把四旋翼平放在桌上，给飞控上电。陀螺仪输出的不是姿态，而是角速度。如果直接对角速度做积分，几秒钟后Roll/Pitch看着还行，Yaw会像一个醉酒的人一样缓慢但坚定地漂走。这不是传感器坏了，而是陀螺仪的零偏在积分过程中被无限放大。要解决这个问题，得引入一个绝对参考——加速度计。但加速度计在四旋翼做机动时又会被运动加速度污染，不能全信。扩展卡尔曼滤波（EKF）在这里做的事，就是用陀螺仪做短期预测、用加速度计做长期校正，把所有不确定性用协方差矩阵显式建模。这篇笔记要聊清楚四旋翼姿态估计为什么绕不开EKF：从IMU的数学建模，到一个能跑的Python最小实现，再到参数怎么调、真机上哪些坑是几乎每个人都会踩一遍的。对应的读者是正在写飞控、做毕设姿态解算，或者第一次把EKF往真机里塞的人。

2. 先把IMU的“脾气”写成数学：EKF的状态方程、观测方程与为什么是它

2.1 为什么不是普通卡尔曼，也不是互补滤波

卡尔曼滤波的前提是线性系统。但四旋翼的姿态运动学方程里全是三角函数和四元数乘法，状态转移天然非线性。普通KF套上去，模型误差会把协方差矩阵一步步带偏，滤波器最后会变成“看起来在运行、实际上在自欺欺人”的黑匣子。EKF做的事很简单粗暴：在每一个时刻，把非线性函数在当前状态附近做一阶泰勒展开，用雅可比矩阵代替原来的线性转移矩阵。这是工程上最成熟的非线性状态估计方案。

互补滤波（Mahony/Madgwick）在小四轴里也大量在用，它用PI结构把加速度计误差补偿进陀螺仪零偏，代码简洁、调参少、在嵌入式上跑得飞快。但它有两个天花板：一是对多传感器融合的扩展很尴尬，加了磁力计、气压计、甚至GPS的速度/位置观测以后，架构会变得绕来绕去；二是零偏估计精度和收敛速度都不如EKF显式建模来得好。如果只是做最基础的四旋翼姿态解算，互补滤波够用；但一旦要往完整导航方向走，EKF是必选项。标题里点名EKF，就是奔着这套可扩展、可显式估计零偏的路线去的。

2.2 状态方程：四元数运动学与陀螺仪零偏的随机游走

姿态估计的状态量，我很少直接用欧拉角。欧拉角在Pitch接近±90°时会出现万向节锁，而且三角函数的雅可比推导容易写错。工程上常见的做法是用四元数表达姿态，再在输出层转换成欧拉角给上层用。但四元数有一个麻烦：它的四个分量必须满足单位模长约束。普通的卡尔曼更新不保证这个约束，所以每当状态传播和观测更新结束后，都得手动归一化一次，这个动作看起来不起眼，漏掉就是姿态跳变的直接原因。

在四旋翼姿态估计这个标题下，我一般把状态向量定为7维：四元数q0..q3加陀螺仪零偏bx,by,bz。连续时间的状态方程是四元数运动学微分方程加上零偏的随机游走模型：q_dot = 0.5 * q ⊗ [0, ω]^T，b_dot = w_b。其中ω是修正零偏后的角速度，w_b是驱动零偏缓慢变化的随机游走白噪声。离散化时，用角速度增量构造一个小角度四元数，再左乘当前四元数，这就是第3章代码里dq = [1, 0.5 * dt * ω] 的来源。小角度近似在dt=1ms/10ms量级完全够用，前提是初始化时姿态误差不能太大。

把零偏放进状态，是这套方案里最关键的一步。陀螺仪零偏不是常数，它会随温度、机械振动和时间缓慢变化。如果只把它当成固定值去校准，飞一会儿姿态又会重新开始漂移。显式建模成随机游走后，EKF可以一边估计姿态、一边在线跟踪零偏的缓慢变化，长时间静止时Roll/Pitch的漂移量能从积分方案的几十度/小时压制到零点几度的水平。代价是状态从4维涨到7维，雅可比矩阵从4x4变成7x7，但对现代飞控的MCU来说这点计算量完全不是问题。

2.3 观测方程：加速度计测量重力投影的“敢信时机”

加速度计作为观测来源，测量模型写出来很朴素：在静止或匀速状态下，加速度计读数等于重力加速度在当前机体坐标系下的投影。对应的观测方程就是z = R(q)^T * g_ned。R(q)是从机体坐标系到导航坐标系的旋转矩阵，这个方程做非线性函数展开后得到观测雅可比H，就能把加速度计读数和状态里的四元数联系起来。

真正要在工程里拿捏的是“敢信时机”。四旋翼一旦做过推机动，加速度计测到的比力里就混入了运动加速度，这时候观测模型被破坏。EKF对这个问题没有魔法，它的做法在数学上依然正确——把观测噪声R阵调大，让滤波器对加速度计不那么信任。但R阵是固定值，所以大家一般会在飞控里加一个加速度计置信度开关，自动检测机体系比力模长偏离1g的程度，偏离越大R越大，或者干脆在剧烈机动时跳过加速度计更新。建模阶段就要把这个“敢信时机”先写清楚，后面第4章调R阵时才不会一头雾水。做建模检查时，我习惯问自己三个问题：状态量是否包含了需要在线估计的误差源？运动学方程是否在dt范围内成立了？观测模型是否只在特定运动条件下成立？三个问题都回答得上来，手里的EKF公式才有资格往嵌入式上搬。

3. 搭一个能跑的最小闭环：纯Python的EKF姿态估计仿真

3.1 生成“会飞的假数据”：真值轨迹与两类传感器噪声

写EKF的第一步不是写EKF，而是先造一个带真值的仿真环境。没有真值对照，滤波器到底估计对了没有全靠猜，这不符合工程习惯。这里用一个正弦叠加的角速度轨迹驱动真值四元数做积分，同时给陀螺仪加上恒定零偏和白噪声，给加速度计加上白噪声。这样生成的数据是可控的，真值已知，后面验证EKF时能直接算误差。

import numpy as np from scipy.linalg import block_diag np.random.seed(2024) dt = 0.01 steps = int(20.0 / dt) t = np.arange(steps) * dt # 真值角速度：三个轴不同频率的正弦叠加，模拟中等机动 w_true = np.zeros((steps, 3)) w_true[:, 0] = 0.10 * np.sin(0.5 * t) w_true[:, 1] = 0.08 * np.sin(1.2 * t) w_true[:, 2] = 0.05 * np.cos(0.8 * t) def quat_mult(p, q): p0, pv = p[0], p[1:] q0, qv = q[0], q[1:] return np.r_[p0*q0 - np.dot(pv, qv), p0*qv + q0*pv + np.cross(pv, qv)] # 用一阶小角度增量积分真值四元数 q_true = np.zeros((steps, 4)) q_true[0] = np.array([1.0, 0, 0, 0]) for i in range(1, steps): dq = np.r_[1.0, 0.5 * dt * w_true[i]] dq /= np.linalg.norm(dq) q_true[i] = quat_mult(q_true[i-1], dq) q_true[i] /= np.linalg.norm(q_true[i])

这段代码模拟的是“假设我们有一架完全知道自己在做什么的理想飞机”。真值四元数用一阶小角度积分生成，对20秒仿真来说精度足够。陀螺仪零偏故意设成非对角的数值[0.02, -0.015, 0.01]，这是为了后面验证EKF的零偏估计能力。随机种子固定，翻车时可复现，这是一个在调试滤波算法时不该省的习惯。

接下来生成传感器测量数据：陀螺仪测量 = 真值角速度 + 零偏 + 白噪声；加速度计测量 = 重力在机体系投影 + 白噪声。注意这里噪声的单位要和后面Q/R矩阵的量纲对得上，否则调参时会怀疑人生。

bias_true = np.array([0.02, -0.015, 0.01]) # rad/s gyro_meas = w_true + bias_true + 0.005 * np.random.randn(steps, 3) def quat_to_rot(q): a, b, c, d = q return np.array([ [1-2*(c*c+d*d), 2*(b*c-a*d), 2*(b*d+a*c)], [2*(b*c+a*d), 1-2*(b*b+d*d), 2*(c*d-a*b)], [2*(b*d-a*c), 2*(c*d+a*b), 1-2*(b*b+c*c)] ]) acc_meas = np.zeros_like(gyro_meas) g_ned = np.array([0, 0, 9.81]) for i in range(steps): acc_meas[i] = quat_to_rot(q_true[i]).T @ g_ned acc_meas[i] += np.random.randn(3) * 0.05

加速度计的噪声我故意给得比实际情况乐观一点，0.05 m/s²的标准差在消费级IMU里算很安静的状态。真机上电机振动一叠加，这个数字基本要放大三到五倍。这么做的好处是先把EKF算法的逻辑验证跑通，再慢慢把数据变脏，定位问题时就能明确区分“算法写错了”还是“传感器数据太烂”。很多人在真机上排错排到崩溃，就是因为没有这一层隔离。

3.2 核心EKF代码：predict与update的工程写法

现在写EKF主体。先定义一个数值雅可比函数，在原型验证阶段用数值雅可比代替手推的解析雅可比，可以少掉一大半公式推导的烦恼。7维状态下的数值雅可比计算量完全可以接受，真机上再换成解析表达式即可。

def numerical_jac(f, x, step=1e-6): n = len(x) m = len(f(x)) J = np.zeros((m, n)) for i in range(n): xp = x.copy(); xp[i] += step xm = x.copy(); xm[i] -= step J[:, i] = (f(xp) - f(xm)) / (2 * step) return J

predict函数负责状态传播。四元数部分用小角度增量乘法完成，零偏部分保持不变（随机游走噪声通过Fw矩阵注入）。这里的关键点在于过程噪声的传播：陀螺仪白噪声和零偏随机游走噪声都要通过对应的雅可比矩阵贡献到状态协方差里，不能只是简单地在P上叠一个对角阵。

def predict(x, P, gyro, dt): q, b = x[:4], x[4:] omega = gyro - b def f_state(x_in): q_in = x_in[:4] b_in = x_in[4:] omega_in = gyro - b_in dqi = np.r_[1.0, 0.5 * dt * omega_in] dqi /= np.linalg.norm(dqi) q_new = quat_mult(q_in, dqi) return np.r_[q_new, b_in] def f_noise(x_in, n_in): q_in = x_in[:4] b_in = x_in[4:] omega_in = gyro - b_in + n_in[:3] dqi = np.r_[1.0, 0.5 * dt * omega_in] dqi /= np.linalg.norm(dqi) b_new = b_in + n_in[3:] return np.r_[quat_mult(q_in, dqi), b_new] F = numerical_jac(lambda xv: f_noise(xv, np.zeros(6)), x) Fw = numerical_jac(lambda nv: f_noise(x, nv), np.zeros(6)) Q = np.diag([0.005**2]*3 + [1e-6]*3) P_pred = F @ P @ F.T + Fw @ Q @ Fw.T return f_state(x), P_pred

F是对状态量x的雅可比，Fw是对过程噪声n的雅可比。Q矩阵里前三个元素是陀螺仪角度随机游走的方差，单位是rad²/s²，后三个是零偏随机游走的方差，单位是rad²/s³。这两个量纲如果在推导时没盯住，滤波器很容易出现“协方差阵正定但估计完全不动”的诡异状况。真实调参时Q的值要从IMU的Allan方差曲线里读，这里先用固定值让仿真闭环跑通。

update函数解决的是观测融合。加速度计的预测观测是重力在当前姿态下的机体系投影，残差通过卡尔曼增益映射到状态修正量。这里有个细节：四元数增量的更新不是直接相加，而是把修正量dx的前四维当作增量四元数，和当前四元数做乘法。这是四元数EKF的标准写法，直接相加会破坏单位模长约束。

def update(x, P, acc): def h(x_in): R = quat_to_rot(x_in[:4]) return R.T @ g_ned H = numerical_jac(h, x) R_meas = np.eye(3) * 0.05**2 z_pred = h(x) y = acc - z_pred S = H @ P @ H.T + R_meas K = P @ H.T @ np.linalg.inv(S) dx = K @ y dq = dx[:4] dq /= np.linalg.norm(dq) if np.linalg.norm(dq) > 1e-12 else 1.0 q_new = quat_mult(dq, x[:4]) q_new /= np.linalg.norm(q_new) b_new = x[4:] + dx[4:] P_new = (np.eye(7) - K @ H) @ P return np.r_[q_new, b_new], P_new

这里为了让原型尽量直白，观测雅可比H用了数值求导。真机上每一次观测更新都要算7个数，状态是7维时数值雅可比要算14次函数求值，对200Hz的更新率来说压力不大。但如果后续要加磁力计、GPS等更多观测，解析雅可比在计算量和复杂度上更有优势。我的习惯是仿真阶段用数值雅可比，真机移植前把解析雅可比手工推导一遍并做数值对比验证，两者差超过1%就要怀疑哪里推错了。

主循环直接跑20秒仿真。每个周期先predict再用加速度计update，时刻检查四元数模长和零偏估计值。

x_est = np.r_[1.0, 0, 0, 0, 0, 0, 0] P_est = np.eye(7) * 1e-3 P_est[4:, 4:] *= 1e-2 err_list = [] for i in range(steps): x_est, P_est = predict(x_est, P_est, gyro_meas[i], dt) x_est, P_est = update(x_est, P_est, acc_meas[i]) x_est[:4] /= np.linalg.norm(x_est[:4]) err = x_est[:4] - q_true[i] err_list.append(np.linalg.norm(err)) print("零偏估计: ", x_est[4:]) print("平均四元数误差: ", np.mean(err_list))

3.3 验证代码没写错的四个信号

跑完这个闭环以后，只盯着误差曲线看不够。我一般检查四个信号。零偏估计是否接近bias_true，如果差得远但姿态误差正常，大概率是Q_bias给小了；四元数模长是否始终保持在1附近，模长偏移说明归一化时机处理有问题；协方差矩阵P是否不发散也不被压得过小，P对角线太小会让滤波器对后续观测失去反应，症状是误差曲线“稳”在一个不正确的值上不动；最后才是姿态误差本身，Roll/Pitch稳态误差应该小于1°，如果误差持续振荡，多半是R阵和Q阵的比例失衡——R太小则噪声串进姿态，Q太小则跟踪太慢。这四个信号全部通过，才算这套最小EKF的数学逻辑站得住。

4. EKF的四个必调参数：R、Q、初始P阵与dt的真实调法

4.1 R阵：加速度计信任度怎么标定才不靠玄学

R阵在EKF里代表“对加速度计测量的信任程度”，工程实现时就是对角线上的三个方差值。取值过小，滤波器会把加速度计的每一个噪声尖峰都当成真实姿态变化，看到的表现是悬停时Roll/Pitch角度高频抖动；取值过大，加速度计的校正作用被削弱，姿态会像陀螺仪纯积分一样低频漂移。很多人把R阵当玄学拍脑袋填，实际上完全可以用离线数据标定：把飞控静止平放，记录30秒加速度计输出，计算每个轴的标准差，平方后填进R阵。静止状态下加速度计噪声通常落在0.02到0.1 m/s²之间，对应R值为4e-4到1e-2。动态场景里要在这个基础上做一个动态放大系数，因为震动和机动都会让实际测量噪声远超静止水平。

放大的具体做法没有标准答案。常见的做法是用机体系比力模长与1g的偏差做启发式门限，偏差超过0.15g时把R阵各元素线性放大两到三倍，超过0.3g时干脆跳过更新步。我见过一些飞控里直接用加速度计低通滤波后的高频能量做R阵缩放，效果更平滑，但需要额外维护一个能量估计器。不管哪种方案，核心思想都是同一句话：动态越激烈，加速度计越不可信。

4.2 Q阵：陀螺仪角度随机游走与零偏随机游走的量纲陷阱

Q阵的设定比R阵更容易翻车，因为很多资料只给“Q大一点跟踪快、Q小一点更平滑”这种模糊口径，但实际调的时候会发现调Q的本质是量纲对不对。对于7维状态模型，过程噪声分成两块：陀螺仪角度随机游走的方差，直接进四元数传播路径，影响的是角速度测量的可信度；零偏随机游走的方差，影响的是滤波器对零偏变化的跟踪速度。

量纲上，角度随机游走是rad²/s²，零偏随机游走是rad²/s³。很多人在代码里把两个值写成相同的数字，结果滤波器对零偏的敏感度和对姿态的敏感度完全错误。工程上的标定方法是Allan方差法：录几小时静止陀螺仪数据，画Allan曲线，在最小点读出角度随机游走系数，在高频段读出零偏不稳定性。如果没有测试条件，常用保守值是角度随机游走方差取0.001²到0.01²，零偏游走取1e-6²到1e-4²。值得注意的是，Q_bias太小会让零偏估计收敛到错误值后“锁死”，Q_bias太大则零偏估计会被加速度计噪声污染。要验证零偏估计是否健康，可以对比EKF估计零偏和静止数据直接平均得到的零偏，两者应该在合理范围内接近。

4.3 初始P阵、dt抖动与协方差“锁死”现象

初始P阵是另一个容易埋雷的点。P0代表对初始状态不确定度的估计，如果给得太小，比如1e-8，卡尔曼增益会从起步阶段就被压死，表现为滤波器几乎不响应观测更新，姿态误差慢慢悠悠往真值爬甚至根本不动，这就是“协方差锁死”。我一般把P0设为1e-3量级的对角阵，零偏对应位置再放大10倍。这个数值不需要很精确，但宁大勿小，大的P0只是让滤波器多花零点几秒收敛，小的P0可能让它永远收敛不到。

参数调整的第二步是dt。理论上EKF要求在时间戳精确的前提下按实际采样间隔做离散化。很多飞控代码里写死固定dt=0.01，但IMU中断抖动、DMA传输延迟、任务调度优先级抢占，都会让真实采样间隔在0.009秒到0.011秒之间跳动。固定dt带来的误差在高频振动下会被放大，最终表现为协方差矩阵和真实系统不匹配。常见做法是用IMU自带的时间戳差分出真实dt，做过限幅保护，比如限制在5ms到20ms之间。如果传感器不支持时间戳，至少要做一个基于定时器捕获的采样时刻记录，然后差值计算，这是真机调试最容易忽略、却直接影响滤波性能的细节。

我习惯把参数整理成一张表贴在代码文件头，每次改参数只动一处：

这张表就是调参时的“后悔药”。不要一次改两三个参数，改完一次、跑一遍离线数据、比对一次误差曲线，永远只让一个变量动，出了问题才能定位到根因。

5. 真机常见的四类翻车现场：EKF姿态估计避坑清单

5.1 现象：静止悬停时姿态低频漂移，越飘越快

现象：四旋翼纹丝不动地悬停，但地面站的Roll/Pitch角度以肉眼可见的速度缓慢漂移，几分钟后偏了5度以上。原因：这不是陀螺仪零偏估计失效，而是加速度计观测校正被完全压制了。排查时会发现R阵被设得过大，或者动态加速度检测逻辑误触发了“跳过观测更新”。解决：先做静止标定确认R阵量级正确，再检查动态置信度逻辑有没有在悬停时误判为剧烈机动。常见误判来源是电机振动被PID控制环路耦合进IMU数据，导致高频能量突变，置信度逻辑误以为在做大机动。把动态置信度的带宽压低，或者用二阶低通滤波预处理比力模长，问题会直接消失。

5.2 现象：大机动后姿态恢复极慢，像“被冻住”

现象：做完一个大幅俯仰动作后，姿态角要好几秒才能慢慢回到正确值，期间飞控姿态和实际姿态明显不一致，飞机有明显“点头”感。原因：大机动期间加速度计观测按预期被降低信任，但Q阵里的陀螺仪噪声给得太小，滤波器在机动结束后依然极度相信陀螺仪的预测姿态，不愿意接受加速度计校正。解决：适当增大Q_gyro，让滤波器在大机动后对陀螺仪积分的信任快速下降，加速收敛。另一个有效手段是在检测到机动结束时临时放大P阵对角线几毫秒，等价于手动加速“清醒”。这个操作在多个开源飞控里都有影子，本质就是给滤波器一剂强心针，让它重新开始信任观测。踩过这个坑之后，我习惯在仿真里加入90度快速俯仰的回转测试，专门验证机动恢复时间是否在0.5秒以内。

5.3 现象：四元数模长偏移，姿态角偶发跳变

现象：姿态曲线在某几个时刻出现尖锐跳变，跳变幅度几度到几十度不等，地面站上能看到一个明显的刺。查看调试变量发现四元数模长在跳变前已经偏离1超过0.01。原因：四元数归一化的时机不对。状态传播会产生模长漂移，观测更新后合成的四元数也可能偏离单位模长，如果只在某一个环节做归一化，另一处的漂移会被保留并放大到下一次更新。解决：predict函数返回前归一化一次，update函数合成新四元数后立刻归一化一次，主循环末尾再兜底归一次。三重归一化看起来冗余，但对数值稳定性是实打实的保护。更干净的方案是改误差状态EKF，把四元数误差单独维护，但这个升级要动整个算法骨架，原型阶段三重归一化已经完全够用。

5.4 现象：仿真里完美收敛，真机上平均误差大一倍还不止

现象：同一份算法，仿真验证时姿态误差稳稳压住0.5度，装到真机上却出现明显抖动和偏差。原因：仿真里的IMU噪声是理想白噪声，真机上电机振动、采样时间戳抖动、ADC量化噪声、甚至机架共振都会破坏EKF的噪声假设。最典型的破坏是时间戳不同步：陀螺仪和加速度计数据在不同时刻到达，但EKF把它们当成同一时刻的测量。解决：在驱动代码里给每个采样打时间戳，预测步用实际dt，更新步之前把陀螺仪数据和加速度计数据按时间戳对齐，差值超过一个采样周期就丢弃或外推。另一个有效做法是统计真机IMU静止噪声的标准差，回填到仿真模型里，重新跑一遍参数调优，而不是把仿真公认好的参数直接固化到真机。这个过程中学到的血泪经验是：仿真从来不是用来验证参数，而是用来验证算法逻辑，参数必须从真机数据里来。

6. 最后一公里：先离线回放、再上真机，验证EKF的唯一可靠方法

6.1 把真机数据录下来回放：比“跑起来感觉稳了”更可信

如果手里没有带真值的光学动捕或转台，最可靠的验证方案是录一段真实IMU数据，然后离线回放。回放时保留原始的陀螺仪和加速度计数采样时间戳，把EKF主循环稍作改造，让每个预测步都使用差分得到的真实dt，这样能同时暴露时间戳对齐问题和参数问题。评估指标上，没有真值就退而求其次：检查悬停段的姿态噪声幅值、大机动后的恢复时间、静止段零偏估计的稳定性。如果手头有一段带真值的数据集，那把估计姿态换算成旋转向量误差，统计均值和标准差，Roll/Pitch稳态均值小于0.5度、标准差小于1度是一个比较实际的目标。

6.2 调参顺序与一个沿用至今的习惯

调参顺序我建议固定成R阵、Q阵、P0、dt相关代码检查。每改一个参数，只录同一段真机数据回放对比，记录日期、参数值、误差统计和环境条件。我第一次上真机时直接把仿真参数套进去，结果悬停状态看起来稳定，一做机动姿态就飘，后来离线回放才发现是加速度计时间戳比陀螺仪晚了整整两个采样周期。这类问题如果有固定回放流程，几分钟就能抓到，不算难查。还有一个沿用至今的习惯是：每次升机前跑一段固定的手持晃动测试，把EKF输出的姿态和机载视觉粗略估算的角度做个直观对比，方向对不对、延迟大不大，一秒内就能感觉出来。希望这个验证方法能帮到你少走一段弯路。

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