简介面向无人机组合导航与MATLAB仿真学习者项目以GPS/INS融合的扩展卡尔曼滤波EKF为核心解决6自由度无人机状态的高精度预测问题适合需要理解滤波理论并开展仿真实验的读者。资源共5个文件以4个MATLAB脚本.m和1个MAT格式数据文件.mat为主压缩包约194KB其中主脚本完成状态递推与量测更新辅助函数处理四元数转欧拉角等坐标变换MAT文件提供校验基准便于直接运行和扩展调试。目前已有140人学习。通过对比ass3_q2与ass3_q3_kf两个脚本可直观看到未滤波与EKF估计的差异结合check.mat中的对照数据可深入验证状态转移矩阵、观测矩阵与噪声协方差的设置并利用均方误差、协方差阵等指标评估滤波器精度为实际无人机导航中的GPS-INS融合提供完整可复现的参考实现。1. GPS-INS融合中的6DOF无人机状态估计为什么绕不开EKF在多旋翼穿过高架桥洞、低空贴地巡检或飞入建筑阴影区域时GPS多路径和卫星丢失会瞬间把定位拉偏数米而惯性导航INS在几秒内还能输出平滑的位置与姿态增量却会因陀螺仪和加速度计的积分漂移而逐渐失去参考价值。把INS作为无人机运动模型的预测源把GPS作为外部测量去定期修正INS积累的误差这种互补关系正是GPS-INS融合的基本设计逻辑。无人机的三轴平移与三轴旋转合并起来就是6个自由度的刚体运动但实际状态估计里大多会用四元数表示姿态使得状态方程出现四元数乘法这类非线性项常规线性卡尔曼滤波无法直接套用于是需要先对系统做一阶线性化再用扩展卡尔曼滤波EKF完成预测和更新。下面围绕一个MATLAB课程项目文件包拆解其中包含quat2euler.m、ass3_q2.m、ass3_q3_kf.m、main_template.m和check.mat正好覆盖从姿态转换、EKF实现到精度验证的完整链路适合刚接手飞控或组合导航任务的一线工程师以及想在MATLAB里完整复现6DOF状态估计算法的研究型开发者。2. 建立6DOF无人机的状态向量与运动学模型2.1 状态向量的维度与索引约定6DOF无人机的“6自由度”描述的是刚体运动能力质心沿X、Y、Z轴的平移范围以及绕三根机体轴的旋转范围。但在EKF的状态向量里状态维度通常会远大于6因为滤波器不仅需要估计运动状态还要把陀螺零偏、加速度计零偏这类不可直接测量的参数一并纳入才能保证预测模型接近真实物理过程。常见的做法是把状态定为16维位置3维、速度3维、四元数4维、陀螺零偏3维、加速度计零偏3维。多出的1维来自四元数它能避免欧拉角在俯仰接近±90度时出现的万向锁奇异性代价是必须持续约束四元数的单位范数。以下代码把状态按物理语义切成索引块后续写状态转移矩阵或观测矩阵时可以直接引用% state_index.m - 16维状态索引约定 % X [pn pe pd vn ve vd qw qx qy qz bgx bgy bgz bax bay baz] ID_POS 1:3; % NED位置 ID_VEL 4:6; % NED速度 ID_QUAT 7:10; % 四元数标量在第一位 ID_BG 11:13; % 陀螺零偏 rad/s ID_BA 14:16; % 加速度计零偏 m/s^2 nX 16;这段代码把状态索引集中在一处定义之后在predict_state、compute_H这类函数里直接使用ID_POS、ID_VEL切片比到处硬编码“1:3”“4:6”可靠得多。注意四元数在MATLAB工具链里的顺序并不统一有的库把标量放在第一个元素有的放在最后使用项目自带的quat2euler.m之前最好先确认它期望的输入顺序再统一到全局约定。2.2 quat2euler.m四元数转欧拉角的实现与边界陷阱拿到压缩包后最值得先翻开的是quat2euler.m。它解决“滤波器内部用四元数更新画轨迹或输出姿态时用欧拉角”的转换问题常见实现如下function euler quat2euler(q) % 输入四元数 [qw qx qy qz]输出 [roll; pitch; yaw] 弧度 qw q(1); qx q(2); qy q(3); qz q(4); roll atan2(2*(qw*qx qy*qz), 1 - 2*(qx^2 qy^2)); pitch asin(2*(qw*qy - qz*qx)); yaw atan2(2*(qw*qz qx*qy), 1 - 2*(qy^2 qz^2)); euler [roll; pitch; yaw]; endroll和yaw都用atan2而不是asin能把角度展开到[-pi, pi]区间避免余弦主值导致的跳变pitch用asin返回值限制在[-pi/2, pi/2]这是欧拉角本征的几何边界。当pitch接近±90度时cos(pitch)接近0roll和yaw在数值上会迅速退化对应的EKF观测矩阵在这片区域也会出现相位翻转。因此我在实际项目里只把欧拉角用作结果评价或初始化滤波器内部不会让欧拉角参与状态递推。注意如果观测模型直接使用欧拉角H矩阵在pitch接近90度时会出现病态这类项目里最常见的做法是四元数参与状态更新最后画图时再调用quat2euler转换。2.3 运动学方程与线性化离散化6DOF无人机的导航级运动学模型可以拆成三个递推块位置是速度的积分速度是加速度计比力经旋转矩阵投影到导航系后再叠加重力加速度的积分姿态四元数则用陀螺角速度构造四元数微分方程。忽略地球自转的简化形式如下p_dot vv_dot C_b^n (a_m - ba) g_nq_dot 0.5 * q ⊗ [0; w_m - bg]其中C_b^n是机体坐标系到NED导航系的旋转矩阵g_n在NED系下为[0; 0; 9.8]⊗表示四元数乘法。这些方程全部包含非线性项EKF的处理方式是在当前工作点计算雅可比矩阵再套用卡尔曼滤波的预测公式。手推完整解析雅可比较费时间课程项目里可以先采用中心差分数值雅可比% numerical_jacobian.m - 对非线性函数 f 求数值雅可比 function J numerical_jacobian(f, x, dx) n length(x); J zeros(length(f(x)), n); for i 1:n xp x; xp(i) xp(i) dx(i); xm x; xm(i) xm(i) - dx(i); J(:, i) (f(xp) - f(xm)) / (2*dx(i)); end end中心差分的步长不能统一取同一个值位置和速度量级较大dx取1e-8左右即可四元数分量本身在-1到1之间规范化误差容易放大差分结果dx建议取1e-4。数值雅可比能加快原型验证但进入性能调优阶段后还是需要把关键通道换成解析表达式否则H矩阵和F矩阵在不同飞行姿态下的精度会直接影响EKF收敛性。状态分组维度传感器来源在EKF中的角色平移位置3GPS观测INS递推直接对应GPS位置量测平移速度3加速度计比力积分直接对应GPS速度量测姿态四元数4陀螺仪驱动递推提供非线性状态转移陀螺零偏3滤波器估计修正角速度输入加速度计零偏3滤波器估计修正比力输入3. 基于EKF的GPS-INS融合实现拆解ass3_q2与ass3_q3_kf3.1 check.mat的数据组织与main_template调用关系main_template.m是主入口负责加载check.mat并调用ass3_q2.m和ass3_q3_kf.m两个作业脚本。按照文件名推断ass3_q2.m很可能是未做EKF融合的基线版本ass3_q3_kf.m才是完整EKF实现。但在看代码之前先确认check.mat里的字段会更稳妥% load_check.m - 查看check.mat中的字段 data load(check.mat); disp(fieldnames(data)); imu data.imu; % 时间戳, gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z gps data.gps; % 时间戳, pn, pe, pd, vn, ve, vd gt data.gt; % 真值通常包含轨迹与姿态实际项目中IMU频率通常为100Hz到200HzGPS频率只有5Hz到20Hz因此EKF预测步几乎每个IMU时刻都要执行更新步只在有GPS新帧的时刻触发。最容易踩的坑是把IMU时间戳和GPS时间戳直接当作同一时刻对齐实际中它们往往存在固定的时钟延迟带进去会让新息出现常值偏置。我一般先做线性插值把GPS观测插到最近的IMU时间栅格上再观察新息是否平滑。3.2 预测步由IMU驱动状态与协方差传播预测步是EKF里唯一每个时刻都必须执行的环节即使GPS被遮挡也要靠它维持状态输出。代码主体如下% ekf_predict.m - 以IMU角速度和比力作为输入 function [Xk, Pk] ekf_predict(X_prev, P_prev, imu_k, dt) p X_prev(ID_POS); v X_prev(ID_VEL); q X_prev(ID_QUAT); bg X_prev(ID_BG); ba X_prev(ID_BA); gyro imu_k(1:3) - bg; acc imu_k(4:6) - ba; % 四元数微分: q_dot 0.5 * q * [0; gyro] omega 0.5 * [0, -gyro(1), -gyro(2), -gyro(3); gyro(1), 0, gyro(3), -gyro(2); gyro(2), -gyro(3), 0, gyro(1); gyro(3), gyro(2), -gyro(1), 0] * q; q_new q dt * omega; q_new q_new / norm(q_new); % 位置速度更新比力经旋转矩阵转NED Cbn quat2rot(q); % 四元数转旋转矩阵 p_new p v*dt 0.5*Cbn*acc*dt^2; v_new v Cbn*acc*dt [0;0;9.8]*dt; Xk [p_new; v_new; q_new; bg; ba]; F compute_state_jacobian(X_prev, imu_k, dt); G eye(nX); Pk F * P_prev * F G * Q * G; end这里位置更新公式考虑了加速度的二阶积分项相比只做vdt的方式在轨迹弯曲段更贴近物理过程。重力加速度直接取9.8 m/s²是在NED系下正方向向下因此加速度计比力里反映的推力变化会与重力分离。F由compute_state_jacobian计算若暂时没有解析表达式可以临时使用上一节的numerical_jacobian。Q矩阵维度必须和GQ*G匹配尤其是陀螺和加速度计噪声的谱密度单位不统一时协方差会快速膨胀或坍缩。3.3 更新步GPS量测、量测矩阵与残差生成更新步只在GPS数据有效时执行。GPS直接给出位置和速度这两个量都是状态的线性函数因此量测矩阵H可以写为常数选择矩阵% ekf_update.m - 基于GPS量测更新状态 function [Xk, Pk, innov] ekf_update(X_pred, P_pred, gps_k, R) z [gps_k(2); gps_k(3); gps_k(4); % pn, pe, pd gps_k(5); gps_k(6); gps_k(7)]; % vn, ve, vd H zeros(6, nX); H(1:3, ID_POS) eye(3); H(4:6, ID_VEL) eye(3); z_pred [X_pred(ID_POS); X_pred(ID_VEL)]; innov z - z_pred; S H * P_pred * H R; K P_pred * H / S; Xk X_pred K * innov; Pk (eye(nX) - K * H) * P_pred; end这段代码里的更新步不需要对H做雅可比线性化因为位置和速度直接就是状态的部分分量。使用K P_pred * H / S而不是inv(S)HP_pred是利用MATLAB的矩阵右除数值上更稳妥。Pk更新之后四元数分量会因新增修正项而偏离单位范数所以必须在更新完成后立即重新归一化并检查四元数对应协方差块是否出现负对角元。注意标准P更新公式(eye - KH)P_pred在浮点运算下容易产生轻微非对称若后续出现负数方差优先换成Joseph形式P (eye-KH)P_pred(eye-KH) KRK。3.4 ass3_q2与ass3_q3_kf的对比思路ass3_q2.m和ass3_q3_kf.m大概率运行在同一套main_template.m框架下区别在于前者可能直接拼接GPS与INS结果或者只用INS递推然后强行覆盖GPS观测后者则通过EKF完成融合。对比时必须先做时间戳对齐再输出同一时刻的位置、速度、姿态画成残差图或误差曲线。常见误用是直接把两条二维轨迹画在一起靠肉眼判断好坏真正需要看的是两者相对真值gt的误差序列以及EKF输出的协方差边界是否覆盖误差。比较代码里也最好用相同初值和相同传感器噪声设置否则差异会同时混入滤波器和初始化两个因素。4. EKF参数调优与发散排查从Q、R到四元数约束4.1 Q和R的物理解释与建议初值EKF里的Q是过程噪声协方差R是量测噪声协方差。Q描述的是“模型本身不可信的部分”R描述的是“传感器测量有多脏”两者构成一对矛盾Q调大滤波器更相信测量轨迹跟手但毛刺多R调大滤波器更相信模型轨迹平滑但易产生滞后。一般先按传感器数据手册的噪声谱密度给初值再通过新息序列微调而不是反复试凑。参考初值如下参数对应噪声源初值参考调大时现象调小时现象Q_gyro陀螺角度随机游走(1e-4)^2姿态跟踪变钝协方差变大姿态残差持续偏置Q_acc加速度计速度随机游走(1e-3)^2速度更信任GPS位置平滑对机体高频振动更敏感R_posGPS位置测量噪声2.0^2 m²滤波输出更平滑收敛变慢轨迹更跟踪GPS但易受多路径R_velGPS速度测量噪声0.5^2 (m/s)²速度估计平缓速度出现高频毛刺从连续时间谱密度转离散Q时需要乘以dt或dt^2单位错了会导致协方差在几个递推周期内数量级漂移。常用转换如下% Q_discrete.m - 连续噪声谱密度转离散过程噪声 Sg 1e-8; % 陀螺白噪声谱密度 (rad^2/s) Sa 1e-6; % 加速度计白噪声谱密度 (m^2/s^3) Sc 1e-10; % 零偏随机游走谱密度 dt 0.01; % IMU周期 Q diag([zeros(1,3), Sa*dt^2*ones(1,3), ... Sg*dt*ones(1,4), Sc*dt*ones(1,6)]);这里位置对应的Q取0因为位置本身不会被随机噪声直接驱动它的不确定来自速度积分四元数按Sg*dt近似严格推导还需要引入四元数噪声传播项但在课程项目中按这个量级起调再用后续的NEES检验校准效率更高。4.2 四元数归一化与P矩阵正定性问题EKF更新步对四元数的修正并不满足单位范数约束直接带着非归一化四元数进入下一轮预测会让旋转矩阵失去正交性姿态估计逐渐漂移。解决方式有二一是更新后立即归一化同时修正P矩阵二是把误差状态定义为小角度向量用误差状态卡尔曼滤波避免四元数约束问题。项目代码里更常见的是第一种% enforce_quat_consistency.m - 更新后修正四元数范数 q Xk(ID_QUAT); Xk(ID_QUAT) q / norm(q); % 保对称 Pk (Pk Pk) / 2; % 投影到半正定锥 [V, D] eig(Pk); D(D 0) 0; Pk V * D * V;把P矩阵投影到半正定锥虽然带有工程妥协色彩但能稳住数值递推。要更规范就直接用Joseph形式更新P避免减法项破坏对称性实际工程中我通常两者结合更新用Joseph形式只有检测到负特征值时才执行上述投影。4.3 从新息序列快速锁定发散原因EKF发散前的征兆几乎都体现在新息序列上。调试时记录每一时刻的新息和它的协方差S然后画出新息序列与2σ边界任何持续超出边界的片段都指向模型或参数问题残差均值恒定非零优先怀疑时间戳未对齐或初始位置设置错误。残差随速度增大而增大状态转移矩阵中速度对位置的耦合项写错。残差在急转弯时飙升检查四元数归一化是否缺失陀螺零偏是否被正确估计。P矩阵出现负对角线改用Joseph更新或检查H矩阵是否把姿态通道误接进GPS位置量测。% innovation_plot.m - 画新息与2sigma边界 innov_store zeros(N, 3); bound_store zeros(N, 3); for k 1:N [~, ~, innov_store(k,:), S_k] ekf_update_verbose(...); bound_store(k,:) 2 * sqrt(diag(S_k(1:3,1:3))); end t (0:N-1) * dt; plot(t, innov_store(:,1), b-, t, bound_store(:,1), r--, t, -bound_store(:,1), r--);图上如果新息在大部分时间落在红虚线以内说明EKF的协方差表达与真实误差水平基本匹配如果新息经常跑出虚线即使最终轨迹RMSE不大滤波器也已经处于过度信任预测或测量的状态后续面对长航时任务容易突然发散。5. 用check.mat做EKF精度验证RMSE与NEES的一致性检查5.1 RMSE指标与坐标系对齐跑完ass3_q3_kf.m后把check.mat中的真值gt与滤波器输出est重采样到同一时间栅格再逐维计算误差% eval_rmse.m - 评估位置/速度/姿态RMSE err est(:, ID_POS) - gt(:, ID_POS); rmse_pos sqrt(mean(sum(err.^2, 2))); euler_err quat2euler(est(:, ID_QUAT)) - quat2euler(gt(:, ID_QUAT)); euler_err wrapToPi(euler_err); rmse_att sqrt(mean(sum(euler_err.^2, 2)));姿态误差必须先wrapToPi限定在[-pi, pi]否则跨越±180度时误差被放大成看似很大的虚假值。RMSE不宜孤立看待如果GPS位置噪声σ约2米位置RMSE却小于0.5米说明滤波器对测量过拟合很可能是R设得过大或Q设得过小使得输出“过于平滑”。5.2 NEES一致性检验RMSE只能说明误差总量无法说明协方差是否可信。NEES通过归一化误差平方来检验P矩阵与真实误差的一致性% nees_check.m - 位置子空间NEES for k 1:N err est(k, ID_POS) - gt(k, ID_POS); Pk P_store{k}(ID_POS, ID_POS); nees(k) err / Pk * err; end nees_mean mean(nees); % 自由度3N较大时95%置信区间约为 [2.36, 3.72]NEES均值远大于3说明P过于乐观协方差低估了真实误差远小于3则说明P过于保守滤波器对模型的信任度偏低。注意NEES对初始P很敏感验证时不要为了曲线好看而把初始协方差设成极小值。5.3 最省事的单步完整性测试想在整条轨迹上反复调参之前可以先做一次单步测试用真实状态做初始值把GPS观测换成真值加零均值小噪声跑一次预测加一次更新检查新息是否围绕0波动、P对角线是否与噪声量级匹配。如果单步就出现偏置基本可以断定是代码里的索引错位或四元数符号问题而不是参数问题如果单步通过再去跑完整数据集就能聚焦到Q和R的调节上。这里最容易忽略的是H矩阵里四元数对应的列要置零否则GPS位置更新会直接改写姿态状态导致输出姿态在每次GPS帧到来时出现台阶我每次写完EKF都会先查这一处。本文还有配套的精品资源点击获取 基于EKF的GPS-INS融合：6DOF无人机状态估计与MATLAB实现
简介面向无人机组合导航与MATLAB仿真学习者项目以GPS/INS融合的扩展卡尔曼滤波EKF为核心解决6自由度无人机状态的高精度预测问题适合需要理解滤波理论并开展仿真实验的读者。资源共5个文件以4个MATLAB脚本.m和1个MAT格式数据文件.mat为主压缩包约194KB其中主脚本完成状态递推与量测更新辅助函数处理四元数转欧拉角等坐标变换MAT文件提供校验基准便于直接运行和扩展调试。目前已有140人学习。通过对比ass3_q2与ass3_q3_kf两个脚本可直观看到未滤波与EKF估计的差异结合check.mat中的对照数据可深入验证状态转移矩阵、观测矩阵与噪声协方差的设置并利用均方误差、协方差阵等指标评估滤波器精度为实际无人机导航中的GPS-INS融合提供完整可复现的参考实现。1. GPS-INS融合中的6DOF无人机状态估计为什么绕不开EKF在多旋翼穿过高架桥洞、低空贴地巡检或飞入建筑阴影区域时GPS多路径和卫星丢失会瞬间把定位拉偏数米而惯性导航INS在几秒内还能输出平滑的位置与姿态增量却会因陀螺仪和加速度计的积分漂移而逐渐失去参考价值。把INS作为无人机运动模型的预测源把GPS作为外部测量去定期修正INS积累的误差这种互补关系正是GPS-INS融合的基本设计逻辑。无人机的三轴平移与三轴旋转合并起来就是6个自由度的刚体运动但实际状态估计里大多会用四元数表示姿态使得状态方程出现四元数乘法这类非线性项常规线性卡尔曼滤波无法直接套用于是需要先对系统做一阶线性化再用扩展卡尔曼滤波EKF完成预测和更新。下面围绕一个MATLAB课程项目文件包拆解其中包含quat2euler.m、ass3_q2.m、ass3_q3_kf.m、main_template.m和check.mat正好覆盖从姿态转换、EKF实现到精度验证的完整链路适合刚接手飞控或组合导航任务的一线工程师以及想在MATLAB里完整复现6DOF状态估计算法的研究型开发者。2. 建立6DOF无人机的状态向量与运动学模型2.1 状态向量的维度与索引约定6DOF无人机的“6自由度”描述的是刚体运动能力质心沿X、Y、Z轴的平移范围以及绕三根机体轴的旋转范围。但在EKF的状态向量里状态维度通常会远大于6因为滤波器不仅需要估计运动状态还要把陀螺零偏、加速度计零偏这类不可直接测量的参数一并纳入才能保证预测模型接近真实物理过程。常见的做法是把状态定为16维位置3维、速度3维、四元数4维、陀螺零偏3维、加速度计零偏3维。多出的1维来自四元数它能避免欧拉角在俯仰接近±90度时出现的万向锁奇异性代价是必须持续约束四元数的单位范数。以下代码把状态按物理语义切成索引块后续写状态转移矩阵或观测矩阵时可以直接引用% state_index.m - 16维状态索引约定 % X [pn pe pd vn ve vd qw qx qy qz bgx bgy bgz bax bay baz] ID_POS 1:3; % NED位置 ID_VEL 4:6; % NED速度 ID_QUAT 7:10; % 四元数标量在第一位 ID_BG 11:13; % 陀螺零偏 rad/s ID_BA 14:16; % 加速度计零偏 m/s^2 nX 16;这段代码把状态索引集中在一处定义之后在predict_state、compute_H这类函数里直接使用ID_POS、ID_VEL切片比到处硬编码“1:3”“4:6”可靠得多。注意四元数在MATLAB工具链里的顺序并不统一有的库把标量放在第一个元素有的放在最后使用项目自带的quat2euler.m之前最好先确认它期望的输入顺序再统一到全局约定。2.2 quat2euler.m四元数转欧拉角的实现与边界陷阱拿到压缩包后最值得先翻开的是quat2euler.m。它解决“滤波器内部用四元数更新画轨迹或输出姿态时用欧拉角”的转换问题常见实现如下function euler quat2euler(q) % 输入四元数 [qw qx qy qz]输出 [roll; pitch; yaw] 弧度 qw q(1); qx q(2); qy q(3); qz q(4); roll atan2(2*(qw*qx qy*qz), 1 - 2*(qx^2 qy^2)); pitch asin(2*(qw*qy - qz*qx)); yaw atan2(2*(qw*qz qx*qy), 1 - 2*(qy^2 qz^2)); euler [roll; pitch; yaw]; endroll和yaw都用atan2而不是asin能把角度展开到[-pi, pi]区间避免余弦主值导致的跳变pitch用asin返回值限制在[-pi/2, pi/2]这是欧拉角本征的几何边界。当pitch接近±90度时cos(pitch)接近0roll和yaw在数值上会迅速退化对应的EKF观测矩阵在这片区域也会出现相位翻转。因此我在实际项目里只把欧拉角用作结果评价或初始化滤波器内部不会让欧拉角参与状态递推。注意如果观测模型直接使用欧拉角H矩阵在pitch接近90度时会出现病态这类项目里最常见的做法是四元数参与状态更新最后画图时再调用quat2euler转换。2.3 运动学方程与线性化离散化6DOF无人机的导航级运动学模型可以拆成三个递推块位置是速度的积分速度是加速度计比力经旋转矩阵投影到导航系后再叠加重力加速度的积分姿态四元数则用陀螺角速度构造四元数微分方程。忽略地球自转的简化形式如下p_dot vv_dot C_b^n (a_m - ba) g_nq_dot 0.5 * q ⊗ [0; w_m - bg]其中C_b^n是机体坐标系到NED导航系的旋转矩阵g_n在NED系下为[0; 0; 9.8]⊗表示四元数乘法。这些方程全部包含非线性项EKF的处理方式是在当前工作点计算雅可比矩阵再套用卡尔曼滤波的预测公式。手推完整解析雅可比较费时间课程项目里可以先采用中心差分数值雅可比% numerical_jacobian.m - 对非线性函数 f 求数值雅可比 function J numerical_jacobian(f, x, dx) n length(x); J zeros(length(f(x)), n); for i 1:n xp x; xp(i) xp(i) dx(i); xm x; xm(i) xm(i) - dx(i); J(:, i) (f(xp) - f(xm)) / (2*dx(i)); end end中心差分的步长不能统一取同一个值位置和速度量级较大dx取1e-8左右即可四元数分量本身在-1到1之间规范化误差容易放大差分结果dx建议取1e-4。数值雅可比能加快原型验证但进入性能调优阶段后还是需要把关键通道换成解析表达式否则H矩阵和F矩阵在不同飞行姿态下的精度会直接影响EKF收敛性。状态分组维度传感器来源在EKF中的角色平移位置3GPS观测INS递推直接对应GPS位置量测平移速度3加速度计比力积分直接对应GPS速度量测姿态四元数4陀螺仪驱动递推提供非线性状态转移陀螺零偏3滤波器估计修正角速度输入加速度计零偏3滤波器估计修正比力输入3. 基于EKF的GPS-INS融合实现拆解ass3_q2与ass3_q3_kf3.1 check.mat的数据组织与main_template调用关系main_template.m是主入口负责加载check.mat并调用ass3_q2.m和ass3_q3_kf.m两个作业脚本。按照文件名推断ass3_q2.m很可能是未做EKF融合的基线版本ass3_q3_kf.m才是完整EKF实现。但在看代码之前先确认check.mat里的字段会更稳妥% load_check.m - 查看check.mat中的字段 data load(check.mat); disp(fieldnames(data)); imu data.imu; % 时间戳, gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z gps data.gps; % 时间戳, pn, pe, pd, vn, ve, vd gt data.gt; % 真值通常包含轨迹与姿态实际项目中IMU频率通常为100Hz到200HzGPS频率只有5Hz到20Hz因此EKF预测步几乎每个IMU时刻都要执行更新步只在有GPS新帧的时刻触发。最容易踩的坑是把IMU时间戳和GPS时间戳直接当作同一时刻对齐实际中它们往往存在固定的时钟延迟带进去会让新息出现常值偏置。我一般先做线性插值把GPS观测插到最近的IMU时间栅格上再观察新息是否平滑。3.2 预测步由IMU驱动状态与协方差传播预测步是EKF里唯一每个时刻都必须执行的环节即使GPS被遮挡也要靠它维持状态输出。代码主体如下% ekf_predict.m - 以IMU角速度和比力作为输入 function [Xk, Pk] ekf_predict(X_prev, P_prev, imu_k, dt) p X_prev(ID_POS); v X_prev(ID_VEL); q X_prev(ID_QUAT); bg X_prev(ID_BG); ba X_prev(ID_BA); gyro imu_k(1:3) - bg; acc imu_k(4:6) - ba; % 四元数微分: q_dot 0.5 * q * [0; gyro] omega 0.5 * [0, -gyro(1), -gyro(2), -gyro(3); gyro(1), 0, gyro(3), -gyro(2); gyro(2), -gyro(3), 0, gyro(1); gyro(3), gyro(2), -gyro(1), 0] * q; q_new q dt * omega; q_new q_new / norm(q_new); % 位置速度更新比力经旋转矩阵转NED Cbn quat2rot(q); % 四元数转旋转矩阵 p_new p v*dt 0.5*Cbn*acc*dt^2; v_new v Cbn*acc*dt [0;0;9.8]*dt; Xk [p_new; v_new; q_new; bg; ba]; F compute_state_jacobian(X_prev, imu_k, dt); G eye(nX); Pk F * P_prev * F G * Q * G; end这里位置更新公式考虑了加速度的二阶积分项相比只做vdt的方式在轨迹弯曲段更贴近物理过程。重力加速度直接取9.8 m/s²是在NED系下正方向向下因此加速度计比力里反映的推力变化会与重力分离。F由compute_state_jacobian计算若暂时没有解析表达式可以临时使用上一节的numerical_jacobian。Q矩阵维度必须和GQ*G匹配尤其是陀螺和加速度计噪声的谱密度单位不统一时协方差会快速膨胀或坍缩。3.3 更新步GPS量测、量测矩阵与残差生成更新步只在GPS数据有效时执行。GPS直接给出位置和速度这两个量都是状态的线性函数因此量测矩阵H可以写为常数选择矩阵% ekf_update.m - 基于GPS量测更新状态 function [Xk, Pk, innov] ekf_update(X_pred, P_pred, gps_k, R) z [gps_k(2); gps_k(3); gps_k(4); % pn, pe, pd gps_k(5); gps_k(6); gps_k(7)]; % vn, ve, vd H zeros(6, nX); H(1:3, ID_POS) eye(3); H(4:6, ID_VEL) eye(3); z_pred [X_pred(ID_POS); X_pred(ID_VEL)]; innov z - z_pred; S H * P_pred * H R; K P_pred * H / S; Xk X_pred K * innov; Pk (eye(nX) - K * H) * P_pred; end这段代码里的更新步不需要对H做雅可比线性化因为位置和速度直接就是状态的部分分量。使用K P_pred * H / S而不是inv(S)HP_pred是利用MATLAB的矩阵右除数值上更稳妥。Pk更新之后四元数分量会因新增修正项而偏离单位范数所以必须在更新完成后立即重新归一化并检查四元数对应协方差块是否出现负对角元。注意标准P更新公式(eye - KH)P_pred在浮点运算下容易产生轻微非对称若后续出现负数方差优先换成Joseph形式P (eye-KH)P_pred(eye-KH) KRK。3.4 ass3_q2与ass3_q3_kf的对比思路ass3_q2.m和ass3_q3_kf.m大概率运行在同一套main_template.m框架下区别在于前者可能直接拼接GPS与INS结果或者只用INS递推然后强行覆盖GPS观测后者则通过EKF完成融合。对比时必须先做时间戳对齐再输出同一时刻的位置、速度、姿态画成残差图或误差曲线。常见误用是直接把两条二维轨迹画在一起靠肉眼判断好坏真正需要看的是两者相对真值gt的误差序列以及EKF输出的协方差边界是否覆盖误差。比较代码里也最好用相同初值和相同传感器噪声设置否则差异会同时混入滤波器和初始化两个因素。4. EKF参数调优与发散排查从Q、R到四元数约束4.1 Q和R的物理解释与建议初值EKF里的Q是过程噪声协方差R是量测噪声协方差。Q描述的是“模型本身不可信的部分”R描述的是“传感器测量有多脏”两者构成一对矛盾Q调大滤波器更相信测量轨迹跟手但毛刺多R调大滤波器更相信模型轨迹平滑但易产生滞后。一般先按传感器数据手册的噪声谱密度给初值再通过新息序列微调而不是反复试凑。参考初值如下参数对应噪声源初值参考调大时现象调小时现象Q_gyro陀螺角度随机游走(1e-4)^2姿态跟踪变钝协方差变大姿态残差持续偏置Q_acc加速度计速度随机游走(1e-3)^2速度更信任GPS位置平滑对机体高频振动更敏感R_posGPS位置测量噪声2.0^2 m²滤波输出更平滑收敛变慢轨迹更跟踪GPS但易受多路径R_velGPS速度测量噪声0.5^2 (m/s)²速度估计平缓速度出现高频毛刺从连续时间谱密度转离散Q时需要乘以dt或dt^2单位错了会导致协方差在几个递推周期内数量级漂移。常用转换如下% Q_discrete.m - 连续噪声谱密度转离散过程噪声 Sg 1e-8; % 陀螺白噪声谱密度 (rad^2/s) Sa 1e-6; % 加速度计白噪声谱密度 (m^2/s^3) Sc 1e-10; % 零偏随机游走谱密度 dt 0.01; % IMU周期 Q diag([zeros(1,3), Sa*dt^2*ones(1,3), ... Sg*dt*ones(1,4), Sc*dt*ones(1,6)]);这里位置对应的Q取0因为位置本身不会被随机噪声直接驱动它的不确定来自速度积分四元数按Sg*dt近似严格推导还需要引入四元数噪声传播项但在课程项目中按这个量级起调再用后续的NEES检验校准效率更高。4.2 四元数归一化与P矩阵正定性问题EKF更新步对四元数的修正并不满足单位范数约束直接带着非归一化四元数进入下一轮预测会让旋转矩阵失去正交性姿态估计逐渐漂移。解决方式有二一是更新后立即归一化同时修正P矩阵二是把误差状态定义为小角度向量用误差状态卡尔曼滤波避免四元数约束问题。项目代码里更常见的是第一种% enforce_quat_consistency.m - 更新后修正四元数范数 q Xk(ID_QUAT); Xk(ID_QUAT) q / norm(q); % 保对称 Pk (Pk Pk) / 2; % 投影到半正定锥 [V, D] eig(Pk); D(D 0) 0; Pk V * D * V;把P矩阵投影到半正定锥虽然带有工程妥协色彩但能稳住数值递推。要更规范就直接用Joseph形式更新P避免减法项破坏对称性实际工程中我通常两者结合更新用Joseph形式只有检测到负特征值时才执行上述投影。4.3 从新息序列快速锁定发散原因EKF发散前的征兆几乎都体现在新息序列上。调试时记录每一时刻的新息和它的协方差S然后画出新息序列与2σ边界任何持续超出边界的片段都指向模型或参数问题残差均值恒定非零优先怀疑时间戳未对齐或初始位置设置错误。残差随速度增大而增大状态转移矩阵中速度对位置的耦合项写错。残差在急转弯时飙升检查四元数归一化是否缺失陀螺零偏是否被正确估计。P矩阵出现负对角线改用Joseph更新或检查H矩阵是否把姿态通道误接进GPS位置量测。% innovation_plot.m - 画新息与2sigma边界 innov_store zeros(N, 3); bound_store zeros(N, 3); for k 1:N [~, ~, innov_store(k,:), S_k] ekf_update_verbose(...); bound_store(k,:) 2 * sqrt(diag(S_k(1:3,1:3))); end t (0:N-1) * dt; plot(t, innov_store(:,1), b-, t, bound_store(:,1), r--, t, -bound_store(:,1), r--);图上如果新息在大部分时间落在红虚线以内说明EKF的协方差表达与真实误差水平基本匹配如果新息经常跑出虚线即使最终轨迹RMSE不大滤波器也已经处于过度信任预测或测量的状态后续面对长航时任务容易突然发散。5. 用check.mat做EKF精度验证RMSE与NEES的一致性检查5.1 RMSE指标与坐标系对齐跑完ass3_q3_kf.m后把check.mat中的真值gt与滤波器输出est重采样到同一时间栅格再逐维计算误差% eval_rmse.m - 评估位置/速度/姿态RMSE err est(:, ID_POS) - gt(:, ID_POS); rmse_pos sqrt(mean(sum(err.^2, 2))); euler_err quat2euler(est(:, ID_QUAT)) - quat2euler(gt(:, ID_QUAT)); euler_err wrapToPi(euler_err); rmse_att sqrt(mean(sum(euler_err.^2, 2)));姿态误差必须先wrapToPi限定在[-pi, pi]否则跨越±180度时误差被放大成看似很大的虚假值。RMSE不宜孤立看待如果GPS位置噪声σ约2米位置RMSE却小于0.5米说明滤波器对测量过拟合很可能是R设得过大或Q设得过小使得输出“过于平滑”。5.2 NEES一致性检验RMSE只能说明误差总量无法说明协方差是否可信。NEES通过归一化误差平方来检验P矩阵与真实误差的一致性% nees_check.m - 位置子空间NEES for k 1:N err est(k, ID_POS) - gt(k, ID_POS); Pk P_store{k}(ID_POS, ID_POS); nees(k) err / Pk * err; end nees_mean mean(nees); % 自由度3N较大时95%置信区间约为 [2.36, 3.72]NEES均值远大于3说明P过于乐观协方差低估了真实误差远小于3则说明P过于保守滤波器对模型的信任度偏低。注意NEES对初始P很敏感验证时不要为了曲线好看而把初始协方差设成极小值。5.3 最省事的单步完整性测试想在整条轨迹上反复调参之前可以先做一次单步测试用真实状态做初始值把GPS观测换成真值加零均值小噪声跑一次预测加一次更新检查新息是否围绕0波动、P对角线是否与噪声量级匹配。如果单步就出现偏置基本可以断定是代码里的索引错位或四元数符号问题而不是参数问题如果单步通过再去跑完整数据集就能聚焦到Q和R的调节上。这里最容易忽略的是H矩阵里四元数对应的列要置零否则GPS位置更新会直接改写姿态状态导致输出姿态在每次GPS帧到来时出现台阶我每次写完EKF都会先查这一处。本文还有配套的精品资源点击获取