简介面向无人机导航与组合导航学习者这份MATLAB工程围绕GPS与INS融合利用扩展卡尔曼滤波EKF实现对6自由度无人机位置、速度和姿态的高精度状态预测。资源共5个文件压缩包仅194KB包含4个.m脚本与1个.mat数据文件脚本涵盖主程序模板、带滤波与不带滤波的两种对比实现以及四元数转欧拉角等辅助函数。已有140人学习下载。通过代码可直观学习EKF线性化建模、状态转移矩阵与观测矩阵设计、噪声协方差参数设定并借助有无滤波器的估计结果对比从均方根误差或协方差阵等指标量化滤波增益掌握GPS与INS互补融合的完整思路还可根据注释修改状态维度和噪声参数将方法迁移到无人机飞控或车载导航等应用场景。适合正在学习卡尔曼滤波或开展定位课程的本科生、研究生与工程师参考。1. GPS-INS 融合为什么非 EKF 不可6DOF 无人机状态估计有个绕不开的矛盾GPS 更新慢消费级约 510 Hz且拿不到姿态IMU 更新快200 Hz 以上但纯积分三分钟误差就到几十米。MATLAB 里用 EKF 做 GPS-INS 融合就是用 GPS 绝对位置周期性约束 IMU 的积分漂移同时用 IMU 的高速输出填补两次 GPS 观测之间的空隙这也是 PX4 等飞控自主定位的通用底座。课程作业里 ass3_q2_ass_q3_kf 这类题目考察的都是同一件事把 6 DOF 状态模型与滤波器预测、更新两步正确落成 MATLAB 代码。难点不在 EKF 公式本身而在状态向量怎么排、雅可比怎么求、Q 和 R 给多大才不炸。下文按「建模 → 代码 → 调参 → 验证」四条线依次展开。2. 6DOF 无人机状态模型状态向量、IMU 运动学与 GPS 量测建模模型决定滤波器能估出什么参数决定估得多好。GPS-INS 融合的模型由状态转移和量测两部分构成下面先定义 15 维状态向量再给出 IMU 递推方程与 GPS 量测方程三者对齐后后面的代码只是方程的机械翻译。试图跳过模型直接套现成的 ekf 算法源码通常会在雅可比和量测矩阵上卡住最后还是要回来补课。2.1 15 维状态向量位置、速度、姿态与零偏怎么排EKF 第一步是定义状态向量。6DOF 无人机在 NED北东地坐标系下最常见的状态是 15 维位置 3 维、速度 3 维、姿态 3 维横滚、俯仰、航向欧拉角再加陀螺零偏 3 维和加速度计零偏 3 维。把零偏纳入状态是 GPS-INS 融合的关键如果状态里只有位置、速度和姿态IMU 的常值零偏会被位置误差吸收位置精度永远上不去一旦零偏成为可估计状态GPS 每次更新就会同时修正位置和零偏位置精度才能逼近 GPS 上限。索引排布约定以下表为准贯穿全文代码索引状态单位说明1:3pn, pe, pdmNED 系位置4:6vn, ve, vdm/sNED 系速度7:9φ, θ, ψrad欧拉角姿态10:12bgx, bgy, bgzrad/s陀螺零偏13:15bax, bay, bazm/s²加速度计零偏MATLAB 中状态与协方差初始化我一般这么写x zeros(15, 1); P blkdiag(1e-2*eye(3), 1e-2*eye(3), 1e-4*eye(3), ... 1e-6*eye(3), 1e-4*eye(3));初始协方差 P0 反映对初始状态的信任程度位置和速度给 1e-2对应约 0.1 m 和 0.1 m/s 的不确定度姿态给 1e-4约 0.57°零偏给得更保守。若初始位置来自单点 GPS水平分量可以放宽到 10² 量级宁可让滤波器先不自信地收敛也不要一开始就锁死到错误位置。blkdiag 参数顺序与表格索引严格对应代码和表不一致是排错时最隐蔽的 bug我见过有人在这上面花掉一下午。2.2 IMU 递推运动学加速度计测的是比力不是加速度IMU 加速度计输出的是比力specific force即单位质量受到的除重力以外的合力。NED 下速度微分方程必须写成 v_dot R_b^n·(a_meas − ba) [0;0;g]漏掉重力项是最常见错误把加速度计读数直接积分垂直通道按 −g 的加速度持续发散几秒内高度就不可接受。NED 下 g 取 9.81 m/s²具体符号以数据手册为准——静止平放时加速度计 z 轴读数约 −1g 还是 1g取决于 z 轴定义。姿态部分用 ZYX 欧拉角速率方程递推核心预测函数如下function x_new predictState(x, imu, dt) % x : 15 维状态列向量索引见 2.1 节表格 % imu : [ax; ay; az; gx; gy; gz]体坐标系下的量测 phi x(7); th x(8); psi x(9); w imu(4:6) - x(10:12); % 去零偏角速度 a imu(1:3) - x(13:15); % 去零偏比力 % 体轴 - NED 旋转矩阵ZYX 欧拉角 R [cos(th)*cos(psi), sin(phi)*sin(th)*cos(psi)-cos(phi)*sin(psi), ... cos(phi)*sin(th)*cos(psi)sin(phi)*sin(psi); cos(th)*sin(psi), sin(phi)*sin(th)*sin(psi)cos(phi)*cos(psi), ... cos(phi)*sin(th)*sin(psi)-sin(phi)*cos(psi); -sin(th), sin(phi)*cos(th), ... cos(phi)*cos(th)]; g [0; 0; 9.81]; x_new x; x_new(1:3) x(1:3) x(4:6)*dt 0.5 * R * a * dt^2; % 位置二阶积分 x_new(4:6) x(4:6) (R*a g) * dt; % 速度一阶积分 p w(1); q w(2); r w(3); phi_dot p tan(th)*(q*sin(phi) r*cos(phi)); theta_dot q*cos(phi) - r*sin(phi); psi_dot (q*sin(phi) r*cos(phi)) / cos(th); x_new(7:9) x(7:9) [phi_dot; theta_dot; psi_dot] * dt; % 零偏在预测步不变不确定性由过程噪声 Q 驱动 end几个要点位置递推里 0.5·R·a·dt² 是二阶积分项200 Hz 的 IMU 数据下它比一次项小两个数量级但对位置精度有可观测改善姿态用欧拉角速率方程计算量小代价是 θ ±90° 时 ψ_dot 奇异处理方案在第 5 章零偏预测步不变本质是随机游走建模其不确定性增长全靠 Q 驱动所以 Q 的零偏通道不能设成 0。2.2.1 动手前先自检R 还是 Rᵀ旋转矩阵方向搞反是 GPS-INS 融合里出现频率最高的问题且表现极具迷惑性静止时一切正常一旦无人机有姿态变化位置和速度就开始朝相反方向漂。写完后做一个快速自检令 φ0、θ0、ψπ/2此时机头指向东R 的第一列应为 [0;1;0]令 θπ/2 时R 的第三行应为 [-1;0;0]。如果自检不过把 R 换成它的转置再试。文件名里带 kf 也不代表这里能直接用线性卡尔曼滤波R 随姿态变化系统本质非线性必须在每个时刻重新线性化。2.3 GPS 量测模型线性 H 矩阵与坐标转换GPS 位置量测方程是线性的z H·x vH [eye(3), zeros(3, 12)]; % 只观测位置前三维这是整个融合里唯一线性的部分所以 EKF 的雅可比只需针对预测步计算。量测噪声 v 假设零均值高斯协方差 R_gps 常取对角阵水平垂直分量分开放。前提是 GPS 位置已投影到 NED拿到经纬高时需先用 geodetic2nedAerospace Toolbox或自写 WGS-84 投影转换。投影残差会进入量测噪声R_gps 里建议留至少 0.5 m 余量否则滤波器会把投影误差当成真实位置去修正反而引入水平偏差。GPS 误差本身有慢变特性多径、星历残余这些分量在 R 里无法完全描述工程上常用一阶马尔可夫模型扩展状态来吸收入门版本可以先忽略但要清楚这个限制存在。3. MATLAB 实现 EKF 核心代码预测步、更新步与主循环网上能搜到的 ekf 算法源码很多能直接对接 6DOF 无人机 GPS-INS 场景的往往缺两部分完整的 15 维状态递推以及与传感器时间戳对齐的更新逻辑。下面把三个函数和一个主循环完整给出直接复制即可跑通最小版本。阅读时建议把每个函数与 2.2 节的方程逐一对照代码只是方程的另一种写法。3.1 预测步F 矩阵的有限差分与符号雅可比EKF 预测步需要状态转移雅可比 F ∂f/∂x。predictState 是非线性函数最稳妥的求法是有限差分不用手推导数适合验证function F numericalJacobian(x, imu, dt) nx numel(x); fx0 predictState(x, imu, dt); delta 1e-6 * max(abs(x), 1); % 按状态量级自适应 F zeros(nx, nx); for i 1:nx xp x; xp(i) xp(i) delta(i); F(:, i) (predictState(xp, imu, dt) - fx0) / delta(i); end end步长取 1e-6·max(|x|,1) 而不是固定 1e-6原因在于状态量级差悬殊位置可达几百米姿态在 10⁻² 量级固定步长会让姿态列的差分结果被浮点舍入淹没。这个函数每次预测要算 15 次状态递推MATLAB 循环开销可观但对入门和调试完全够用。3.1.1 符号雅可比一次推导长期复用生产项目里推荐用 Symbolic Math Toolbox 导出解析雅可比运行时快一个量级syms posn pose posd veln vele veld phi th psi bgx bgy bgz bax bay baz dt real syms ax ay az gx gy gz real x_sym [posn; pose; posd; veln; vele; veld; phi; th; psi; ... bgx; bgy; bgz; bax; bay; baz]; imu_sym [ax; ay; az; gx; gy; gz]; f_sym predictState(x_sym, imu_sym, dt); % 内部运算需支持符号变量 F_sym jacobian(f_sym, x_sym); F_fun matlabFunction(F_sym, Vars, {x_sym, imu_sym, dt});用这个方案的前提是 predictState 里只有 cos、sin、矩阵乘法这类支持符号变量重载的运算R 矩阵必须显式写成三角函数组合。调试时用有限差分和符号雅可比各算一版两者最大误差超过 1e-6 就说明旋转矩阵排布或索引映射有误。协方差递推用一阶离散近似nx 15; F_d eye(nx) F * dt; % 一阶近似 Q_d F * G_c * Q_c * G_c * F * dt; % 过程噪声离散化 P_pred F_d * P * F_d Q_d;F_d 只保留一阶项200 Hz 的 IMU 数据足够IMU 降到 50 Hz 以下时才考虑二阶项。G_c 与 Q_c 在第 4 章给出这里先假设已存在于工作区。3.2 更新步GPS 观测修正与 Joseph 形式function [x_upd, P_upd] gpsUpdate(x_pred, P_pred, z, R_gps) H [eye(3), zeros(3, 12)]; z_hat x_pred(1:3); % 预测位置 y z - z_hat; % 新息 S H * P_pred * H R_gps; % 新息协方差 K P_pred * H / S; % 用右除避免显式 inv x_upd x_pred K * y; IKH eye(15) - K * H; P_upd IKH * P_pred * IKH K * R_gps * K; % Joseph 形式 end两个细节值得交代。K P_pred·H/S 用矩阵右除MATLAB 走线性求解而不是显式求逆数值稳定性更好P 更新用 Joseph 形式而非教材常见的 P (I−KH)P代价是两次额外矩阵乘法但能保证对称正定长期运行不退化。GPS 数据缺失或搜星数不足时这一帧应跳过更新步直接把预测结果作为输出滤波器抗退化能力首先来自数据质量把关。3.3 主循环高频 IMU 预测叠低频 GPS 更新% imuData : N x 7列 [t, ax, ay, az, gx, gy, gz] % gpsData : M x 4列 [t, pn, pe, pd]已转 NED x zeros(15,1); P blkdiag(1e-2*eye(3), 1e-2*eye(3), 1e-4*eye(3), ... 1e-6*eye(3), 1e-4*eye(3)); t_prev imuData(1,1); gps_idx 1; state_hist zeros(size(imuData,1), 15); for k 1:size(imuData,1) t imuData(k,1); dt t - t_prev; t_prev t; % 预测步每帧 IMU 都执行 imu imuData(k, 2:7); F numericalJacobian(x, imu, dt); x predictState(x, imu, dt); F_d eye(15) F*dt; Q_d F * G_c * Q_c * G_c * F * dt; P F_d * P * F_d Q_d; % 更新步时间戳到达 GPS 时刻才执行 while gps_idx size(gpsData,1) gpsData(gps_idx,1) t [x, P] gpsUpdate(x, P, gpsData(gps_idx, 2:4), R_gps); gps_idx gps_idx 1; end state_hist(k,:) x; end主循环结构是「IMU 驱动、GPS 触发」IMU 每帧做预测GPS 用 while 而非 if 处理因为两个传感器时间戳未必严格对齐同一 IMU 时刻可能积累多条 GPS 观测。时间同步是隐藏的坑IMU 与 GPS 时钟不同源时先做时间戳对齐否则 GPS 位置相对预测状态有几十毫秒延迟高动态飞行下等效于在量测里注入额外噪声直观表现是机动时位置估计震荡。4. EKF 参数调优Q、R、P0 怎么设与 MATLAB 发散排查同一套 EKF 代码能跑出完全不同的结果参数占了大部分原因。Q、R、P0 三个矩阵分别对应模型噪声、量测噪声和初始不确定度调参顺序也按这个优先级来先固定 R再粗调 Q最后用 P0 修正收敛速度不要三个一起动。4.1 过程噪声 Q物理含义与 MEMS 传感器初值预测步里的 G_c 和 Q_c 定义如下sigma_a 0.05; % 加速度计白噪声标准差, m/s^2 sigma_g 0.01; % 陀螺白噪声标准差, rad/s sigma_bg 1e-5; % 陀螺零偏随机游走强度 sigma_ba 1e-4; % 加速度计零偏随机游走强度 Q_c diag([sigma_a^2*ones(1,3), sigma_g^2*ones(1,3), ... sigma_bg^2*ones(1,3), sigma_ba^2*ones(1,3)]); G_c zeros(15, 12); G_c(4:6, 1:3) eye(3); % 加速度噪声 - 速度通道 G_c(7:9, 4:6) eye(3); % 角速度噪声 - 姿态通道 G_c(10:12, 7:9) eye(3); % 陀螺零偏随机游走 G_c(13:15, 10:12) eye(3); % 加速度计零偏随机游走Q 的物理含义是「模型对 IMU 的信任程度」Q 大则滤波器认为 IMU 噪声大GPS 修正权重提高Q 小则相反。常见错误是把四个通道设成同一数量级姿态通道与零偏通道的尺度差几个量级统一设置会让某个通道过度自信或过度不自信。上表初值覆盖常见 MEMS 级 IMU更精确的值应来自 Allan 方差分析得到的数据手册噪声密度。4.2 量测噪声 RGPS 精度分档与实测标定R_gps diag([sig_x^2, sig_y^2, sig_z^2]);不同定位模式下的典型水平/垂直标准差如下。量测噪声略大于实际精度问题不大但设得比实际好很多一定会发散——滤波器被虚假的小噪声欺骗过度信任 GPSGPS 模式水平 σ (m)垂直 σ (m)消费级单点2446SBAS 增强1223RTK / PPK0.020.050.05最可靠的标定是实测把无人机静止放在已知点采集几分钟 GPS 数据直接算位置序列标准差作为 σ。静止时多径误差明显偏大算出的值偏保守恰好适合当 EKF 的 R。R 明显大于实际噪声时滤波器反应迟钝、输出滞后反过来 R 太小则高频抖动剧烈两者在位置时间序列上的表现很容易区分。4.3 发散排查从新息、P 矩阵和零偏推断病根发散时不要盲目试参数先看三个信号。第一是新息 y均值明显非零说明是模型偏差而非噪声问题查旋转矩阵转置、重力符号、欧拉角定义是否与 IMU 数据一致。第二是 P 矩阵对角线位置通道的 P 在几秒内掉到 1e-6 以下说明 Q 太小或 R 太大滤波器已「锁死」新观测不再起作用。第三是零偏估计正常几分钟收敛到稳定值若持续漂移且与姿态强相关说明零偏与姿态存在可观测性冲突通常是机动激励不足——无人机长时间悬停时零偏无法与重力方向解耦。提示在 MATLAB 中用调试模式逐帧看卡尔曼增益 K 的范数变化比盯着位置误差更容易定位问题。K 收敛到稳定小值是正常的但 K 振荡或趋近 0 说明数值问题或参数失配。5. EKF 验证与进阶NIS 一致性检验、欧拉角奇异与四元数替代5.1 用归一化新息平方NIS验证滤波器一致性滤波器没发散不等于调对了先做一致性检验。归一化新息平方 NIS yᵀS⁻¹y 在滤波器一致时服从自由度等于量测维数的卡方分布这里量测是三维位置nis zeros(N_gps, 1); % ... 主循环里每次 gpsUpdate 后记录 ... nis(idx) (y / S) * y; % idx 为当前 GPS 帧序号 chi2_lo chi2inv(0.025, 3); % 下界 0.216 chi2_hi chi2inv(0.975, 3); % 上界 9.348 fprintf(NIS 超出上界比例: %.2f%%\n, mean(nis chi2_hi)*100);超出上界的帧占比如果明显高于 2.5%说明 Q 或 R 与实际噪声不匹配大部分帧的 NIS 都远小于下界说明滤波器过度自信P 被压得过小需要调大 Q 或调小 R。NIS 的均值应接近自由度 3偏离太多就回第 4 章逐项检查。5.2 欧拉角奇异与四元数替代方案predictState 里 ψ_dot 的分母是 cos(θ)俯仰接近 ±90° 时姿态预测会爆炸。多旋翼和固定翼平飞场景 θ 一般限制在 ±45° 内欧拉角表示够用但做筋斗、垂直爬升或倾转旋翼过渡的 6DOF 无人机必须换四元数。常见做法是把状态改成 16 维 [q(4); p(3); v(3); bg(3); ba(3)]姿态用四元数乘法更新且每步归一化量测 H 仍是 [zeros(3,4), eye(3), zeros(3,9)]改动集中在预测函数与雅可比推导。不想大改滤波器的折中方案是预测步内部用四元数递推输出再转回欧拉角并在 |cos θ| 低于阈值时冻结 ψ 更新——对于以平飞为主、偶尔大俯仰的机型这是性价比最高的处理。本文还有配套的精品资源点击获取