搞组合导航这块的人都知道纯惯性系统一般撑不过两分钟姿态会慢慢漂、位置会越跑越偏单靠卫星定位又受不了遮挡和多路径干扰。所以GPS加惯导基本是行业标配而把它们融合起来的核心工具最常用的是扩展卡尔曼滤波器EKF。这个项目的标题很直接对配备惯性测量单元IMU和GPS的系统实现不变扩展卡尔曼滤波器并配MATLAB代码。说白了就是给一个15维的惯导/GPS组合滤波问题换一个更不容易“飘”的滤波框架。我实际跑下来最大的感受是常规EKF在传感器数据质量好的时候差别不大但GPS一断、车辆一颠簸IEKF的优势就非常明显——姿态误差和协方差估计都稳得多不会出现那种“滤波器以为自己准得不行其实已经错了十万八千里”的危险情况。所以这篇文章就把整个项目的推导、代码结构和调参经验拆开来讲适合想用MATLAB做惯性导航、传感器融合、以及研究李群滤波的开发者参考。1. 这个项目到底要解决什么问题1.1 常规EKF在惯导/GPS融合里的老毛病先聊一个实际场景一台车在高架桥下面开了三分钟GPS被桥体完全遮住只剩IMU在积分推算位置。按常规EKF的算法逻辑GPS量测中断期间状态协方差矩阵会按照模型噪声不断增长等GPS恢复后滤波器又用一份“膨胀到离谱”的协方差去计算增益结果就是一个很大的修正量直接砸进状态里位置轨迹很容易出现一个明显的跳变。跳变还算小事更麻烦的是“虚假自信”。因为常规EKF把姿态误差当成普通向量来做加减姿态本身是旋转矩阵不是线性空间线性化误差一旦变大协方差就会低估真实误差。滤波器的表现是前几分钟一切正常然后某个急转弯或者剧烈加减速之后姿态估计明明已经开始偏但协方差仍然显示“收敛良好”后续量测校正也变得迟钝最终滤波结果在不知不觉中发散掉。这就是传统EKF在带旋转系统上的核心问题用线性空间的“加法”去处理李群上才存在的姿态状态原理上就存在隐患。1.2 不变EKF的改法把修正从加法换成乘法不变扩展卡尔曼滤波器Invariant EKF简称IEKF最核心的改动是不再把误差定义成“当前估计减去真值”而是把系统状态放到一个具有群结构的状态空间里用左乘或右乘的方式定义误差。例如对惯性导航来说可以把位置、速度、姿态组合成一个矩阵得到一个叫SE2(3)的李群元素。真实状态与估计状态之间的误差用[ \xi\log\left(X_{\mathrm{true}}^{-1}\hat{X}\right) ]这样的对数映射来表示。修正的时候也不是直接往状态上“加”一个修正量而是[ \hat{X}\leftarrow\hat{X}\exp(\xi) ]做一次指数映射修正。这样做的好处很直接线性化后的误差动力学对状态本身不敏感滤波器不会再因为某一时刻的姿态偏离较大就出现协方差与真实误差严重失配的情况。尤其对惯导这种“旋转 积分”耦合很强的系统这种结构性处理方法比硬调参数补救靠谱得多。1.3 项目范围与适用对象这个项目里我实现了一套完整的仿真与滤波流程先产生一条参考轨迹再按100Hz频率生成IMU数据、按1Hz频率生成GPS测量然后用IEKF做融合估计。文中给到的MATLAB代码可以直接编译运行也支持改成标准EKF做对比实验。适合阅读这篇内容的人有两类一类是做GNSS/INS组合导航的学生和工程师需要把理论算法落成可跑的代码另一类是研究滤波算法、但之前只写过普通EKF的开发者想理解“不变”这个思路到底动了哪块蛋糕并快速建立一套最小可复现的验证环境。2. 系统建模和方程推导从IMU递推到GPS更新2.1 状态向量怎么选惯导/GPS融合滤波的状态量业内基本已经形成共识直接选15维向量[ x\left[p,\ v,\ q,\ b_g,\ b_a\right] ]其中(p)是三维位置(v)是三维速度(q)是四元数表示的姿态(b_g)和(b_a)分别是陀螺仪和加速度计的零偏。为什么姿态用四元数而不直接用欧拉角因为欧拉角在大姿态角变化时存在万向锁问题而且旋转合成的物理含义不直观四元数虽然多一个维度但更新公式优雅只要维持归一化就能覆盖所有可能姿态。还要注意一点在这个IEKF框架里名义状态和误差状态是分开的。名义状态负责积分递推误差状态才是卡尔曼滤波真正处理的量。误差状态通常维度更低比如姿态误差用三维旋转向量表示这样协方差矩阵始终是方阵运算开销也更小。2.2 IMU运动学递推的连续与离散形式先给出连续时间运动学模型。设陀螺仪测量值为(\omega_m)加速度计测量值为(a_m)则[ \dot{R}R(\omega_m-b_g)_{\times} ][ \dot{v}R(a_m-b_a)g ][ \dot{p}v ]这里((a)_{\times})表示三维向量对应的反对称矩阵(g)为重力矢量。注意这个方程是在世界坐标系下展开的所以重力会显式出现在速度微分项里。MATLAB代码里做离散化时我采用零阶保持假设认为每个IMU采样周期内角速度和加速度保持不变。那么姿态更新写成[ R_{k1}R_k\exp\left((\omega_m-b_g)\Delta t\right) ]这里的(\exp)是旋转矩阵的指数映射对应到代码实现就是先做反对称矩阵再求矩阵指数或者直接用四元数增量相乘。速度与位置的离散更新写成[ v_{k1}v_k(R_ka_kg)\Delta t ][ p_{k1}p_kv_k\Delta t\frac{1}{2}(R_ka_kg)\Delta t^2 ]位置更新里保留二阶项是为了避免在大加速度环境下出现明显积分滞后。这个细节在车辆急加速或剧烈转弯时对轨迹平滑度影响很大。2.3 GPS观测模型与量测方程GPS通常输出的定位结果是经纬高但在滤波之前会转换到局部直角坐标系。我习惯先把原点设到起始点附近然后转成ENU东北天坐标。在这个坐标系下GPS量测方程可以简单写为[ z_{\mathrm{GPS}}pn_g ]其中(n_g\sim\mathcal{N}(0,R_g))是位置测量噪声。常规EKF里对应量测矩阵是[ H\begin{bmatrix}0_{3\times3}0_{3\times3}I_30_{3\times3}0_{3\times3}\end{bmatrix} ]但在IEKF的实现里量测更新要先把GPS位置误差映射到不变误差坐标中再通过李代数修正状态而不是直接对位置元素做加减。这样做的结果是当载体姿态本身存在偏差时位置修正量和姿态修正量之间不会出现明显耦合异常。2.4 不变误差的数学直觉如果用一句话概括IEKF的误差逻辑常规EKF的误差是(\hat{x}-x)IEKF的误差是(\hat{X}X^{-1})或者(X^{-1}\hat{X})其中(X)是群元素。对惯导组合导航把((R,v,p))拼成SE2(3)的矩阵形式后误差的李代数坐标在小扰动假设下会得到一组线性化方程。推导过程比较长但结论很有价值误差转移矩阵里姿态相关项变成以IMU角速度为参数的指数映射速度/位置项里出现对称的反对称矩阵结构最终协方差预测不再强烈依赖当前姿态估计值。这正是“不变”名字的由来滤波器的误差传播模型在状态变换下保持不变我们能从基本原理上消除一部分线性化误差。3. MATLAB代码实现与解析3.1 建议的工程文件结构代码不建议堆成一个单文件哪怕项目很小拆成几个模块对排查问题也方便许多。我这边采用这种结构imu_gps_iekf/ main_iekf.m simulate_trajectory.m params.m iekm_predict.m iekm_update.m plot_results.mparams.m集中管理所有噪声参数、初始协方差、滤波器频率。simulate_trajectory.m负责生成参考轨迹和传感器数据。main_iekf.m是主循环先跑IMU预测有GPS时再触发量测更新。plot_results.m把所有估计结果画出来对比。拆开的好处是后面想换传感器模型或改为标准EKF对比只需要替换一个函数不用动主流程。3.2 参数初始化代码先看params.m里几个关键参数%% 传感器采样率 fs_imu 100; % IMU 频率 Hz fs_gps 1; % GPS 频率 Hz dt_imu 1 / fs_imu; %% IMU噪声参数 sigma_gyro 0.02 * pi / 180; % 陀螺仪白噪声 std: 0.02 deg/s sigma_accel 0.05; % 加速度计白噪声 std: 0.05 m/s^2 sigma_bg 1e-5; % 陀螺仪零偏随机游走 sigma_ba 1e-4; % 加速度计零偏随机游走 %% GPS量测噪声 sigma_gps [0.3; 0.3; 0.8]; % 位置噪声std: 水平0.3m 垂直0.8m %% 初始协方差 P0 blkdiag(1e-4*eye(3), ... % 姿态误差方差 rad^2 0.1*eye(3), ... % 速度误差方差 (m/s)^2 10*eye(3), ... % 位置误差方差 m^2 1e-4*eye(3), ... % 陀螺零偏方差 1e-3*eye(3)); % 加速度计零偏方差 %% 重力矢量 (ENU坐标下) g0 [0; 0; -9.8];这里有几个点要强调。垂直方向GPS噪声给到0.8m是因为普通民用GPS高度定位精度比水平精度差不少不给大一点高度通道容易出现抖动。初始姿态协方差给的是1e-4弧度平方如果一开始姿态完全未知这个值要放大到1e-2以上否则收敛时间会特别长。3.3 预测步骤的MATLAB实现预测函数的作用就是完成状态递推和协方差递推。状态递推直接用离散化方程协方差递推则利用误差状态转移矩阵。function [state, P] iekm_predict(state, P, gyro, accel, dt, param) R state.R; v state.v; p state.p; bg state.bg; ba state.ba; % 去掉零偏后的角速度和加速度 w gyro - bg; a accel - ba; % 姿态更新左乘旋转增量 R_new R * expm(skew(w) * dt); % 速度与位置更新 acc_eff R * a param.g0; v_new v acc_eff * dt; p_new p v * dt 0.5 * acc_eff * dt * dt; % 误差状态转移矩阵简化但体现不变结构的形式 F eye(15); F(1:3, 1:3) expm(-skew(w) * dt); F(4:6, 1:3) -skew(R * a) * dt; F(4:6, 4:6) eye(3); F(7:9, 1:3) -0.5 * skew(R * a) * dt * dt; F(7:9, 4:6) eye(3) * dt; F(7:9, 7:9) eye(3); % 离散噪声协方差 Qd param.Q * dt; % 协方差递推 P F * P * F Qd; % 写回状态 state.R R_new; state.v v_new; state.p p_new; end function S skew(x) S [0, -x(3), x(2); x(3), 0, -x(1); -x(2), x(1), 0]; end眼尖的读者会发现我这里协方差递推用的是常用导航误差状态简化版而不是从李群几何严格推出来的完整表达。实际工程中完全严格的IEKF还要考虑误差状态的伴随矩阵但那段代码不适合作为入门示例。先用这个框架理解“误差状态预测 名义状态递推”的双层结构后面再精读相关文献逐项替换F矩阵。预测里最容易犯的错是漏掉(F(7:9,4:6)I\cdot dt)这一项。位置由速度积分而来如果这一块为零长期运行位置协方差就不会随着速度误差增长GPS恢复后的修正会非常不自然。3.4 更新步骤的MATLAB实现更新时间名义状态的修正不再依赖加法直接修正而是利用误差向量做指数映射修正。这里给出一个GPS位置量测下的实现function [state, P] iekm_update(state, P, z_gps, param) % 量测矩阵位置 H zeros(3, 15); H(1:3, 7:9) eye(3); S H * P * H param.R_gps; K P * H / S; y z_gps - state.p; % 新息 dx K * y; % 误差状态向量 % 从误差状态中拆出旋转向量 dphi dx(1:3); dv dx(4:6); dp dx(7:9); % 指数映射修正姿态 dR expm(skew(dphi)); state.R state.R * dR; % 右乘修正 state.v state.v dv; state.p state.p dp; % 协方差更新 P (eye(15) - K * H) * P; end和常规EKF的更新相比这里有个很值得注意的操作增益乘以新息后的结果不是直接加到整个状态向量上而是分块处理。姿态部分通过指数映射修正速度和位置部分才用普通加法。因为姿态误差本质是旋转量放在切空间里处理能自然避免四元数更新后又要重新归一化的尴尬。如果后续需要扩展GPS只输出经纬高也要记住把坐标转换放在z_gps传入之前完成不要在滤波循环里反复写坐标变换影响可读性还会降低可复现性。3.5 主循环与调度逻辑主循环结构并不复杂外层按IMU时间步推进for k 1:length(imu_data) % 每个IMU周期做一次预测 [state, P] iekm_predict(state, P, imu_data(k).gyro, imu_data(k).accel, dt_imu, param); % 判断这一时刻是否有GPS量测 if gps_index size(gps_data, 2) abs(imu_time(k) - gps_time(gps_index)) 1e-6 [state, P] iekm_update(state, P, gps_data(:, gps_index), param); gps_index gps_index 1; end % 记录估计结果 est_R(:, :, k) state.R; est_v(:, k) state.v; est_p(:, k) state.p; endGPS频率低不用每个IMU周期都检查但检查成本几乎可以忽略所以写成“有就更新没有就继续预测”的调度逻辑最简单可靠。如果GPS延迟较大就要考虑“时间对齐”的问题那就是另一个复杂度等级了本文先不展开。4. 调参、仿真与避坑记录4.1 噪声矩阵怎么调才不玄学初次跑滤波大部分人都会遇到两个极端协方差给太小滤波器反应迟钝轨迹平滑过头协方差给太大噪声全进来了轨迹抖动得像心电图。以陀螺仪白噪声为例如果器件手册只给了角度随机游走可以粗略换算成白噪声标准差。但很多时候没有器件手册就得多花几分钟从静置数据里实际算标准差。把IMU静止放置一分钟采集所有角速度数据直接做标准差统计就是比较可靠的(\sigma_{\omega})。加速度计同理。这个实测值比手册上的理论值靠谱太多。GPS噪声参数也要区别对待。水平方向可以给0.2到0.5米垂直方向保守一点给0.8到1.5米。在城市峡谷场景下多路径会明显恶化定位精度如果还在用开阔环境的高精度参数滤波器就会在GPS更新时产生多余抖动。一个更稳妥的方案是把GPS协方差视为动态值卫星数量多时给0.5米卫星数量少时放宽到3米。4.2 初始协方差和零偏估计的重要性初始协方差矩阵决定了滤波器从“信任传感器”还是“信任初始值”开始。如果初始状态从GPS单点定位得到位置初始协方差可以设(10)平方米级如果是从已知点起步直接设成(0.01)平方米级也不会出问题。陀螺仪零偏和加速度计零偏初始值如果完全未知建议初始协方差不要给太小。零偏收敛一般需要几百秒期间如果GPSo频率低零偏误差可能和姿态误差纠缠在一起。实际项目里我通常会启动后先让车静止或直线行驶一小段用前置对准脚本估算初始零偏再送入滤波器。这样能明显加快整体收敛速度也减少后续轨迹偏移。4.3 几个容易踩的坑第一姿态更新之后忘了对四元数归一化。很多人用四元数更新状态但名义状态更新时直接返回四元数不做归一化。短期看不出问题几分钟后四元数范数就会偏离1导致所有旋转计算变形。第二把加速度计数据直接当“运动加速度”。IMU的加速度计测到的是比力包含重力分量如果直接积分会把重力当成运动加速度一次次加进去位置结果会快速飞掉。使用之前一定要先扣除重力也就是状态方程里那个(R(a_m-b_a)g)项。第三GPS高度噪声被机械地一起修正。由于民用GPS高度通道误差很大当垂直方向噪声给太小时位置会在每次GPS更新时出现明显的Z轴抖动。要么把(\sigma_{z})放宽要么对水平和高垂直通道分开建模。第四零偏随机游走参数给成零。如果(Q)里没有零偏随机游走项协方差会随时间把零偏误差越压越小滤波器就开始“自信过头”后面零偏一发生温漂整个系统都没法纠正。即使IMU零偏稳定我也不建议把随机游走设成0至少给1e-6量级。再补充一个值得记录的问题写expm(skew(w)*dt)时如果角速度非常大直接矩阵指数可能会引入数值误差。建议小角度近似来处理或者用四元数增量解析式dq [cos(0.5 * norm_w * dt); sin(0.5 * norm_w * dt) * w / norm_w]; q_new quatmultiply(q, dq);实测下来这种四元数方法在机动大的场合更稳而且代码更易读。4.4 仿真结果对比标准EKF与IEKF我用同一组仿真数据分别跑了标准EKF和IEKFGPS更新频率都是1Hz测试过程中专门加了40秒GPS完全缺失的时段。对比项标准EKF不变EKF姿态误差大机动后易出现突变误差误差曲线平稳无异常跳变位置误差GPS中断期间累计误差大恢复后跳变明显累计误差略小恢复后平滑修正协方差一致性中断期间明显低估误差与实际误差匹配度更高代码复杂度低中等但不超过普通EKF太多从代价角度看IEKF代码并没有比标准EKF复杂一个数量级主要区别是把误差状态转移矩阵和状态修正方式换成不变形式。换来的一致性提升却非常值。特别是GPS中断场景下IEKF能让我更放心地把滤波器输出交给上层路径规划而不用额外写一堆异常修正逻辑。5. 实测心得与扩展思路5.1 几个让我提升最大的调试技巧先说一个调试习惯每次改完滤波器参数先不看位置误差只看姿态误差和协方差对角元趋势。因为姿态误差的问题会直接传导到速度、位置解上如果姿态通道已经乱了位置调再久都不会有好结果。再有就是“协方差可视化”。在MATLAB里把(P)矩阵对角元素画出来一旦发现某个通道的方差降到接近机器精度基本就是模型有bug而不是“滤波器收敛得特别好”。正常工作情况下的协方差不会无限下压而是会稳定在一个物理量级。最后是别过分依赖仿真数据。仿真数据里传感器误差模型和真实硬件总有差距。我通常在仿真调通以后把代码接到真实IMU/GPS串口数据上跑一小段离线回放重点观察静止起步、低速转弯、GPS丢星恢复三个片段。大多数算法问题在这种回放里都会暴露出来。5.2 后续能怎么扩展这个框架留了很好的扩展口子。想加入磁力计只需在量测更新里增加一个观测方程想加入里程计轮速同样在量测矩阵里加对应行。因为IEKF的状态本体依然是(R, v, p)多传感器只是多几个约束结构不用大改。如果研究重点是李群理论下一步可以试着把F矩阵替换成严格的右不变误差转移矩阵并对比两种实现下的NEESS指标。如果想走向工程可以考虑改成即插即用的MEX或C代码并加上传感器时间同步层。无论往哪个方向走先把不确定EKF这套预测/更新框架跑通跑稳后面做任何扩展都会顺手很多。我在实际调试这个项目时最深刻的体会是卡尔曼滤波的很多问题不是源于噪声矩阵调得不够准而是源于状态空间本身的结构被破坏了。IEKF的意义不在于它把所有误差都消除而在于它让误差模型不再被姿态状态“牵着鼻子走”。这一点只要你跑一次带GPS中断的对比实验马上就能感受到。