简介这份资源面向计算机、电子信息工程、数学等专业的大学生以及需要完成课程设计、期末大作业或毕业设计的同学提供一套基于Matlab实现的15维经典ESKF误差状态卡尔曼滤波GPSIMU组合导航仿真参考方案。压缩包共4个文件包含2个m源码文件与2个mat数据文件整体约1.82MB源码负责滤波与仿真流程实现数据文件用于直接加载运行与结果验证便于对照调试。资源围绕GPS与IMU融合定位展开涉及状态误差建模、观测更新与轨迹解算等核心环节适合具备一定Matlab基础、能够自行阅读代码并调试修改的读者。目前已有1467人学习下载可作为组合导航入门与课程实践的参考资料帮助理解ESKF在惯性/卫星组合导航中的工程实现思路与数据组织方式。1. 15维ESKF组合导航仿真为什么GPS丢星时它还能稳住做无人机、无人车或者移动机器人定位的工程师大概率都遇到过这个场景GPS信号在城市峡谷里突然跳变或者干脆丢星几秒钟纯惯导积分立刻发散位置漂出去几十米。这时候如果有一套误差状态卡尔曼滤波Error-State Kalman Filter, ESKF在跑把GPS和IMU的数据融合起来姿态和位置就能在丢星期间靠IMU递推维持住等GPS恢复再拉回来。这个标题讲的正是这件事用Matlab搭一套15维状态的ESKF把GPS的位置观测和IMU的角速度、比力观测组合起来做导航仿真。15维状态不是随便定的它对应的是位置误差3维、速度误差3维、姿态误差3维、加速度计零偏3维、陀螺仪零偏3维加起来正好15。这套配置在低成本MEMS IMU加单点GPS的组合里是经典方案既能估计姿态和位置又能在线估计零偏避免零偏随时间累积把积分带偏。适合谁看如果你手头有IMU和GPS的原始数据想用Matlab快速验证一套组合导航算法或者正在做多传感器融合的课程设计、工程预研这套仿真能直接跑通也能改成你自己的数据接口。2. 15维ESKF的状态定义与预测更新从连续方程到离散递推2.1 状态向量和误差定义ESKF的核心思想是把“真实状态”拆成“名义状态”加“误差状态”。名义状态用IMU积分递推误差状态用卡尔曼滤波估计。15维误差状态向量写成δx [δp, δv, δθ, δba, δbg]^T其中δp是位置误差3×1δv是速度误差3×1δθ是姿态误差3×1用旋转向量表示δba是加速度计零偏误差3×1δbg是陀螺仪零偏误差3×1。名义状态则是p、v、q、ba、bgq用四元数表示。为什么姿态误差用3维而不是四元数的4维因为四元数有模长约束直接滤波会破坏归一化。用旋转向量轴角表示误差局部线性化后就是3维更新完再转回四元数数值上更稳。这是ESKF比直接EKF做四元数滤波更受欢迎的原因之一。2.2 连续时间误差状态方程名义状态的递推在Matlab里就是IMU积分% 名义状态递推简化版实际用四元数更新 % dt: 采样间隔 % acc_meas: 加速度计测量值(3x1)gyro_meas: 陀螺仪测量值(3x1) % ba, bg: 当前零偏估计 % q: 当前姿态四元数 % v, p: 当前速度和位置 acc_nav quat2rotm(q) * (acc_meas - ba) [0; 0; -9.81]; % 转到导航系并加重力 v v acc_nav * dt; p p v * dt; omega gyro_meas - bg; q quatmultiply(q, quat_from_omega(omega * dt)); % 四元数积分 q q / norm(q); % 归一化误差状态的连续时间方程是δp_dot δv δv_dot -R * [acc_meas - ba]× * δθ - R * δba n_a δθ_dot -[omega]× * δθ - δbg - n_g δba_dot n_ba δbg_dot n_bg其中R是姿态旋转矩阵[·]×是反对称矩阵n_a和n_g是加速度计和陀螺仪的噪声n_ba和n_bg是零偏随机游走噪声。这个方程在Matlab里写成矩阵形式就是F矩阵15×15离散化用一阶近似% 构造连续时间F矩阵 F zeros(15, 15); F(1:3, 4:6) eye(3); % δp_dot δv F(4:6, 7:9) -R * skew(acc_meas - ba); % δv对δθ F(4:6, 10:12) -R; % δv对δba F(7:9, 7:9) -skew(omega); % δθ对δθ F(7:9, 13:15) -eye(3); % δθ对δbg % 离散化 Fd eye(15) F * dt; Qd Q * dt; % Q是连续噪声协方差简化处理这里skew()是反对称矩阵函数Q是过程噪声协方差通常按IMU的噪声密度和零偏随机游走参数来设。实际写的时候Qd的离散化更准确的做法是用Van Loan方法或者泰勒展开但一阶近似在dt0.01s时够用。2.3 协方差预测和状态递推预测步的协方差更新P Fd * P * Fd Qd;这一步在Matlab里就是矩阵乘法15×15的矩阵运算量很小跑100Hz也没压力。名义状态按2.2的积分递推误差状态在预测步不更新保持为零因为ESKF的误差状态在观测更新前不注入名义状态。注意Qd的设置直接决定滤波器的“信任倾向”。Qd设大了滤波器更信GPSIMU积分漂移会被快速修正但GPS噪声也会被放大Qd设小了丢星期间靠IMU维持更稳但GPS恢复后收敛慢。我一般先用IMU数据手册的噪声密度算Q再根据实际轨迹调2到5倍。3. GPS位置观测更新与零偏反馈观测矩阵和注入时机3.1 观测模型和H矩阵GPS给的是导航系下的位置观测观测方程z p_gps p_true n_gps误差形式的观测方程δz H * δx n_gpsH矩阵是15维的只有位置部分非零H zeros(3, 15); H(1:3, 1:3) eye(3); % 只观测位置误差如果GPS还能给速度比如某些接收机输出多普勒速度H可以扩展成6×15但经典单点GPS一般只给位置所以这里按3维观测写。3.2 卡尔曼增益和状态更新观测更新在Matlab里% R_gps: GPS位置观测噪声协方差(3x3) S H * P * H R_gps; K P * H / S; delta_x K * (z_gps - p); % 新息 GPS位置 - 名义位置 P (eye(15) - K * H) * P;这里z_gps是GPS测量位置p是名义状态里的位置。新息就是两者之差。K是15×3的增益矩阵。3.3 误差注入名义状态和零偏反馈更新完误差状态后要把它注入名义状态然后清零% 注入位置、速度 p p delta_x(1:3); v v delta_x(4:6); % 注入姿态旋转向量转四元数再乘 dtheta delta_x(7:9); dq quat_from_rotvec(dtheta); q quatmultiply(q, dq); q q / norm(q); % 注入零偏 ba ba delta_x(10:12); bg bg delta_x(13:15); % 误差状态清零 delta_x zeros(15, 1);注入姿态的时候要注意顺序是q q * dq还是q dq * q取决于误差定义是局部扰动还是全局扰动。常见做法是局部扰动即q_true q_nominal * dq所以右乘。这个细节搞反了姿态会越滤越歪而且不容易发现因为小角度下差别不明显大角度才暴露。零偏反馈是ESKF能长期稳定的关键。没有零偏估计陀螺仪零偏会让姿态缓慢漂移加速度计零偏会让速度发散。15维状态里专门留了6维给零偏就是让滤波器在线估计它们。实际跑的时候零偏估计收敛需要一定激励比如有转弯、加减速静止时零偏可观性差收敛慢。提示GPS更新频率一般比IMU低常见是IMU 100Hz、GPS 10Hz。Matlab仿真里用一个计数器判断是否到GPS更新时刻到了就做观测更新没到就只做预测。这样时间同步逻辑清晰也接近实际工程。4. Matlab仿真框架搭建数据生成、滤波主循环和轨迹对比4.1 仿真数据生成没有实测数据的时候先用Matlab生成一段带噪声的IMU和GPS数据来验证算法。轨迹可以设成一段直线加转弯比如% 生成真实轨迹简化匀速直线转弯 dt 0.01; % IMU采样间隔 T 60; % 仿真时长 t 0:dt:T; N length(t); % 真实位置、速度、姿态示例先直线后转弯 p_true zeros(3, N); v_true zeros(3, N); q_true zeros(4, N); q_true(:, 1) [1; 0; 0; 0]; for k 2:N if t(k) 30 v_true(:, k) [1; 0; 0]; % 直线 else v_true(:, k) [cos(0.1*(t(k)-30)); sin(0.1*(t(k)-30)); 0]; % 转弯 end p_true(:, k) p_true(:, k-1) v_true(:, k) * dt; q_true(:, k) q_true(:, k-1); % 简化姿态不变 end然后加噪声生成IMU测量% IMU噪声参数典型MEMS acc_noise 0.05; % m/s^2 gyro_noise 0.005; % rad/s ba_true [0.02; -0.01; 0.03]; % 加速度计零偏 bg_true [0.001; 0.002; -0.001]; % 陀螺仪零偏 acc_meas zeros(3, N); gyro_meas zeros(3, N); for k 1:N acc_meas(:, k) acc_noise * randn(3, 1) ba_true; gyro_meas(:, k) gyro_noise * randn(3, 1) bg_true; endGPS测量按10Hz生成加位置噪声gps_rate 10; gps_idx 1:round(1/(gps_rate*dt)):N; gps_noise 2.0; % 米 z_gps p_true(:, gps_idx) gps_noise * randn(3, length(gps_idx));4.2 滤波主循环主循环把预测和更新串起来% 初始化 p_est zeros(3, N); v_est zeros(3, N); q_est zeros(4, N); ba_est zeros(3, N); bg_est zeros(3, N); P eye(15) * 1.0; % 初始协方差 ba_est(:, 1) zeros(3, 1); bg_est(:, 1) zeros(3, 1); q_est(:, 1) [1; 0; 0; 0]; gps_counter 1; for k 2:N % 预测 [p_est(:, k), v_est(:, k), q_est(:, k), ba_est(:, k), bg_est(:, k), P] ... eskf_predict(p_est(:, k-1), v_est(:, k-1), q_est(:, k-1), ... ba_est(:, k-1), bg_est(:, k-1), P, ... acc_meas(:, k), gyro_meas(:, k), dt); % GPS更新 if mod(k-1, round(1/(gps_rate*dt))) 0 gps_counter length(gps_idx) [p_est(:, k), v_est(:, k), q_est(:, k), ba_est(:, k), bg_est(:, k), P] ... eskf_update(p_est(:, k), v_est(:, k), q_est(:, k), ... ba_est(:, k), bg_est(:, k), P, ... z_gps(:, gps_counter), gps_noise^2 * eye(3)); gps_counter gps_counter 1; end endeskf_predict和eskf_update就是前面2.3和3.2的代码封装成函数。这样主循环干净方便替换成实测数据接口。4.3 轨迹对比和误差曲线跑完之后画图对比figure; plot3(p_true(1, :), p_true(2, :), p_true(3, :), k-, LineWidth, 1.5); hold on; plot3(p_est(1, :), p_est(2, :), p_est(3, :), r--, LineWidth, 1.5); legend(真实轨迹, ESKF估计); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); grid on; % 位置误差 pos_err sqrt(sum((p_est - p_true).^2, 1)); figure; plot(t, pos_err); xlabel(时间 (s)); ylabel(位置误差 (m)); grid on;正常情况下位置误差应该在GPS噪声量级2米左右波动丢星期间会缓慢增大GPS恢复后拉回。如果误差持续发散先查Q和R的比例再查姿态注入方向。注意仿真里GPS噪声设2米是单点GPS的典型值实际可能更大。R_gps设成噪声方差不要设成标准差否则滤波器会过度信任GPS轨迹跳变明显。5. 避坑与排查ESKF仿真里最容易翻车的5个地方5.1 姿态注入方向搞反轨迹缓慢旋转现象跑一段时间后估计轨迹相对真实轨迹整体旋转位置误差越来越大但滤波器不报错。原因误差状态注入名义姿态时用了左乘而不是右乘或者旋转向量转四元数的顺序错了。小角度下看不出来大角度或者长时间积分后累积成明显旋转。解决确认误差定义是局部扰动注入用q quatmultiply(q, dq)。写个单元测试给一个已知小角度扰动注入后看姿态变化是否符合预期。5.2 Q矩阵设得太大GPS噪声被放大现象估计轨迹在GPS更新时刻跳变明显位置误差曲线毛刺多滤波后比GPS原始还差。原因过程噪声Q设得过大滤波器认为IMU积分不可信每次GPS更新都大幅修正把GPS噪声也带进来了。解决按IMU数据手册的噪声密度算Q先设小一点看丢星期间的发散速度。如果丢星10秒位置漂移超过预期再适当增大Q。我一般从数据手册值开始调2到5倍。5.3 零偏估计不收敛速度缓慢漂移现象静止或者匀速段速度估计有缓慢漂移零偏估计值一直在变但不收敛。原因零偏可观性需要激励。静止时加速度计零偏和姿态耦合陀螺仪零偏和姿态变化耦合没有足够激励就分不开。另外零偏随机游走噪声设得太小滤波器不敢调零偏。解决仿真里加一段转弯或者加减速给零偏提供可观性。零偏随机游走噪声按数据手册设不要设成0。实际工程里如果长时间静止零偏估计会漂可以加零速修正ZUPT辅助。5.4 GPS更新时刻判断错误时间不同步现象GPS更新次数不对或者更新时刻和GPS数据索引错位导致新息计算用错数据。原因Matlab里用mod判断更新时刻但GPS数据索引和IMU索引的对应关系没对齐。比如gps_idx从1开始但循环里k从2开始差一个偏移。解决单独维护gps_counter每次更新后加1不要用k直接算GPS索引。仿真前先打印gps_idx和实际更新时刻确认对齐。5.5 协方差矩阵不对称或者非正定现象跑一段时间后P矩阵出现负特征值卡尔曼增益计算异常估计结果跳变。原因数值误差累积P (I - K*H)*P在浮点运算下可能失去对称性。Matlab默认双精度短时间没问题长时间或者Q/R比例极端时会暴露。解决每次更新后强制对称P (P P)/2。更稳的做法用Joseph形式更新P (I-KH)P(I-KH) KRK。仿真时长超过几分钟建议用Joseph形式。6. 进阶技巧用实测数据替换仿真接口和参数标定仿真跑通之后下一步是把实测IMU和GPS数据接进来。接口替换不复杂但有几个细节决定成败。第一时间戳对齐。实测数据里IMU和GPS的时间戳往往不是整倍数关系不能简单按计数器判断。常见做法是把GPS时间戳插值到IMU时间轴上或者用最近邻匹配。Matlab里用interp1做线性插值但要注意GPS位置插值会引入额外误差高动态场景下建议用零阶保持。第二IMU单位换算。不同IMU输出单位不一样有的加速度计给g陀螺仪给度每秒要统一到m/s²和rad/s。这个搞错了滤波器直接发散而且不容易发现因为量纲错误在矩阵运算里不报错。第三零偏初始值。实测数据里IMU零偏可能比仿真大得多初始P里零偏对应的方差要设大一点给滤波器足够的调整空间。我一般初始零偏方差设成(0.1 m/s²)²和(0.01 rad/s)²跑几十秒后看收敛情况再调。第四GPS噪声R的标定。仿真里R设2米实测可能5米甚至10米。标定方法静止段采集GPS位置算标准差平方就是R。如果GPS有跳变先做粗差剔除再算。第五验证方法。没有高精度参考轨迹的时候用闭环一致性检验看新息序列是否白噪声归一化新息平方NIS是否在卡方分布置信区间内。NIS持续偏大说明R设小了偏小说明R设大了。这个检验比看轨迹对比更客观。% NIS计算示例 nis zeros(1, length(gps_idx)); for i 1:length(gps_idx) innovation z_gps(:, i) - p_est(:, gps_idx(i)); S H * P * H R_gps; nis(i) innovation / S * innovation; end % 3维观测95%置信区间对应卡方值约7.81 % 如果nis均值远大于7.81R设小了我自己的习惯是每次换IMU或者换GPS模块先跑一段静止数据标定零偏和噪声再跑动态数据调Q和R。这套流程走下来ESKF基本能稳定工作。希望帮到你。本文还有配套的精品资源点击获取