搞组合导航这块的人都知道，纯惯性系统一般撑不过两分钟，姿态会慢慢漂、位置会越跑越偏；单靠卫星定位又受不了遮挡和多路径干扰。所以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_{k+1}=R_k\exp\left((\omega_m-b_g)\Delta t\right) ]

这里的(\exp)是旋转矩阵的指数映射，对应到代码实现就是先做反对称矩阵再求矩阵指数，或者直接用四元数增量相乘。速度与位置的离散更新写成：

[ v_{k+1}=v_k+(R_ka_k+g)\Delta t ]

[ p_{k+1}=p_k+v_k\Delta t+\frac{1}{2}(R_ka_k+g)\Delta t^2 ]

位置更新里保留二阶项，是为了避免在大加速度环境下出现明显积分滞后。这个细节在车辆急加速或剧烈转弯时，对轨迹平滑度影响很大。

2.3 GPS观测模型与量测方程

GPS通常输出的定位结果是经纬高，但在滤波之前会转换到局部直角坐标系。我习惯先把原点设到起始点附近，然后转成ENU东北天坐标。在这个坐标系下，GPS量测方程可以简单写为：

[ z_{\mathrm{GPS}}=p+n_g ]

其中(n_g\sim\mathcal{N}(0,R_g))是位置测量噪声。常规EKF里对应量测矩阵是

[ H=\begin{bmatrix}0_{3\times3}&0_{3\times3}&I_3&0_{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.m

• params.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; end

GPS频率低，不用每个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和IEKF，GPS更新频率都是1Hz，测试过程中专门加了40秒GPS完全缺失的时段。

从代价角度看，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中断的对比实验，马上就能感受到。